#![no_main]
#![no_std]

extern crate alloc;

use alloc::boxed::Box;
use az::*;
use core::cell::RefCell;
use cortex_m::delay::Delay;
use embedded_alloc::Heap;
use embedded_hal::{blocking::spi::Write, digital::v2::InputPin, digital::v2::OutputPin};
use fixed::types::I16F16;
use fugit::RateExtU32;
use lights_core::{App, BodyPattern, DashboardPattern, Event, Instant, FPS, UI};
use panic_halt as _;
use rp_pico::{
    entry,
    hal::{
        clocks::init_clocks_and_plls,
        gpio::{FunctionSio, Pin, PinId, PullDown, PullUp, SioInput, SioOutput},
        pac::{CorePeripherals, Peripherals},
        spi::{Enabled, Spi, SpiDevice, ValidSpiPinout},
        watchdog::Watchdog,
        Clock, Sio,
    },
    Pins,
};

#[global_allocator]
static HEAP: Heap = Heap::empty();

const LIGHT_SCALE: I16F16 = I16F16::lit("256.0");
const DASHBOARD_BRIGHTESS: u8 = 1;
const BODY_BRIGHTNESS: u8 = 8;

struct DebouncedButton<P: PinId> {
    debounce: Instant,
    pin: Pin<P, FunctionSio<SioInput>, PullUp>,
}

impl<P: PinId> DebouncedButton<P> {
    fn new(pin: Pin<P, FunctionSio<SioInput>, PullUp>) -> Self {
        Self {
            debounce: Instant((0 as u32).into()),
            pin,
        }
    }

    fn is_low(&self, time: Instant) -> bool {
        if time <= self.debounce {
            return false;
        }
        self.pin.is_low().unwrap_or(false)
    }

    fn set_debounce(&mut self, time: Instant) {
        self.debounce = time + Instant((250 as u32).into());
    }
}

struct BikeUI<
    D: SpiDevice,
    P: ValidSpiPinout<D>,
    LeftId: PinId,
    RightId: PinId,
    PreviousId: PinId,
    NextId: PinId,
    BrakeId: PinId,
> {
    spi: RefCell<Spi<Enabled, D, P, 8>>,
    left_blinker_button: DebouncedButton<LeftId>,
    right_blinker_button: DebouncedButton<RightId>,
    previous_animation_button: DebouncedButton<PreviousId>,
    next_animation_button: DebouncedButton<NextId>,
    brake_sensor: Pin<BrakeId, FunctionSio<SioInput>, PullUp>,

    brake_enabled: bool,
}

impl<
        D: SpiDevice,
        P: ValidSpiPinout<D>,
        LeftId: PinId,
        RightId: PinId,
        PreviousId: PinId,
        NextId: PinId,
        BrakeId: PinId,
    > BikeUI<D, P, LeftId, RightId, PreviousId, NextId, BrakeId>
{
    fn new(
        spi: Spi<Enabled, D, P, 8>,
        left_blinker_button: Pin<LeftId, FunctionSio<SioInput>, PullUp>,
        right_blinker_button: Pin<RightId, FunctionSio<SioInput>, PullUp>,
        previous_animation_button: Pin<PreviousId, FunctionSio<SioInput>, PullUp>,
        next_animation_button: Pin<NextId, FunctionSio<SioInput>, PullUp>,
        brake_sensor: Pin<BrakeId, FunctionSio<SioInput>, PullUp>,
    ) -> Self {
        Self {
            spi: RefCell::new(spi),
            left_blinker_button: DebouncedButton::new(left_blinker_button),
            right_blinker_button: DebouncedButton::new(right_blinker_button),
            previous_animation_button: DebouncedButton::new(previous_animation_button),
            next_animation_button: DebouncedButton::new(next_animation_button),
            brake_sensor,

            brake_enabled: false,
        }
    }
}

