#![no_main]
#![no_std]

use panic_reset as _;
//use panic_semihosting as _;

mod config;
mod geo;
mod gps;
mod radio;
mod shell;
mod status;
mod voltage;

use systick_monotonic::fugit::Instant;

pub const MAX_PACKET_LEN: usize = 8191;
pub const SYS_TICK_FREQ: u32 = 1000;

pub type MyInstant = Instant<u64, 1, { SYS_TICK_FREQ }>;

#[rtic::app(device = stm32f4xx_hal::pac, peripherals = true, dispatchers = [USART1, USART6])]
mod app {
    use cortex_m::singleton;
    use hal::{
        adc::{config::AdcConfig, Adc},
        flash::LockedFlash,
        gpio,
        otg_fs::{UsbBus, UsbBusType, USB},
        pac::USART2,
        prelude::*,
        rcc::{Clocks, Rcc},
        serial::{self, Serial},
        spi::{Mode, Phase, Polarity, Spi},
    };
    use half::f16;
    use ham_cats::{
        buffer::Buffer,
        packet::Packet,
        whisker::{Gps, NodeInfoBuilder, Route},
    };
    use stm32f4xx_hal as hal;
    use systick_monotonic::{fugit::Instant, ExtU64, Systick};
    use usb_device::prelude::*;

    use crate::{
        config::{Config, GpsSetting, Maybe},
        gps::{GpsModule, GpsPos},
        radio::RadioManager,
        shell::Shell,
        status::Status,
        voltage::VoltMeter,
        MAX_PACKET_LEN, SYS_TICK_FREQ,
    };

    const SYS_CLK: u32 = 84_000_000;
    const H_CLK: u32 = 21_000_000;

    const LED_BLINK_RATE: u64 = 250;
    const UNDERVOLT_THRES_LEN: u8 = 20; // in # LED blinks

    const HARDWARE_ID: u16 = 0x7c84;
    const SOFTWARE_ID: u8 = 1;

    const MODE: Mode = Mode {
        polarity: Polarity::IdleLow,
        phase: Phase::CaptureOnFirstTransition,
    };

    #[monotonic(binds = SysTick, default = true)]
    type MyMono = Systick<{ SYS_TICK_FREQ }>;

    #[local]
    struct Local {
        green: gpio::Pin<'B', 15, gpio::Output>,
        usb_detect: gpio::Pin<'A', 5, gpio::Input>,
        gps_serial: Serial<USART2>,
        flash: LockedFlash,
        buf: &'static mut [u8; MAX_PACKET_LEN],
        gps_enable: gpio::Pin<'C', 8, gpio::Output<gpio::OpenDrain>>,
        bootup_time: Instant<u64, 1, { SYS_TICK_FREQ }>,
        cycles_below_thres: u8,
    }

    #[shared]
    struct Shared {
        red: gpio::Pin<'B', 14, gpio::Output>,
        radio: Option<RadioManager<'static>>,
        gps: GpsModule,
        volt_meter: VoltMeter,

        usb_dev: UsbDevice<'static, UsbBusType>,
        shell: Shell,
        config: Config,
        status: Status,
    }

    fn setup_clocks(rcc: Rcc) -> Clocks {
        rcc.cfgr
            .use_hse(25.MHz())
            .hclk(H_CLK.Hz())
            .sysclk(SYS_CLK.Hz())
            .pclk1(24.MHz())
            .pclk2(24.MHz())
            .require_pll48clk()
            .freeze()
    }

