use std::{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/ttyACM0", 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: true}));
    let telemetry: Arc<Mutex<Telemetry>> = Arc::new(Mutex::new(Telemetry { distance: f32::NAN }));

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

    // Telemetry receive thread
    {
        let reader = port.try_clone().unwrap();
        let reader = BufReader::new(reader);
        let telemetry = telemetry.clone();
        std::thread::spawn(move || recv_telem(telemetry, reader));
    }

    // Command transmit thread (main)
    loop {
        let command = command.lock().unwrap().clone();
        let encoded: Vec<u8, 20> = to_vec_cobs(&command).unwrap();
        if port.write(&encoded).is_err() {
            println!("disconnected from robot");
            return;
        };

        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]) {
            *telemetry.lock().unwrap() = telem;
        }
        recv_buf.clear();
    }
}

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)?;

        // eof
        if len == 0 {
            *command.lock().unwrap() = Command::default(); // stop motors
            println!("disconnected from LabVIEW");
            return anyhow::Ok(());
        }


        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_all(b"\r\n")?;
        writer.flush()?;
    }
}