use anyhow;

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

use esp_idf_hal::prelude::*;
use esp_idf_hal::serial;
use esp_idf_hal::gpio::*;

use ublox::*;

use crate::modem::Msg;
use crate::serial::SerialIO;

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

impl<UART: serial::Uart> Device<UART> {
    pub fn new<T: Sync + Send>(port: serial::Serial<UART, Gpio12<T>, Gpio13<T>>) -> Device<UART> {
        let parser = Parser::default();
        let (tx, rx) = port.split();
        Device { 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 {
                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<()> {
        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;
                    }
                }
            })?;
        }
        Ok(())
    }

    /// 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 mut device = Device::new(serial);

    // 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(),
        )
        .unwrap();
    device.wait_for_ack::<CfgPrtUart>(Duration::from_millis(3000)).unwrap();
    println!("CfgPrtUart acked!");

    // Enable the NavPosVelTime packet
    device
        .write_all(
            &CfgMsgAllPortsBuilder::set_rate_for::<NavPosVelTime>([0, 0, 1, 0, 0, 0])
                .into_packet_bytes(),
        )
        .unwrap();
    device.wait_for_ack::<CfgMsgAllPorts>(Duration::from_millis(3000)).unwrap();
    println!("CfgMsgAllPorts acked!");

    // 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();
                        println!(
                            "Latitude: {:.5} Longitude: {:.5} Altitude: {:.2}m",
                            pos.lat, pos.lon, pos.alt
                        );
                        println!(
                            "Speed: {:.2} m/s Heading: {:.2} degrees",
                            vel.speed, vel.heading
                        );
                        println!("Sol: {:?}", sol);
                    }
                }
                _ => {
                    println!("{:?}", packet);
                }
            })?
    }

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