use std::time::Duration;

use anyhow::Context;
use common::{Command, SensorData};
use gilrs::{Axis, Event, Gilrs};
use nalgebra::Vector3;
use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter}, net::{tcp::OwnedReadHalf, TcpStream}, task::JoinHandle, time::{sleep, Instant}};

#[tokio::main]
async fn main() -> anyhow::Result<()> {
    println!("Hello, world!");

    let mut gamepads = Gilrs::new().unwrap();
    let (_, gamepad) = gamepads.gamepads().nth(0).context("no gamepads")?;
    let ly = gamepad.axis_code(Axis::LeftStickY).context("no left joystick")?;
    let rx = gamepad.axis_code(Axis::RightStickX).context("no right joystick")?;
    let lb = gamepad.button_code(gilrs::Button::LeftTrigger).context("no lb")?;
    let a = gamepad.button_code(gilrs::Button::South).context("no a")?;
    let mut last_a = false;

    let server = "130.215.211.79:3322";
    let mut robot = TcpStream::connect(server).await?;
    robot.set_nodelay(true)?;

    let (telem, robot_controller) = robot.into_split();

    tokio::spawn(print_telem(telem));

    let mut robot_controller = BufWriter::new(robot_controller);

    let mut active_gamepad = None;
    let mut last_event = Instant::now();

    loop {
        while let Some(Event { id,..}) = gamepads.next_event() {
            active_gamepad = Some(id);
            last_event = Instant::now();
        }

        let Some(gamepad) = active_gamepad else {
            continue;
        };

        let gamepad = gamepads.gamepad(gamepad);

        let values = gamepad.state();

        let mut ly= values.axis_data(ly).map(|d| d.value()).unwrap_or(0.0);
        let mut rx= values.axis_data(rx).map(|d| d.value()).unwrap_or(0.0);

        let boost = values.button_data(lb).map(|b| b.is_pressed()).unwrap_or(false);

        let a = values.button_data(a).map(|b| b.is_pressed()).unwrap_or(false);

        for axis in [&mut rx, &mut ly] {
            if axis.abs() < 0.05 {
                *axis = 0.0;
            }

            if !boost {
                *axis *= 0.3;
            }
        }

        if !gamepad.is_connected() {
            ly = 0.0;
            rx = 0.0;
        }

        let cmd = Command::Twist(ly, rx);
        println!("twist");

        let encoded = postcard::to_stdvec(&cmd)?;
        robot_controller.write_u32(encoded.len() as u32).await?;
        robot_controller.write_all(&encoded).await?;

        if a != last_a {
            let black = Vector3::repeat(0u8);
            let green = Vector3::new(254u8, 0, 254);
            let cmd = Command::SetLed(if a {green} else {black});
            let encoded = postcard::to_stdvec(&cmd)?;
            robot_controller.write_u32(encoded.len() as u32).await?;
            robot_controller.write_all(&encoded).await?;
            println!("led");
        }
        last_a = a;


        if let Err(e) = robot_controller.flush().await {
            println!("flush fail: {e}, reconnecting");
            robot = TcpStream::connect(server).await?;
            robot.set_nodelay(true)?;

            let (telem, robot) = robot.into_split();

            tokio::spawn(print_telem(telem));

            robot_controller = BufWriter::new(robot);
        };

        sleep(Duration::from_millis(65)).await;
    }
}

async fn print_telem(mut telem: OwnedReadHalf) -> anyhow::Result<()> {
    loop {
        let len = telem.read_u32().await? as usize;
        let mut buf = vec![0; len];
        telem.read_exact(&mut buf).await?;
        let telem: SensorData = postcard::from_bytes(&buf)?;

        match telem {
            SensorData::BusVoltage(voltage) => {
                println!("voltage: {voltage}");
            }
            SensorData::AmbientTemperature(temp) => {
                println!("pico temp {}", temp * 9./5. + 32.);
            }
            _ => {}
        }

    }
}