#![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)]

// we can't use UART I/O and GNSS at the same time
#[cfg(all(feature = "uart", feature = "gnss_ubx", feature = "gnss_l67k"))]
compile_error!(
    "feature \"uart\" and features \"gnss_ubx\" or \"gnss_l67k\" cannot be enabled at the same time"
);
// autonomous CAM generation need a time and position source (aka _gnss)
#[cfg(all(feature = "cam_tx", not(feature = "_gnss")))]
compile_error!("feature \"cam_tx\" needs a time and position source (either GNSS or UART)");
// we can't have both GNSS drivers enabled at the same time
#[cfg(all(feature = "gnss_ubx", feature = "gnss_l67k"))]
compile_error!("feature \"gnss_ubx\" and feature \"gnss_l67k\" cannot be enabled at the same time");
// at least one V2X feature needs to be enabled
#[cfg(not(any(feature = "spat", feature = "cam", feature = "denm")))]
compile_error!("at least one V2X feature (\"spat\", \"cam\" or \"denm\") needs to be enabled");

use alloc::string::ToString as _;
use alloc::vec::Vec;
use core::cell::RefCell;
#[cfg(feature = "spat")]
use core::cell::UnsafeCell;

use c_its_parser::gn as geonetworking;
#[cfg(feature = "cam_tx")]
use c_its_parser::standards::extensions::ItsStationType;
use critical_section::Mutex;
use crossbeam_queue::ArrayQueue;
use embassy_executor::Spawner;
#[cfg(feature = "gnss_l67k")]
use embassy_gps::gps::l76k;
use esp_backtrace as _;
use esp_hal::clock::CpuClock;
use esp_hal::timer::timg::TimerGroup;
#[cfg(target_arch = "riscv32")]
use esp_println::println;
use esp32_cits_core::radio;
use geonetworking::Decode as _;
use log::{error, info, warn};

extern crate alloc;

mod applogic;
#[cfg(feature = "ble")]
mod ble;
mod cache;
mod geo_alg;
#[cfg(any(feature = "uart", feature = "ble"))]
mod io;
#[cfg(feature = "screen")]
mod screen;
#[cfg(feature = "gnss_ubx")]
mod ubx;

const WIFI_CHANNEL: radio::Channel = 180;
const WIFI_TXPOWER: i8 = 10; // dBm, 2..20
#[cfg(feature = "spat")]
const SPAT_RATE_LIMIT: u8 = 5; // keep every n-th message
#[cfg(any(feature = "cam_tx", feature = "uart"))]
const GN_IS_MOBILE: bool = true;
#[cfg(feature = "cam_tx")]
const OWN_STATION_TYPE: ItsStationType = ItsStationType::Cyclist;
#[cfg(feature = "cam_tx")]
const OWN_VEHICLE_WIDTH: u8 = 7; // in 10cm steps!
#[cfg(feature = "cam_tx")]
const OWN_VEHICLE_LENGTH: u16 = 18; // in 10cm steps!
#[cfg(feature = "_uart")]
const SERIAL_FIFO_SIZE_THLD: u8 = 100; // hardware has max. 128 byte

// 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!();

// Print a backtrace and reset (instead of halting)
#[panic_handler]
fn panic_handler(info: &core::panic::PanicInfo) -> ! {
    println!("");
    println!("====================== PANIC ======================");
    println!("{}", info);

    println!("\nBacktrace:");
    let backtrace = esp_backtrace::Backtrace::capture();
    for frame in backtrace.frames() {
        println!("- 0x{:x}", frame.program_counter());
    }

    println!("================== BACKTRACE END ==================");

    // reboot
    println!("-> Doing a SW reset after panic...\n");
    esp_hal::system::software_reset();
}

