#![no_std]
#![no_main]
#![deny(
    clippy::mem_forget,
    reason = "mem::forget is generally not safe to do with esp_hal types, especially those \
    holding buffers for the duration of a data transfer."
)]
// #![deny(clippy::large_stack_frames)]

use alloc::vec::Vec;
use core::cell::RefCell;

use critical_section::Mutex;
use crossbeam_queue::ArrayQueue;
use embassy_executor::Spawner;
#[cfg(feature = "gnss")]
use embassy_gps::gps::l76k;
use esp_backtrace as _;
use esp_hal::clock::CpuClock;
use esp_hal::timer::timg::TimerGroup;
#[cfg(feature = "screen")]
use esp_hal::{delay, gpio, spi, time};
#[cfg(all(target_arch = "riscv32", feature = "spat"))]
use esp_println::println;
use esp_radio::wifi;
use log::{debug, error, info, warn};

extern crate alloc;

mod applogic;
#[cfg(feature = "spat")]
mod cache;
mod geo_alg;
mod radio;
#[cfg(feature = "screen")]
mod screen;

const WIFI_CHANNEL: radio::Channel = 180;

// This creates a default app-descriptor required by the esp-idf bootloader.
// For more information see: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
esp_bootloader_esp_idf::esp_app_desc!();

#[cfg(feature = "gnss")]
static GNSS_UPDATE: Mutex<RefCell<Option<embassy_gps::types::GpsFix>>> =
    Mutex::new(RefCell::new(None));
static WIFI_RX_QUEUE: Mutex<RefCell<Option<ArrayQueue<Vec<u8>>>>> = Mutex::new(RefCell::new(None));

