use alloc::string::String;
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, 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;

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

#[allow(dead_code)] //FIXME: Allow switching to this via configure option
const GPS_TEST_DATA: &str = include_str!("../test.nmea");

#[embassy_executor::task]
pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
    Backoff::from_secs(5).forever().attempt::<_, (), ()>(async || {
        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;
        //let mut iter = GPS_TEST_DATA.as_bytes().iter();
        info!("GPS is ready!");
        loop {
            let mut buf = [0; 1];
            i2c_bus.read(0x10, &mut buf).await.map_err(|_| { Err::<(), ()>(()) }).ok();
            //buf[0] = *(iter.next().unwrap());
            if (buf[0] as char == '\n' || buf[0] as char == '\r') && !strbuf.is_empty() {
                if let Ok(result) = parser.parse(&strbuf) {
                    match parser.fix_type {
                        None if has_lock => {
                            events.send(Measurement::GPS(None)).await;
                            has_lock = false
                        },
                        None => (),
                        Some(_) => {
                            if !has_lock {
                                has_lock = true;
                            }

                            //TODO: 4 satellites seems to be "Some" fix, 6 is a perfect fix
                            //TODO: Only send updates when we get the correct nmea sentence
                            if let (Some(lat), Some(lng)) = (parser.latitude, parser.longitude) {
                                events.send(Measurement::GPS(Some(Vector2::new(lat, lng)))).await;
                            }
                        }
                    }
                    log::info!("nmea={result:?} raw={strbuf:?}");
                    log::debug!("nmea={parser:?}");
                    log::info!("speed={:?} altitude={:?} lat={:?} lng={:?} fix={:?}", parser.speed_over_ground, parser.altitude, parser.latitude, parser.longitude, parser.fix_type);
                    for sat in parser.satellites() {
                        info!("\t{} snr={:?} prn={:?}", sat.gnss_type(), sat.snr(), sat.prn())
                    }
                } else {
                    log::warn!("Unhandled NMEA {strbuf:?}");
                }
                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();
}