    #[init]
    fn init(ctx: init::Context) -> (Shared, Local, init::Monotonics) {
        let dp = ctx.device;
        let gpioa = dp.GPIOA.split();
        let gpiob = dp.GPIOB.split();
        let gpioc = dp.GPIOC.split();

        let clocks = setup_clocks(dp.RCC.constrain());

        // Setup flash
        let mut flash = LockedFlash::new(dp.FLASH);

        let config = Config::load_from_flash(&mut flash).unwrap_or_default();

        // setup LEDs
        let mut red = gpiob.pb14.into_push_pull_output();
        let mut green = gpiob.pb15.into_push_pull_output();

        red.set_low();
        green.set_low();

        // setup volt meter
        let volt_pin = gpioc.pc0.into_analog();
        let volt_adc = Adc::adc1(dp.ADC1, true, AdcConfig::default());
        let mut volt_meter = VoltMeter::new(volt_pin, volt_adc, config.min_voltage.into());

        // Detect if we're connected to USB or not
        let usb_detect = gpioa.pa5.into_pull_down_input();

        // need to give time for the pull-down to take effect
        let mut delay = dp.TIM5.delay_us(&clocks);
        delay.delay_us(100u8);

        let usb_connected = usb_detect.is_high();

        // setup radio
        let radio = if usb_connected {
            None
        } else {
            let mosi = gpiob.pb5.into_alternate();
            let miso = gpiob.pb4.into_alternate();
            let sclk = gpiob.pb3.into_alternate();

            let sdn = gpiob.pb12.into_push_pull_output();
            let cs = gpioa.pa8.into_push_pull_output();

            let spi = Spi::new(dp.SPI1, (sclk, miso, mosi), MODE, 10.MHz(), &clocks);

            let buf = singleton!(: [u8; MAX_PACKET_LEN] = [0; MAX_PACKET_LEN]).unwrap();
            let radio = RadioManager::new(spi, sdn, cs, delay, buf, &config);

            radio
        };

        // Setup GPS power
        let mut gps_enable = gpioc.pc8.into_open_drain_output();
        gps_enable.set_high(); // disable

        // Setup GPS serial
        let pins = (gpioa.pa2, gpioa.pa3);
        let mut gps_serial = Serial::new(
            dp.USART2,
            pins,
            serial::Config::default()
                .baudrate(9600.bps())
                .wordlength_8(),
            &clocks,
        )
        .unwrap()
        .with_u8_data();
        gps_serial.listen(serial::Event::Rxne);

        // Setup USB
        static mut EP_MEMORY: [u32; 1024] = [0; 1024];
        static mut USB_BUS: Option<usb_device::bus::UsbBusAllocator<UsbBusType>> = None;

        let usb = USB {
            usb_global: dp.OTG_FS_GLOBAL,
            usb_device: dp.OTG_FS_DEVICE,
            usb_pwrclk: dp.OTG_FS_PWRCLK,
            pin_dm: gpioa.pa11.into(),
            pin_dp: gpioa.pa12.into(),
            hclk: clocks.hclk(),
        };
        unsafe {
            USB_BUS.replace(UsbBus::new(usb, &mut EP_MEMORY));
        }

        let usb_serial = usbd_serial::SerialPort::new(unsafe { USB_BUS.as_ref().unwrap() });
        let usb_dev = UsbDeviceBuilder::new(
            unsafe { USB_BUS.as_ref().unwrap() },
            UsbVidPid(0x16c0, 0x27dd),
        )
        .manufacturer("cats.radio")
        .product("CATS mobile transceiver")
        .serial_number("4242")
        .device_class(usbd_serial::USB_CLASS_CDC)
        .build();

        let bootup_time = monotonics::now();

        let shell = Shell::new(config.clone(), usb_serial);

        let buf = cortex_m::singleton!(: [u8; MAX_PACKET_LEN] = [0; MAX_PACKET_LEN]).unwrap();

        let undervolt = volt_meter.below_threshold();

        let status = if usb_connected {
            Status::Programming(false)
        } else if undervolt {
            Status::Sleep
        } else if radio.is_none() {
            Status::Error(false)
        } else if !config.enable_transmit {
            green.set_high();

            Status::TxDisabled(false)
        } else {
            green.set_high();

            if config.should_enable_gps() {
                gps_enable.set_low();
            }

            transmit_position::spawn().unwrap();
            radio_tick::spawn().unwrap();

            Status::Normal
        };

        led_handler::spawn().unwrap();

        (
            Shared {
                red,
                radio,
                gps: GpsModule::new(),
                volt_meter,

                usb_dev,
                shell,
                config,
                status,
            },
            Local {
                green,
                usb_detect,
                gps_serial,
                flash,
                buf,
                gps_enable,
                bootup_time,
                cycles_below_thres: 0,
            },
            init::Monotonics(Systick::new(ctx.core.SYST, H_CLK)),
        )
    }