#[allow(
    clippy::large_stack_frames,
    reason = "it's not unusual to allocate larger buffers etc. in main"
)]
#[esp_rtos::main]
async fn main(spawner: Spawner) -> ! {
    // generator version: 1.3.0
    // generator parameters: --chip esp32c5 -o esp32c5-wroom-1-psram -o alloc -o log -o unstable-hal -o wifi -o esp-backtrace -o embassy

    esp_println::logger::init_logger_from_env();

    let config = esp_hal::Config::default().with_cpu_clock(CpuClock::max());
    let peripherals = esp_hal::init(config);

    // The following pins are used to bootstrap the chip. They are available
    // for use, but check the datasheet of the module for more information on them.
    // - GPIO2
    // - GPIO3
    // - GPIO7
    // - GPIO25
    // - GPIO26
    // - GPIO27
    // - GPIO28

    esp_alloc::heap_allocator!(#[esp_hal::ram(reclaimed)] size: 65536);
    esp_alloc::psram_allocator!(peripherals.PSRAM, esp_hal::psram);

    let timg0 = TimerGroup::new(peripherals.TIMG0);
    let sw_interrupt =
        esp_hal::interrupt::software::SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
    esp_rtos::start(timg0.timer0, sw_interrupt.software_interrupt0);

    info!("Embassy initialized!");

    // ST7789 172*320px screen
    // SCL: D8/  GPIO8  (SPI_SCK)
    // SDA: D10/ GPIO10 (SPI_MOSI)
    // RES: D3/  GPIO7
    // DC:  D4/  GPIO23
    // CS:  D5/  GPIO24
    // BL:  TODO (hard-wire to high? or D1/ GPIO0)
    #[cfg(feature = "screen")]
    let mut spi_buffer = [0_u8; 512];
    #[cfg(feature = "screen")]
    let mut display = {
        let (spi, cs) = setup_st7789_spi(
            peripherals.GPIO8,
            peripherals.GPIO10,
            peripherals.GPIO24,
            peripherals.GPIO9,
            peripherals.SPI2,
        )
        .expect("Failed to build SPI config");
        let output_config = gpio::OutputConfig::default();
        let dc_output = gpio::Output::new(peripherals.GPIO23, gpio::Level::Low, output_config);
        let rst_output = gpio::Output::new(peripherals.GPIO7, gpio::Level::High, output_config);
        let _ = gpio::Output::new(peripherals.GPIO0, gpio::Level::High, output_config); // backlight

        let mut display_delay = delay::Delay::new();

        let spi_device = embedded_hal_bus::spi::ExclusiveDevice::new_no_delay(spi, cs)
            .expect("Failed to initialize SPI device");

        mipidsi::Builder::new(
            mipidsi::models::ST7789,
            mipidsi::interface::SpiInterface::new(spi_device, dc_output, &mut spi_buffer),
        )
        .display_size(screen::HEIGHT + 34, screen::WIDTH) // somehow this display has 34 px of invisible space on the left (in native orientation)
        .reset_pin(rst_output)
        .invert_colors(mipidsi::options::ColorInversion::Inverted)
        .orientation(mipidsi::options::Orientation::new().rotate(mipidsi::options::Rotation::Deg90))
        .init(&mut display_delay)
        .expect("Failed to initialize display")
    };

    #[cfg(feature = "screen")]
    let _ = screen::draw_slash_screen(&mut display).inspect_err(log_screen_error);

    // Setup WiFi
    let (_wifi_controller, interfaces) = esp_radio::wifi::new(
        peripherals.WIFI,
        esp_radio::wifi::ControllerConfig::default(),
    )
    .expect("Failed to initialize Wi-Fi controller");

    critical_section::with(|cs| {
        WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2)));
    });

    let mut sniffer = interfaces.sniffer;
    radio::setup_wifi_sniffer(WIFI_CHANNEL, &mut sniffer, handle_frame)
        .expect("Fatal error initializing 802.11p sniffer");

    info!("WiFi promiscuous mode on channel {WIFI_CHANNEL} running");

    // Seeed XIAO L67K:
    // RX:      D7/ GPIO12
    // TX:      D6/ GPIO11
    // WAKEUP:  D0/ GPIO1 -> HIGH for active, LOW for Sleep
    // RESET:   D2/ GPIO25 -> HIGH for normal, LOW for Reset
    #[cfg(feature = "gnss")]
    spawner.spawn(
        gnss_task(
            peripherals.GPIO1,
            peripherals.GPIO25,
            peripherals.UART0,
            peripherals.GPIO12,
            peripherals.GPIO11,
        )
        .expect("Failed to spawn GNSS task"),
    );
    #[cfg(feature = "gnss")]
    info!("GNSS task started, waiting for GNSS fix...");

    let mut state = applogic::State::new();
    loop {
        embassy_time::Timer::after(embassy_time::Duration::from_millis(5)).await;

        #[cfg(feature = "gnss")]
        critical_section::with(|cs| {
            let gnss_update_ref = GNSS_UPDATE.borrow(cs);

            if let Some(fix) = gnss_update_ref.replace(None) {
                state.update_with_gpsfix(&fix);

                if fix.get_timestamp().is_some() {
                    info!("{}", state.print_fix());

                    // run GLOSA algorithm
                    #[cfg(feature = "spat")]
                    let signal_groups = state.run_glosa();

                    for sig in &signal_groups {
                        let duration_str = sig
                            .min_end_sec
                            .map(|v| alloc::format!("for {v} s"))
                            .unwrap_or_default();
                        println!("GLOSA: {} {} {duration_str}", sig.maneuver, sig.phase);
                    }

                    #[cfg(feature = "screen")]
                    let _ = screen::draw_glosa(&mut display, &signal_groups)
                        .inspect_err(log_screen_error);
                }
            }
        });

        critical_section::with(|cs| {
            let queue_rc = WIFI_RX_QUEUE.borrow(cs).borrow();
            // unwrap is fine b/c we stored something in it before
            let queue = queue_rc.as_ref().unwrap();

            if let Some(data) = queue.pop() {
                match c_its_parser::de::decode(&data, c_its_parser::Headers::IEEE802LlcGnBtp) {
                    Ok(msg) => {
                        use c_its_parser::ItsMessage;
                        use c_its_parser::standards::extensions::ItsMessageId;

                        debug!("Got new {:?} message", ItsMessageId::from(&msg));

                        match msg {
                            #[cfg(feature = "spat")]
                            ItsMessage::Mapem {
                                geonetworking: _,
                                transport: _,
                                etsi,
                            } => state.handle_mapem(&etsi),
                            #[cfg(feature = "spat")]
                            ItsMessage::Spatem {
                                geonetworking: _,
                                transport: _,
                                etsi,
                            } => {
                                if state.initialized() {
                                    state.handle_spatem(&etsi);
                                }
                            }

                            #[cfg(feature = "cam")]
                            ItsMessage::Cam {
                                geonetworking: _,
                                transport: _,
                                etsi,
                            } => applogic::cam::handle_cam(&etsi),

                            #[cfg(feature = "denm")]
                            ItsMessage::DenmV2 {
                                geonetworking: _,
                                transport: _,
                                etsi,
                            } => applogic::denm::handle_denm(&etsi),
                            #[cfg(feature = "denm")]
                            ItsMessage::DenmV1 {
                                geonetworking: _,
                                transport: _,
                                etsi: _,
                            } => {
                                // irrelevant since all received DENMs should be parsed as v2
                            }
                        }
                    }
                    Err(err) => {
                        // will give false-positives when a message is received which wasn't enabled
                        debug!("Failed to parse V2X message: {err}");
                    }
                }
            }
        });

        // maintenance
        state.prune();
    }
}

