use alloc::string::String;
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
use embassy_sync::{blocking_mutex::raw::NoopRawMutex, channel::DynamicSender};
use embassy_time::Timer;
use embedded_hal_async::i2c::I2c as _;
use esp_hal::{i2c::master::I2c, Async};
use log::*;
use nalgebra::Vector2;
use nmea::{Nmea, sentences::FixType};

use crate::{backoff::Backoff, events::{Measurement, SensorSource, SensorState}};

// FIXME: We need a way to put the GPS to sleep when the system goes to sleep
#[embassy_executor::task]
pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus: I2cDevice<'static, NoopRawMutex, I2c<'static, Async>>) {
    Backoff::from_secs(5).forever().attempt::<_, (), ()>(async || {
        events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Offline)).await;
        Backoff::from_secs(5).forever().attempt(async || {
            info!("Initializing GPS");
            // Enable a bunch of data? idk
            let bytes = "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
            i2c_bus.write(0x10, bytes.as_bytes()).await?;

            // 1hz updates
            let bytes = "$PMTK220,1000*1F\r\n";
            i2c_bus.write(0x10, bytes.as_bytes()).await?;

            // 1hz position fix
            let bytes = "$PMTK300,1000,0,0,0,0*1C\r\n";
            i2c_bus.write(0x10, bytes.as_bytes()).await?;

            // Antenna updates
            let bytes = "$PGCMD,33,1*6C\r\n";
            i2c_bus.write(0x10, bytes.as_bytes()).await
        }).await.unwrap();

        let mut strbuf = String::new();

        let mut parser = Nmea::default();
        let mut parsing = false;
        let mut has_lock = false;
        events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::AcquiringFix)).await;
        info!("GPS is ready!");
        loop {
            let mut buf = [0; 1];
            i2c_bus.read(0x10, &mut buf).await.map_err(|_| { Err::<(), ()>(()) }).ok();
            if (buf[0] as char == '\n' || buf[0] as char == '\r') && !strbuf.is_empty() {
                match parser.parse_for_fix(&strbuf) {
                    Ok(FixType::Invalid) if has_lock => {
                        // TODO: Send a Measurement::SensorOffline(SensorSource::GPS) here instead
                        events.send(Measurement::GPS(None)).await;
                        events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Degraded)).await;
                        has_lock = false
                    },
                    Ok(FixType::Invalid) => {
                        debug!("Waiting for fix {parser:?}");
                    },
                    Ok(fix_type) => {
                        if !has_lock {
                            has_lock = true;
                            info!("Got a fix of type {fix_type:?}");
                            events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Online)).await;
                        }

                        if let (Some(lat), Some(lng)) = (parser.latitude, parser.longitude) {
                            events.send(Measurement::GPS(Some(Vector2::new(lat, lng)))).await;
                        }

                        if let (Some(date), Some(time)) = (parser.fix_date, parser.fix_time) {
                            let now = date.and_time(time).and_utc();
                            info!("GPS time is {now}");
                        }
                    },
                    Err(nmea::Error::ParsingError(_)) => {
                        debug!("NMEA could not parse: {strbuf}");
                    },
                    Err(err) => {
                        error!("NMEA error on {strbuf} {err:?}");
                    }
                }
                strbuf = String::new();
                parsing = false;
                // Update frequency is 1hz, so we should never get an update faster than once per second
                Timer::after_secs(1).await;
            } else if strbuf.is_empty() && (buf[0] as char == '$' || buf[0] as char == '!') {
                parsing = true;
                strbuf.push(buf[0] as char);
                Timer::after_millis(10).await;
            } else if parsing {
                strbuf.push(buf[0] as char);
                Timer::after_millis(10).await;
            } else {
                // If there is no data ready for some reason, wait 500ms, which should place us at least somewhere after the next data frame is ready to read.
                Timer::after_millis(500).await;
            }
        }
    }).await.ok();
}