    #[task(priority = 3, local = [], shared = [red, radio, config, status])]
    fn radio_tick(mut ctx: radio_tick::Context) {
        (ctx.shared.radio, ctx.shared.config).lock(|r, c| {
            r.as_mut()
                .unwrap()
                .tick(&mut ctx.shared.red, Some((&c.callsign, c.ssid)))
                .unwrap()
        });

        if ctx.shared.status.lock(|s| matches!(s, Status::Normal)) {
            radio_tick::spawn_after(20u64.millis()).unwrap();
        }
    }

    #[task(priority = 2, local = [buf, bootup_time], shared = [red, radio, gps, config, status, volt_meter])]
    fn transmit_position(mut ctx: transmit_position::Context) {
        let mut config = ctx.shared.config;
        let (black_hole, transmit_period_seconds) =
            config.lock(|c| (c.black_hole, c.transmit_period_seconds));

        let pos = match config.lock(|c| c.gps) {
            GpsSetting::Disabled => None,
            GpsSetting::Fixed(lat, lon) => Some(GpsPos {
                latitude: lat,
                longitude: lon,
                altitude_meters: 0.0,
                speed_ms: 0.0,
                true_course: 0.0,
            }),
            GpsSetting::Receiver => ctx.shared.gps.lock(|gps| gps.pos()),
        };

        let should_transmit = match pos {
            Some(pos) => !black_hole.in_black_hole(pos.latitude, pos.longitude),
            None => true,
        };

        if should_transmit {
            let mut buf = [0; MAX_PACKET_LEN];
            let mut cats = Packet::new(&mut buf);

            let voltage = ctx.shared.volt_meter.lock(|vm| vm.voltage());
            let temp = ctx
                .shared
                .radio
                .lock(|r| r.as_mut().unwrap().get_temp().unwrap());
            let uptime_secs = (monotonics::now() - *ctx.local.bootup_time)
                .to_secs()
                .try_into()
                .unwrap_or(0);

            let mut info_builder = NodeInfoBuilder::default()
                .hardware_id(HARDWARE_ID)
                .software_id(SOFTWARE_ID)
                .uptime(uptime_secs)
                .voltage(voltage)
                .xcvr_temperature(temp);

            config.lock(|config| {
                cats.add_identification(ham_cats::whisker::Identification {
                    icon: config.icon,
                    callsign: config.callsign,
                    ssid: config.ssid,
                })
                .unwrap();

                if !config.comment.is_empty() {
                    cats.add_comment(&config.comment).unwrap();
                }

                cats.add_route(Route::new(config.max_hops)).unwrap();

                if let Maybe::Some(antenna_height) = config.antenna_height {
                    info_builder = info_builder.antenna_height(antenna_height);
                }

                if let Maybe::Some(gain) = config.antenna_gain {
                    info_builder = info_builder.antenna_gain(gain as f64);
                }

                if let Maybe::Some(power) = config.tx_power {
                    info_builder = info_builder.tx_power(power as f64);
                }
            });

            cats.add_node_info(info_builder.build()).unwrap();

            if let Some(pos) = pos {
                cats.add_gps(Gps::new(
                    pos.latitude,
                    pos.longitude,
                    f16::from_f32(pos.altitude_meters),
                    0,
                    pos.true_course as f64,
                    f16::from_f32(pos.speed_ms),
                ))
                .unwrap();
            }

            let buf = ctx.local.buf;
            let mut x = Buffer::new_empty(buf);
            cats.fully_encode(&mut x).unwrap();

            (ctx.shared.radio, config).lock(|r, c| {
                r.as_mut()
                    .unwrap()
                    .tx(&mut ctx.shared.red, &x, Some((&c.callsign, c.ssid)))
                    .unwrap();
            });
        }

        if ctx.shared.status.lock(|s| matches!(s, Status::Normal)) {
            transmit_position::spawn_after(transmit_period_seconds.secs()).unwrap();
        }
    }