#[cfg(feature = "_gnss")]
static GNSS_UPDATE: Mutex<RefCell<Option<applogic::GnssFix>>> = Mutex::new(RefCell::new(None));
static WIFI_RX_QUEUE: Mutex<RefCell<Option<ArrayQueue<Vec<u8>>>>> = Mutex::new(RefCell::new(None));
#[cfg(feature = "_uart")]
static UART_RX_BUF: Mutex<RefCell<Option<Vec<u8>>>> = Mutex::new(RefCell::new(None));
#[cfg(feature = "_uart")]
static SERIAL: Mutex<RefCell<Option<esp_hal::uart::Uart<'static, esp_hal::Blocking>>>> =
    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 -o ble-trouble

    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);
    // COEX needs more RAM - so we've added some more
    esp_alloc::heap_allocator!(size: 64 * 1024);
    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!");

    // Setup ST7789 screen
    // SCL: D8/  GPIO8  (SPI_SCK)
    // SDA: D10/ GPIO10 (SPI_MOSI)
    // RES: D3/  GPIO7
    // DC:  D4/  GPIO23 (display clear)
    // CS:  D5/  GPIO24 (chip select)
    // BL:  D1/  GPIO0  (backlight)
    #[cfg(feature = "screen")]
    let mut spi_buffer = [0_u8; 512];
    #[cfg(feature = "screen")]
    let mut screen = {
        // 240*280px in portrait orientation
        let (display, size) = screen::make_large_display(
            peripherals.GPIO8,
            peripherals.GPIO10,
            peripherals.GPIO7,
            peripherals.GPIO23,
            peripherals.GPIO24,
            peripherals.GPIO0,
            peripherals.SPI2,
            &mut spi_buffer,
        )
        .expect("Fatal error building screen");

        let mut screen = screen::Handler::new(display, size);

        let _ = screen
            .update(screen::ScreenState::Initial)
            .inspect_err(log_screen_error);

        screen
    };

    // Setup UART I/O channel
    #[cfg(feature = "uart")]
    setup_uart0(
        115_200,
        Some(b'\n'),
        peripherals.GPIO12,
        peripherals.GPIO11,
        peripherals.UART0,
        serial_handler,
    )
    .expect("Failed to initialize UART");

    // Setup WiFi
    // sadly, it doesn't work any more when we move `esp_radio::wifi::new` to `setup_wifi_sniffer`
    let (mut wifi_controller, interfaces) = esp_radio::wifi::new(
        peripherals.WIFI,
        esp_radio::wifi::ControllerConfig::default(),
    )
    .expect("Failed to initialize Wi-Fi controller");

    if let Err(err) = wifi_controller.set_band_mode(esp_radio::wifi::BandMode::_5G) {
        error!("Failed to set WiFi to 5GHz only: {err}");
    }
    if let Err(err) = wifi_controller.set_max_tx_power(WIFI_TXPOWER * 4) {
        error!("Failed to set WiFi TX power: {err}");
    }

    critical_section::with(|cs| {
        WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2)));
    });
    let mut wlan_iface = interfaces.sniffer;
    radio::setup_wifi_sniffer(WIFI_CHANNEL, &mut wlan_iface, handle_frame)
        .expect("Fatal error initializing 802.11p sniffer");

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

    let mac = esp_hal::efuse::base_mac_address();
    info!("WiFi MAC: {mac}");

    // Setup BLE
    #[cfg(feature = "ble")]
    {
        let controller = ble::make_controller(peripherals.BT);
        spawner.spawn(ble::run(controller).expect("Failed to spawn BLE task"));
    }

    // Setup GNSS:
    // 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_l67k")]
    spawner.spawn(
        gnss_l67k_task(
            peripherals.GPIO1,
            peripherals.GPIO25,
            peripherals.UART0,
            peripherals.GPIO12,
            peripherals.GPIO11,
        )
        .expect("Failed to spawn GNSS task"),
    );

    // U-Blox module:
    // RX:      D7/ GPIO12
    // TX:      D6/ GPIO11
    // RESET:   D2/ GPIO25 -> HIGH for normal, LOW for Reset
    #[cfg(feature = "gnss_ubx")]
    {
        setup_uart0(
            9600,
            Some(b'\n'),
            peripherals.GPIO12,
            peripherals.GPIO11,
            peripherals.UART0,
            serial_handler,
        )
        .expect("Failed to initialize UART for GNSS");

        // do some settings (UART receive and parsing runs in main loop)
        spawner
            .spawn(gnss_ubx_config(peripherals.GPIO25).expect("Failed to spawn GNSS config task"));
    }

    #[cfg(feature = "_gnss")]
    info!("GNSS task started, waiting for GNSS fix...");

    #[cfg(any(feature = "cam_tx", feature = "uart"))]
    let (mut seq_no, mac_bytes) = {
        // unwrap is fine since [`esp_hal::efuse::MacAddress`] is just an `[u8; 6]`
        let mac_bytes = mac.as_bytes().try_into().unwrap();

        (0, mac_bytes)
    };

    let mut state = applogic::State::new();
    #[cfg(feature = "cam_tx")]
    let mut cam = {
        let station_id = make_station_id(mac);
        info!("V2X Station ID: {station_id}");

        applogic::cam_tx::CamState::new(
            station_id,
            OWN_STATION_TYPE,
            OWN_VEHICLE_LENGTH,
            OWN_VEHICLE_WIDTH,
        )
    };
    #[cfg(feature = "uart")]
    let mut uart_input = io::Parser::default();
    #[cfg(feature = "gnss_ubx")]
    let mut ubx_parser = ublox::Parser::default();
    loop {
        embassy_time::Timer::after(embassy_time::Duration::from_millis(1)).await;

        #[cfg(feature = "_uart")]
        {
            let mut new_data = None;
            critical_section::with(|cs| {
                let mut buf_rc = UART_RX_BUF.borrow(cs).borrow_mut();
                // unwrap is fine b/c we stored something in it before
                let buf = buf_rc.as_mut().unwrap();

                // we move the buffer to new_data to do the processing outside of the critical section lock
                if !buf.is_empty() {
                    new_data = Some(core::mem::take(buf));
                }
            });

            if let Some(data) = new_data {
                #[cfg(feature = "uart")]
                {
                    for msg in uart_input.parse(&data) {
                        match msg.payload {
                            Some(io::msg::serial_input_msg::Payload::Position(fix)) => {
                                let pos_state = fix.into();

                                critical_section::with(|cs| {
                                    let gnss_update_ref = GNSS_UPDATE.borrow(cs);
                                    gnss_update_ref.replace(Some(pos_state));
                                });
                            }
                            Some(io::msg::serial_input_msg::Payload::TxMsg(tx_request)) => {
                                use esp32_cits_core::tx::GnItsPayload as _;

                                info!(
                                    "UART: Got new TX message: Type {} with {} bytes",
                                    tx_request.message_type,
                                    tx_request.payload.len()
                                );
                                let own_position = state.pos();
                                let time = state.time().and_utc();

                                match tx_request.encode_packet(
                                    mac_bytes,
                                    own_position,
                                    time,
                                    GN_IS_MOBILE,
                                    &mut seq_no,
                                ) {
                                    Ok(data) => {
                                        if let Err(err) = radio::send_80211_bcast_frame(
                                            &mut wlan_iface,
                                            &mac_bytes.clone(),
                                            &data,
                                        ) {
                                            warn!("Failed to send 802.11 frame: {err}");
                                        }
                                    }
                                    Err(err) => warn!("Failed to create GN message: {err}"),
                                }
                            }
                            None => {}
                        }
                    }
                }
                #[cfg(feature = "gnss_ubx")]
                {
                    let mut it = ubx_parser.consume_ubx(&data);
                    loop {
                        match it.next() {
                            Some(Ok(ublox::UbxPacket::Proto27(packet_ref))) => match &packet_ref {
                                ublox::proto27::PacketRef::MonVer(packet) => {
                                    println!("{}", ubx::pretty_print_mon_ver(packet));
                                }
                                ublox::proto27::PacketRef::MonComms(packet) => {
                                    println!("{}", ubx::pretty_print_mon_comms(packet));
                                }
                                ublox::proto27::PacketRef::MonRf(packet) => {
                                    println!("{}", ubx::pretty_print_mon_rf(packet));
                                }
                                ublox::proto27::PacketRef::NavSat(packet) => {
                                    println!("{}", ubx::pretty_print_nav_sat(packet));
                                }
                                ublox::proto27::PacketRef::NavPvt(pvt) => {
                                    // error can be ignored b/c it's just when no GNSS fix yet
                                    if let Ok(pos_state) = applogic::GnssFix::try_from(pvt) {
                                        critical_section::with(|cs| {
                                            let gnss_update_ref = GNSS_UPDATE.borrow(cs);
                                            gnss_update_ref.replace(Some(pos_state));
                                        });
                                    }

                                    // update GNSS HB on screen
                                    let _ = screen.update_gnss(pvt);
                                }
                                ublox::proto27::PacketRef::AckNak(nak) => {
                                    error!("UBX: {nak:?}");
                                }
                                ublox::proto27::PacketRef::AckAck(ack) => {
                                    info!("UBX: {ack:?}");
                                }
                                _ => {
                                    println!("UBX: {packet_ref:?}");
                                }
                            },
                            Some(Err(_)) => {
                                // Received a malformed packet
                            }
                            None => {
                                // The internal buffer is now empty
                                break;
                            }
                        }
                    }
                }
            }
        }

        #[cfg(feature = "ble")]
        {
            critical_section::with(|cs| {
                let mut data_rc = ble::RX_DATA.borrow(cs).borrow_mut();
                if let Some(pos) = data_rc.take() {
                    let gnss_update_ref = GNSS_UPDATE.borrow(cs);
                    gnss_update_ref.replace(Some(pos.into()));
                }
            });
        }

        #[cfg(feature = "_gnss")]
        {
            let mut new_position = false;
            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);
                    new_position = true;

                    #[cfg(feature = "cam_tx")]
                    {
                        use esp32_cits_core::tx::GnItsPayload as _;
                        let time = state.time().and_utc();

                        cam.update(time, fix.clone().into());

                        match cam.encode_packet(
                            mac_bytes,
                            &fix.into(),
                            time,
                            GN_IS_MOBILE,
                            &mut seq_no,
                        ) {
                            Ok(data) => {
                                if let Err(err) = radio::send_80211_bcast_frame(
                                    &mut wlan_iface,
                                    &mac_bytes.clone(),
                                    &data,
                                ) {
                                    warn!("Failed to send 802.11 frame: {err}");
                                }
                            }
                            Err(err) => warn!("Failed to create CAM: {err}"),
                        }
                    }
                }
            });

            if new_position {
                info!("{}", state.print_fix());

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

                    if let applogic::GlosaOutput::Locked(data) = &glosa_data {
                        for sig in &data.signal_groups {
                            let duration_str = sig
                                .timing
                                .as_ref()
                                .map(|v| alloc::format!("for {} s", v.min_end_sec))
                                .unwrap_or_default();
                            println!("GLOSA: {} {} {duration_str}", sig.maneuver, sig.phase);
                        }
                    }

                    #[cfg(feature = "screen")]
                    let _ = screen
                        .update(glosa_data.into())
                        .inspect_err(log_screen_error);
                }
            }
        }

        let mut new_data = None;
        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() {
                new_data = Some(data);
            }
        });
        if let Some(data) = new_data {
            match c_its_parser::de::decode(&data, c_its_parser::Headers::None) {
                Ok(msg) => {
                    use c_its_parser::ItsMessage;
                    // use c_its_parser::standards::extensions::ItsMessageId;

                    // info!("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(any(feature = "cam", feature = "cam_tx"))]
                        ItsMessage::Cam {
                            geonetworking: _,
                            transport: _,
                            etsi,
                        } => {
                            #[cfg(feature = "cam")]
                            applogic::cam::handle_cam(&etsi);

                            // Send data to BLE thread
                            #[cfg(all(feature = "ble", feature = "cam"))]
                            critical_section::with(|cs| {
                                let ble_update_ref = ble::TX_DATA.borrow(cs);
                                ble_update_ref.replace(Some(ble::BleSendable::Cam(etsi.into())));
                            });
                        }

                        #[cfg(feature = "denm")]
                        ItsMessage::DenmV2 {
                            geonetworking: _,
                            transport: _,
                            etsi,
                        } => {
                            applogic::denm::handle_denm(&etsi);

                            // Send data to BLE thread
                            #[cfg(feature = "ble")]
                            critical_section::with(|cs| {
                                let ble_update_ref = ble::TX_DATA.borrow(cs);
                                ble_update_ref.replace(Some(ble::BleSendable::Denm(etsi.into())));
                            });
                        }
                        #[cfg(feature = "denm")]
                        ItsMessage::DenmV1 {
                            geonetworking: _,
                            transport: _,
                            etsi: _,
                        } => {
                            // irrelevant since all received DENMs should be parsed as v2
                        }
                    }
                }
                Err(err) => {
                    // we already filtered out all unsupported message types in the wifi rx callback
                    // so these are actual errors
                    warn!("Failed to parse V2X message: {err}");
                }
            }
        }

        // maintenance
        state.prune();
    }
}

