use anyhow;

use std::{
    sync::mpsc::SyncSender,
    thread,
    time::{Duration, Instant},
    io::{Read, Write},
};

use esp_idf_hal::prelude::*;
use esp_idf_hal::serial::{self, Rx, Tx};

use ublox::*;

use crate::types::*;
use crate::serial::SerialIO;

struct GpsModule<UART: serial::Uart> {
    port: SerialIO<UART>,
    parser: Parser<Vec<u8>>,
}

impl<UART: serial::Uart> GpsModule<UART> {
    pub fn new(tx: Tx<UART>, rx: Rx<UART>) -> Self {
        let parser = Parser::default();
        GpsModule { port: SerialIO::new(tx, rx), parser }
    }

    pub fn write_all(&mut self, data: &[u8]) -> std::io::Result<()> {
        println!("WRITE: {:?}", data);
        self.port.write(data).map(|_| ())
    }

    pub fn update<T: FnMut(PacketRef)>(&mut self, mut cb: T) -> std::io::Result<()> {
        println!("UPDATING ...     ...     ...");
        loop {
            let mut local_buf = [0; 1024];
            let nbytes = self.read_port(&mut local_buf)?;
            if nbytes == 0 {
                println!("no bytes to read :(");
                break;
            }
            // parser.consume adds the buffer to its internal buffer, and
            // returns an iterator-like object we can use to process the packets
            let mut it = self.parser.consume(&local_buf[..nbytes]);
            while let Some(Ok(packet)) = it.next() {
                println!("READ: {:?}", packet);
                cb(packet);
            }
        }
        Ok(())
    }

    pub fn wait_for_ack<T: UbxPacketMeta>(&mut self, timeout: Duration) -> std::io::Result<bool> {
        let mut found_packet = false;
        println!("LOOKING FOR ACK ...");
        let start = Instant::now();
        while !found_packet && start.elapsed() < timeout {
            self.update(|packet| {
                if let PacketRef::AckAck(ack) = packet {
                    if ack.class() == T::CLASS && ack.msg_id() == T::ID {
                        println!("FOUND PACKET: {} {}", ack.class(), ack.msg_id());
                        found_packet = true;
                    }
                }
                else if let PacketRef::AckNak(nak) = packet {
                    println!("NAK PACKET: {} {}", nak.class(), nak.msg_id());
                }
            })?;
        }
        println!("exiting wait_for_ack");
        Ok(found_packet)
    }

    /// Reads the serial port, converting timeouts into "no data received"
    fn read_port(&mut self, output: &mut [u8]) -> std::io::Result<usize> {
        match self.port.read(output) {
            Ok(b) => Ok(b),
            Err(e) => {
                if e.kind() == std::io::ErrorKind::TimedOut {
                    Ok(0)
                } else {
                    Err(e)
                }
            }
        }
    }
}

pub fn main<T: Sync + Send>(
    tx: esp_idf_hal::gpio::Gpio12<T>,
    rx: esp_idf_hal::gpio::Gpio13<T>,
    uart: serial::UART2,
    sender: SyncSender<Msg>,
) -> Result<(), anyhow::Error> {
    let serial_pins = serial::Pins {
        tx,
        rx,
        cts: None,
        rts: None,
    };

    let serial_config = serial::config::Config::default()
        .baudrate(Hertz(9600))
        .data_bits(serial::config::DataBits::DataBits8)
        .parity_none()
        .flow_control(serial::config::FlowControl::None)
        .stop_bits(serial::config::StopBits::STOP1);

    let serial: serial::Serial<serial::UART2, _, _> = serial::Serial::new(
        uart,
        serial_pins,
        serial_config,
    )?;

    let (tx, rx) = serial.split();
    let mut device = GpsModule::new(tx, rx);

    // Configure the device to talk UBX
    device
        .write_all(
            &CfgPrtUartBuilder {
                portid: UartPortId::Uart1,
                reserved0: 0,
                tx_ready: 0,
                mode: UartMode::new(DataBits::Eight, Parity::None, StopBits::One),
                baud_rate: 9600,
                in_proto_mask: InProtoMask::all(),
                out_proto_mask: OutProtoMask::UBLOX,
                flags: 0,
                reserved5: 0,
            }
            .into_packet_bytes(),
        )?;
    device.wait_for_ack::<CfgPrtUart>(Duration::from_millis(3000)).unwrap();
    println!("CfgPrtUart acked!");

    // Set interval for the NavPosVelTime packet
    println!("Sending set_rate_for::<NavPosVelTime> ...");
    for i in 1..5 {
        device
            .write_all(
                &CfgMsgAllPortsBuilder::set_rate_for::<NavPosVelTime>([0, 1, 1, 0, 0, 0])
                .into_packet_bytes(),
            )
            .unwrap();
        println!("SENT set_rate_for::<NavPosVelTime>({}) !!!", i);
        if let Ok(true) = device.wait_for_ack::<CfgMsgSinglePort>(Duration::from_millis(3000)) {
            println!("Setting rate for NavPosVelTime acked! Exiting loop ...");
            break
        }
    }

    // Send a packet request for the MonVer packet
    //device
    //    .write_all(&UbxPacketRequest::request_for::<MonVer>().into_packet_bytes())
    //    .unwrap();

    // Start reading data
    println!("Opened u-blox device, waiting for solutions...");
    for _ in 0..20 {
        device
            .update(|packet| match packet {
                PacketRef::MonVer(packet) => {
                    println!(
                        "SW version: {} HW version: {}",
                        packet.software_version(),
                        packet.hardware_version()
                    );
                    println!("{:?}", packet);
                }
                PacketRef::NavPosVelTime(sol) => {
                    let has_posvel = sol.fix_type() == GpsFix::Fix3D
                        || sol.fix_type() == GpsFix::GPSPlusDeadReckoning;

                    if has_posvel {
                        let pos: Position = (&sol).into();
                        let vel: Velocity = (&sol).into();
                        let solution = Solution {
                            latitude: pos.lat,
                            longitude: pos.lon,
                            altitude: pos.alt,
                            speed: vel.speed,
                            direction: vel.heading,
                        };
                        println!("Sol: {:?}", solution);
                        sender.send(Msg::Gps(solution));
                    }
                }
                _ => {
                    println!("{:?}", packet);
                }
            })?;
        thread::sleep(Duration::from_millis(1000));
    }

    println!("exiting GPS sender loop :)");
    Ok(())
}