    #[task(priority = 2, local = [green, usb_detect, gps_enable, cycles_below_thres],
		   shared = [red, status, volt_meter, radio])]
    fn led_handler(mut ctx: led_handler::Context) {
        led_handler::spawn_after(LED_BLINK_RATE.millis()).unwrap();

        let mut status = ctx.shared.status;
        let cur_status = status.lock(|x| *x);

        if ctx.local.usb_detect.is_high() && !matches!(cur_status, Status::Programming(_)) {
            status.lock(|s| *s = Status::Programming(false));

            ctx.shared.red.lock(|red| red.set_low());
            ctx.local.gps_enable.set_high();
        }

        match cur_status {
            Status::Normal | Status::TxDisabled(_) | Status::Error(_) => {
                if ctx.shared.volt_meter.lock(|vm| vm.below_threshold()) {
                    *ctx.local.cycles_below_thres += 1;
                } else {
                    *ctx.local.cycles_below_thres = 0;
                }

                if *ctx.local.cycles_below_thres > UNDERVOLT_THRES_LEN {
                    status.lock(|s| *s = Status::Sleep);
                    ctx.local.gps_enable.set_high();
                    ctx.local.green.set_low();
                    ctx.shared.red.lock(|red| red.set_low());
                    ctx.shared
                        .radio
                        .lock(|r| r.as_mut().map(|x| x.sleep().unwrap()));
                }
            }

            Status::Sleep => {
                if ctx.shared.volt_meter.lock(|vm| vm.above_threshold()) {
                    cortex_m::peripheral::SCB::sys_reset();
                }
            }

            Status::Programming(_) => {}
        }

        status.lock(|s| match s {
            Status::Normal | Status::Sleep => {}
            Status::Programming(b) => {
                ctx.local.green.set_state((*b).into());
                *b = !*b;
            }
            Status::TxDisabled(b) => {
                ctx.shared.red.lock(|red| red.set_state((*b).into()));
                *b = !*b;
            }
            Status::Error(b) => {
                ctx.shared.red.lock(|red| red.set_state((*b).into()));
                ctx.local.green.set_state((!*b).into());
                *b = !*b;
            }
        });
    }

    #[task(binds=USART2, priority = 3, shared=[gps], local=[gps_serial])]
    fn gps_uart(mut ctx: gps_uart::Context) {
        let gps_serial = ctx.local.gps_serial;

        while let Ok(b) = gps_serial.read() {
            ctx.shared.gps.lock(|gps| gps.consume(b))
        }
    }

    #[task(binds=OTG_FS, priority = 2, local=[flash], shared=[usb_dev, shell, volt_meter])]
    fn usb_fs(cx: usb_fs::Context) {
        let usb_fs::SharedResources {
            mut usb_dev,
            mut shell,
            mut volt_meter,
        } = cx.shared;

        (&mut usb_dev, &mut shell).lock(|usb_dev, shell| {
            while usb_dev.poll(&mut [shell.ushell.get_serial_mut()]) {
                shell.poll(cx.local.flash, &mut volt_meter);
            }
        });
    }

    #[idle]
    fn idle(_: idle::Context) -> ! {
        loop {
            cortex_m::asm::wfi();
        }
    }
}