#[allow(
    clippy::needless_pass_by_value,
    reason = "adhering to callback interface"
)]
#[allow(clippy::too_many_lines)]
fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) {
    #[cfg(feature = "spat")]
    const PRUNE_INTERVAL: chrono::Duration = chrono::Duration::seconds(60);
    #[cfg(feature = "spat")]
    static mut RATELIMIT_CACHE_LAST_PRUNE: UnsafeCell<chrono::NaiveDateTime> =
        UnsafeCell::new(chrono::NaiveDateTime::MIN);

    #[cfg(feature = "spat")]
    static mut SPAT_RATELIMIT_CACHE: UnsafeCell<cache::Cache<u32, RatelimitData>> = UnsafeCell::new(
        cache::Cache::<u32, RatelimitData>::new(chrono::Duration::seconds(20)),
    );

    #[cfg(feature = "spat")]
    let now = {
        let time = esp_hal::time::Instant::now().duration_since_epoch();
        // unwrap is fine since value range is u64 microseconds
        chrono::DateTime::from_timestamp(time.as_secs().cast_signed(), 0)
            .unwrap()
            .naive_utc()
    };

    match esp32_cits_core::rx::is_broadcast_frame(&frame) {
        Err(err) => {
            warn!("{err}");
            return;
        }
        Ok(false) => {
            // drop frame
            return;
        }
        Ok(true) => {
            // continue
        }
    }

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

    // parse GN headers
    match c_its_parser::pcap::remove_wlan_headers(frame.data)
        .map_err(|err| alloc::format!("Failed to parse WLAN headers: {err}"))
        .and_then(|data| {
            geonetworking::Packet::decode(data)
                .map_err(|err| alloc::format!("Failed to parse GN headers: {err:?}"))
        }) {
        Err(err) => {
            warn!("{err}");
        }
        Ok(packet) => {
            // drop hopped (for now)
            if packet.decoded.is_hopped() {
                return;
            }

            // "parse" ITS PDU Header
            if let Ok((payload, message_id, station_id)) = get_its_header(&packet) {
                // drop all unsupported message IDs and rate-limit SPATEMs
                if match message_id {
                    #[cfg(feature = "denm")]
                    c_its_parser::standards::extensions::ItsMessageId::Denm => false,
                    #[cfg(feature = "cam")]
                    c_its_parser::standards::extensions::ItsMessageId::Cam => false,
                    #[cfg(feature = "spat")]
                    c_its_parser::standards::extensions::ItsMessageId::Spatem => {
                        // reduce SPAT rate per station ID
                        let mut drop_msg = false;
                        unsafe {
                            #[allow(
                                static_mut_refs,
                                reason = "this function will only run consecutive"
                            )]
                            let spat_cache = SPAT_RATELIMIT_CACHE.get();

                            (*spat_cache).update_or_init(now, station_id, |i| {
                                i.msg_count = i.msg_count.wrapping_add(1);

                                if (i.msg_count % SPAT_RATE_LIMIT) > 0 {
                                    drop_msg = true;
                                }
                            });
                        }

                        drop_msg
                    }
                    #[cfg(feature = "spat")]
                    c_its_parser::standards::extensions::ItsMessageId::Mapem => false,
                    _ => true,
                } {
                    return;
                }

                // 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(payload.to_vec()).is_err() {
                        error!("V2X RX queue is full");
                    }
                });

                // Send data to UART
                #[cfg(feature = "uart")]
                {
                    let message = io::msg::RawMsgRx {
                        msg_type: message_id.as_u8().into(),
                        payload: frame.data.to_vec(),
                    }
                    .serialize_uart_proto();

                    send_uart0(&message);
                }
            }
        }
    }

    #[cfg(feature = "spat")]
    unsafe {
        #[allow(static_mut_refs, reason = "this function will only run consecutive")]
        let last_prune_time = RATELIMIT_CACHE_LAST_PRUNE.get();
        if *last_prune_time + PRUNE_INTERVAL < now {
            #[allow(static_mut_refs, reason = "this function will only run consecutive")]
            let spat_cache = SPAT_RATELIMIT_CACHE.get();
            (*spat_cache).prune(now);

            *last_prune_time = now;
        }
    }
}

