use std::{f32::NAN, io::{BufRead, BufReader, BufWriter, Read, Write}, net::TcpListener, sync::{Arc, Mutex}, thread::sleep, time::Duration};

use common::{Command, Telemetry};
use postcard::to_vec_cobs;
use heapless::Vec;

fn main() {
    let mut port = serialport::new("/dev/ttyACM1", 115200)
        .timeout(Duration::from_secs(1000))
        .open().expect("port missing");
    println!("connected over serial");

    let command: Arc<Mutex<Command>> = Arc::new(Mutex::new(Command { left: 0.0, right: 0.0, ultrasonic_enable: false}));
    let telemetry: Arc<Mutex<Telemetry>> = Arc::new(Mutex::new(Telemetry { distance: f32::NAN }));

    {
        let command = command.clone();
        let telemetry = telemetry.clone();
        std::thread::spawn(move || server(command,telemetry));
    }


        let reader = port.try_clone().unwrap();
        let mut reader = BufReader::new(reader);
    loop {
        let command = command.lock().unwrap().clone();
        let encoded: Vec<u8, 20> = to_vec_cobs(&command).unwrap();
        port.write(&encoded).expect("port write fail");
        println!("sending");

        let mut recv_buf = std::vec::Vec::new();
        let Ok(len) = reader.read_until(0, &mut recv_buf) else {continue;};
        if let Ok(telem) = postcard::from_bytes_cobs::<Telemetry>(&mut recv_buf[0..len]) {
            println!("dist: {}", telem.distance);
            *telemetry.lock().unwrap() = telem;
        }
        sleep(Duration::from_millis(200));
    }
}

fn recv_telem<T: Read>(telemetry: Arc<Mutex<Telemetry>>, mut reader: BufReader<T>) {
    let mut recv_buf = std::vec::Vec::new();
    loop {
        let Ok(len) = reader.read_until(0, &mut recv_buf) else {continue;};
        if let Ok(telem) = postcard::from_bytes_cobs::<Telemetry>(&mut recv_buf[0..len]) {
            println!("dist: {}", telem.distance);
            *telemetry.lock().unwrap() = telem;
        }
    }
}

fn server(command: Arc<Mutex<Command>>, telemetry: Arc<Mutex<Telemetry>>) {
    let listener = TcpListener::bind("0.0.0.0:4242").unwrap();

    while let Ok((stream, _)) = listener.accept() {
        println!("connected over IP");
        let command1 = command.clone();
        let telemetry1 = telemetry.clone();
        std::thread::spawn(move || {
            if let Err(e) = handle_connection(command1, telemetry1, stream) {
                println!("IP connection dropped: {}", e);
            }
        });
    }
}

fn handle_connection(command: Arc<Mutex<Command>>, telemetry: Arc<Mutex<Telemetry>>, stream: std::net::TcpStream) -> anyhow::Result<()> {
    let writer = stream.try_clone().unwrap();
    let mut writer = BufWriter::new(writer);
    let mut reader = BufReader::new(stream);

    loop {
        let mut data_buf = std::vec::Vec::new();
        let len = reader.read_until(b'\n', &mut data_buf)?;

        let Ok(new_command) = serde_json::from_slice::<Command>(&data_buf[0..len]) else {continue;};

        *command.lock().unwrap() = new_command.clone();
        println!("received l{} r{}", new_command.left, new_command.right);

        let mut telem = telemetry.lock().unwrap().clone();

        if telem.distance.is_nan() {
            telem.distance = -1.0;
        }
        println!("sent d{}", telem.distance);

        serde_json::to_writer(&mut writer, &telem).unwrap();
        writer.write(&[b'\r',b'\n'])?;
        writer.flush()?;
    }
}