#[allow(
    clippy::needless_pass_by_value,
    reason = "adhering to callback interface"
)]
fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
    // Ignore frames with errors and non-data frames
    if frame.rx_cntl.rx_state != 0 {
        warn!("Received frame has RX error: {}", frame.rx_cntl.rx_state);
        return;
    }

    if !radio::PromiscuousPktType::Data.matches(frame.frame_type) {
        debug!("Received frame is not a DATA frame: {}", frame.frame_type);
        return;
    }

    // Ignore non-broadcast frames
    if frame.len < (4 + 6) {
        warn!(
            "Received frame too small to be a valid broadcast frame: {} bytes",
            frame.len
        );
        return;
    }
    let dest_mac = &frame.data[4..10];
    if dest_mac != [0xff; 6] {
        warn!("Received frame is not broadcast frame: Dest MAC {dest_mac:02x?}");
        return;
    }

    debug!("Received frame with {} bytes", frame.len);

    // Send data to main thread
    critical_section::with(|cs| {
        let queue_rc = WIFI_RX_QUEUE.borrow(cs).borrow();
        // unwrap is fine b/c we stored something in it before
        let queue = queue_rc.as_ref().unwrap();

        if queue.push(frame.data.to_vec()).is_err() {
            error!("V2X RX queue is full");
        }
    });
}

#[cfg(feature = "gnss")]
#[allow(clippy::large_stack_frames, reason = "GNSS driver needs some space")]
#[embassy_executor::task]
async fn gnss_task(
    gps_wakeup: esp_hal::peripherals::GPIO1<'static>,
    gps_reset: esp_hal::peripherals::GPIO25<'static>,
    uart: esp_hal::peripherals::UART0<'static>,
    uart_rx: esp_hal::peripherals::GPIO12<'static>,
    uart_tx: esp_hal::peripherals::GPIO11<'static>,
) {
    use embassy_gps::gps::GpsFsm;

    let mut gps = l76k::esp::L76kFsm::new_sep(
        l76k::esp::GpsHw {
            reinit: gps_reset,
            standby: gps_wakeup,
        },
        || {
            esp_hal::uart::Uart::new(uart, esp_hal::uart::Config::default().with_baudrate(9600))
                .expect("Failed to create UART for GNSS")
                .with_rx(uart_rx)
                .with_tx(uart_tx)
                .into_async()
        },
    )
    .await;

    let mut fix_drop_message = false;
    loop {
        if let Ok(Some(embassy_gps::types::GpsEvent::Fix(fix))) = gps.step().await {
            // drop every second message as we always get each fix twice
            fix_drop_message = !fix_drop_message;
            if fix_drop_message {
                // send to main thread
                critical_section::with(|cs| {
                    let gnss_update_ref = GNSS_UPDATE.borrow(cs);
                    gnss_update_ref.replace(Some(fix));
                });
            }
        } else {
            // FSM will recover automatically from errors and ignore other event types
        }
    }
}

/// Builds the SPI interface for the ST7789 display
///
/// Returns (spi master, chip-select) tuple or SPI config error
#[cfg(feature = "screen")]
fn setup_st7789_spi(
    scl: esp_hal::peripherals::GPIO8<'static>,
    sda: esp_hal::peripherals::GPIO10<'static>,
    cs: esp_hal::peripherals::GPIO24<'static>,
    miso: esp_hal::peripherals::GPIO9<'static>,
    spi: esp_hal::peripherals::SPI2<'static>,
) -> Result<
    (
        spi::master::Spi<'static, esp_hal::Blocking>,
        gpio::Output<'static>,
    ),
    spi::master::ConfigError,
> {
    let spi_config = spi::master::Config::default()
        .with_frequency(time::Rate::from_mhz(10))
        .with_mode(spi::Mode::_0);
    let spi: spi::master::Spi<'_, esp_hal::Blocking> = spi::master::Spi::new(spi, spi_config)?
        .with_sck(scl)
        .with_mosi(sda)
        .with_miso(miso);

    let cs_output = gpio::Output::new(cs, gpio::Level::High, gpio::OutputConfig::default());

    Ok((spi, cs_output))
}

#[cfg(feature = "screen")]
fn log_screen_error<E>(error: &E)
where
    E: core::fmt::Debug,
{
    error!("graphics failed: {error:?}");
}