#[cfg(feature = "spat")]
#[derive(Debug, Default, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
struct RatelimitData {
    station_id: u32,
    msg_count: u8,
}

#[cfg(feature = "spat")]
impl cache::Cachable<u32> for RatelimitData {
    fn key(&self) -> u32 {
        self.station_id
    }
}
#[cfg(feature = "spat")]
impl cache::Initable<Self, u32> for RatelimitData {
    fn init(key: u32) -> Self {
        Self {
            station_id: key,
            ..Default::default()
        }
    }
}

fn get_its_header<'p>(
    packet: &geonetworking::Decoded<geonetworking::Packet<'p>>,
) -> Result<
    (
        &'p [u8],
        c_its_parser::standards::extensions::ItsMessageId,
        u32,
    ),
    alloc::string::String,
> {
    let payload = packet.decoded.btp_payload()?;

    if payload.len() < 6 {
        return Err("ITS payload too small for ItsPduHeader".to_string());
    }

    if let Ok(message_id) = payload[1].try_into() {
        let station_id_buf: [u8; 4] = payload[2..6].try_into().unwrap();
        let station_id = u32::from_be_bytes(station_id_buf);

        Ok((payload, message_id, station_id))
    } else {
        Err("Unknown message ID in ItsPduHeader".to_string())
    }
}