impl<
        D: SpiDevice,
        P: ValidSpiPinout<D>,
        LeftId: PinId,
        RightId: PinId,
        PreviousId: PinId,
        NextId: PinId,
        BrakeId: PinId,
    > UI for BikeUI<D, P, LeftId, RightId, PreviousId, NextId, BrakeId>
{
    fn check_event(&mut self, current_time: Instant) -> Option<Event> {
        if self.brake_sensor.is_high().unwrap_or(true) && !self.brake_enabled {
            self.brake_enabled = true;
            Some(Event::Brake)
        } else if self.brake_sensor.is_low().unwrap_or(false) && self.brake_enabled {
            self.brake_enabled = false;
            Some(Event::BrakeRelease)
        } else if self.left_blinker_button.is_low(current_time) {
            self.left_blinker_button.set_debounce(current_time);
            Some(Event::LeftBlinker)
        } else if self.right_blinker_button.is_low(current_time) {
            self.right_blinker_button.set_debounce(current_time);
            Some(Event::RightBlinker)
        } else if self.previous_animation_button.is_low(current_time) {
            self.previous_animation_button.set_debounce(current_time);
            Some(Event::PreviousPattern)
        } else if self.next_animation_button.is_low(current_time) {
            self.next_animation_button.set_debounce(current_time);
            Some(Event::NextPattern)
        } else {
            None
        }
    }

    fn update_lights(&self, dashboard_lights: DashboardPattern, body_lights: BodyPattern) {
        let mut lights: [u8; 260] = [0; 260];
        lights[256] = 0xff;
        lights[257] = 0xff;
        lights[258] = 0xff;
        lights[259] = 0xff;
        for (idx, rgb) in dashboard_lights.iter().enumerate() {
            lights[(idx + 1) * 4 + 0] = 0xe0 + DASHBOARD_BRIGHTESS;
            lights[(idx + 1) * 4 + 1] = (I16F16::from(rgb.r) * LIGHT_SCALE).saturating_as();
            lights[(idx + 1) * 4 + 2] = (I16F16::from(rgb.b) * LIGHT_SCALE).saturating_as();
            lights[(idx + 1) * 4 + 3] = (I16F16::from(rgb.g) * LIGHT_SCALE).saturating_as();
        }
        for (idx, rgb) in body_lights.iter().enumerate() {
            lights[(idx + 4) * 4 + 0] = 0xe0 + BODY_BRIGHTNESS;
            lights[(idx + 4) * 4 + 1] = (I16F16::from(rgb.b) * LIGHT_SCALE).saturating_as();
            lights[(idx + 4) * 4 + 2] = (I16F16::from(rgb.g) * LIGHT_SCALE).saturating_as();
            lights[(idx + 4) * 4 + 3] = (I16F16::from(rgb.r) * LIGHT_SCALE).saturating_as();
        }
        let mut spi = self.spi.borrow_mut();
        spi.write(lights.as_slice());
    }
}

#[entry]
fn main() -> ! {
    {
        use core::mem::MaybeUninit;
        const HEAP_SIZE: usize = 8096;
        static mut HEAP_MEM: [MaybeUninit<u8>; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE];
        unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) }
    }

    let mut pac = Peripherals::take().unwrap();
    let core = CorePeripherals::take().unwrap();
    let sio = Sio::new(pac.SIO);
    let mut watchdog = Watchdog::new(pac.WATCHDOG);

    let pins = Pins::new(
        pac.IO_BANK0,
        pac.PADS_BANK0,
        sio.gpio_bank0,
        &mut pac.RESETS,
    );

    let clocks = init_clocks_and_plls(
        12_000_000u32,
        pac.XOSC,
        pac.CLOCKS,
        pac.PLL_SYS,
        pac.PLL_USB,
        &mut pac.RESETS,
        &mut watchdog,
    )
    .ok()
    .unwrap();

    let mut delay = Delay::new(core.SYST, clocks.system_clock.freq().to_Hz());
    let mut spi_clk = pins.gpio10.into_function();
    let mut spi_sdo = pins.gpio11.into_function();
    let spi = Spi::<_, _, _, 8>::new(pac.SPI1, (spi_sdo, spi_clk));
    let mut spi = spi.init(
        &mut pac.RESETS,
        clocks.peripheral_clock.freq(),
        1_u32.MHz(),
        embedded_hal::spi::MODE_1,
    );

    let left_blinker_button = pins.gpio16.into_pull_up_input();
    let right_blinker_button = pins.gpio17.into_pull_up_input();
    let previous_animation_button = pins.gpio27.into_pull_up_input();
    let next_animation_button = pins.gpio26.into_pull_up_input();
    let brake_sensor = pins.gpio18.into_pull_up_input();

    let mut led_pin = pins.led.into_push_pull_output();

    let ui = BikeUI::new(
        spi,
        left_blinker_button,
        right_blinker_button,
        previous_animation_button,
        next_animation_button,
        brake_sensor,
    );

    let mut app = App::new(Box::new(ui));

    led_pin.set_high();

    let mut time = Instant::default();
    let delay_ms = 1000 / (FPS as u32);
    loop {
        app.tick(time);

        delay.delay_ms(delay_ms);
        time = time + Instant(delay_ms.into());
    }
}