#[cfg(feature = "gnss_l67k")]
#[allow(clippy::large_stack_frames, reason = "GNSS driver needs some space")]
#[embassy_executor::task]
async fn gnss_l67k_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
                let pos_state = applogic::GnssFix {
                    time: fix.get_timestamp(),
                    latitude_deg: fix.latitude_deg,
                    longitude_deg: fix.longitude_deg,
                    heading_deg: fix.true_course_deg,
                    speed_mps: fix.speed_over_ground,
                };

                critical_section::with(|cs| {
                    let gnss_update_ref = GNSS_UPDATE.borrow(cs);
                    gnss_update_ref.replace(Some(pos_state));
                });
            }
        } else {
            // FSM will recover automatically from errors and ignore other event types
        }
    }
}

#[cfg(feature = "gnss_ubx")]
#[embassy_executor::task]
async fn gnss_ubx_config(reset_pin: esp_hal::peripherals::GPIO25<'static>) {
    // reset the module
    let mut gnss_reset = esp_hal::gpio::Output::new(
        reset_pin,
        esp_hal::gpio::Level::Low,
        esp_hal::gpio::OutputConfig::default(),
    );
    embassy_time::Timer::after(embassy_time::Duration::from_millis(2)).await;
    gnss_reset.set_high();

    embassy_time::Timer::after(embassy_time::Duration::from_secs(1)).await;

    info!("UBX: Enabling ubx protocol with NAV-PVT and NAV-SAT");
    let buffer = ublox::packets::cfg_val::CfgValSetBuilder {
        version: 1,
        layers: ublox::packets::cfg_val::CfgLayerSet::RAM,
        reserved1: 0,
        cfg_data: &[
            ublox::cfg_val::CfgVal::Uart1OutProtNmea(false),
            ublox::cfg_val::CfgVal::Uart1OutProtUbx(true),
            // CFG-RATE-MEAS
            ublox::cfg_val::CfgVal::RateMeas(500),
            // CFG-MSGOUT-UBX_NAV_PVT_UART1
            ublox::cfg_val::CfgVal::MsgOutUbxNavPvtUart1(1),
            // CFG-MSGOUT-UBX_NAV_SAT_UART1
            // ublox::cfg_val::CfgVal::MsgOutUbxNavSatUart1(10),
        ],
    }
    .into_packet_vec();
    send_uart0(&buffer);
}

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

#[cfg(feature = "_uart")]
/// Configures UART0 with an interrupt handler and saves it to `SERIAL`
fn setup_uart0(
    baud: u32,
    cmd_char: Option<u8>,
    gpio12: esp_hal::peripherals::GPIO12<'static>,
    gpio11: esp_hal::peripherals::GPIO11<'static>,
    uart0: esp_hal::peripherals::UART0<'static>,
    handler: esp_hal::interrupt::InterruptHandler,
) -> Result<(), alloc::string::String> {
    critical_section::with(|cs| {
        UART_RX_BUF
            .borrow(cs)
            .replace(Some(alloc::vec::Vec::with_capacity(255)));
    });

    let mut uart = esp_hal::uart::Uart::new(
        uart0,
        esp_hal::uart::Config::default()
            .with_baudrate(baud)
            .with_rx(
                esp_hal::uart::RxConfig::default()
                    .with_fifo_full_threshold(u16::from(SERIAL_FIFO_SIZE_THLD))
                    .with_timeout(50),
            ),
    )
    .map_err(|err| alloc::format!("UART0 config error: {err}"))?
    .with_rx(gpio12)
    .with_tx(gpio11);

    if let Some(char) = cmd_char {
        uart.set_at_cmd(
            esp_hal::uart::AtCmdConfig::default()
                .with_cmd_char(char)
                .with_char_num(1)
                .with_pre_idle_count(0) // command start can be right after other data
                .with_post_idle_count(0) // we don't expect a gap between the command start and subsequent text
                .with_gap_timeout(5), // needs to be >0 to work
        );
    }
    uart.set_interrupt_handler(handler);
    uart.listen(
        esp_hal::uart::UartInterrupt::AtCmd
            | esp_hal::uart::UartInterrupt::RxTimeout
            | esp_hal::uart::UartInterrupt::RxFifoFull, // just to be save
    );

    critical_section::with(|cs| SERIAL.borrow_ref_mut(cs).replace(uart));

    Ok(())
}

#[cfg(feature = "_uart")]
#[esp_hal::handler]
fn serial_handler() {
    critical_section::with(|cs| {
        let mut serial = SERIAL.borrow_ref_mut(cs);
        let mut uart_buf = UART_RX_BUF.borrow_ref_mut(cs);

        if let (Some(serial), Some(uart_buf)) = (serial.as_mut(), uart_buf.as_mut()) {
            let mut buf = [0u8; SERIAL_FIFO_SIZE_THLD as usize];

            match serial.read_buffered(&mut buf) {
                Ok(0) => {}
                Ok(len) => {
                    uart_buf.extend_from_slice(&buf[..len]);
                }
                Err(err) => error!("Failed to read from UART: {err}"),
            }

            serial.clear_interrupts(
                esp_hal::uart::UartInterrupt::RxFifoFull
                    | esp_hal::uart::UartInterrupt::RxTimeout
                    | esp_hal::uart::UartInterrupt::AtCmd,
            );
        }
    });
}

#[cfg(feature = "_uart")]
fn send_uart0(data: &[u8]) {
    critical_section::with(|cs| {
        let mut serial = SERIAL.borrow_ref_mut(cs);

        if let Some(serial) = serial.as_mut()
            && let Err(err) = serial.write(data)
        {
            error!("Failed to write to UART: {err:?}");
        }
    });
}

#[cfg(all(feature = "esp", feature = "cam_tx"))]
/// Creates a station ID from the WLAN MAC address
///
/// Will use `0xC173` as static prefix and uses last 16 bit from MAC address for the remainder.
///
/// # Panics
/// Won't panic in practive b/c of pre-conditions
#[must_use]
pub fn make_station_id(mac: esp_hal::efuse::MacAddress) -> u32 {
    // unwrap is fine here since we can be sure that the MacAddress is 6 byte
    // and a slice of 2 bytes can be converted to [u8; 2]
    let mac_last_16bit = u16::from_be_bytes(mac.as_bytes().get(4..6).unwrap().try_into().unwrap());

    0xC173_0000 | u32::from(mac_last_16bit)
}