use defmt::Format;

use embassy_executor::Spawner;
use embassy_stm32::gpio::{Level, Output, Speed};
use embassy_stm32::peripherals;
use embassy_stm32::time::Hertz;
use embassy_stm32::usb::Driver;
use embassy_stm32::{interrupt, Config};
use embassy_time::{Duration, Timer};
use embassy_usb::Builder;
use embassy_usb_serial::{CdcAcmClass, State};
use futures::future::join;

use core::fmt::Write;

use heapless::String;

#[embassy_executor::task]
pub async fn usb_task(usb: peripherals::USB, dp_pin: peripherals::PA12, dm_pin: peripherals::PA11) {
    let irq = interrupt::take!(USB_LP_CAN1_RX0);
    let driver = Driver::new(usb, irq, dp_pin, dm_pin);

    // Create embassy-usb Config
    let config = embassy_usb::Config::new(0xc0de, 0xcafe);
    //config.max_packet_size_0 = 64;

    // Create embassy-usb DeviceBuilder using the driver and config.
    // It needs some buffers for building the descriptors.
    let mut device_descriptor = [0; 256];
    let mut config_descriptor = [0; 256];
    let mut bos_descriptor = [0; 256];
    let mut control_buf = [0; 7];

    let mut state = State::new();

    let mut builder = Builder::new(
        driver,
        config,
        &mut device_descriptor,
        &mut config_descriptor,
        &mut bos_descriptor,
        &mut control_buf,
        None,
    );

    // Create classes on the builder.
    let mut class = CdcAcmClass::new(&mut builder, &mut state, 64);

    // Build the builder.
    let mut usb = builder.build();

    let az_actual: u16 = 0;
    let el_actual: u16 = 0;

    // Do stuff with the class!
    let usb_handler_fut = async {
        loop {
            class.wait_connection().await;
            defmt::info!("USB connected");
            let mut packet = [0; 64];
            let mut buffer: String<64> = String::new();

            loop {
                let n = match class.read_packet(&mut packet).await {
                    Ok(n) => n,
                    Err(err) => {
                        defmt::error!("Unable to read packet: {}", err);
                        break;
                    }
                };

                for byte in &packet[..n] {
                    if buffer.len() == 64 {
                        buffer.clear();
                    }
                    buffer.push(*byte as char).unwrap();
                }

                let line_end = match buffer.rfind('\r') {
                    Some(n) => n,
                    _ => continue,
                };

                defmt::info!("Line buffer: {:x}", buffer.as_bytes());

                if line_end > 0 {
                    let cmd = parse_command(&buffer.as_str()[..line_end]);
                    defmt::info!("Command: {}", cmd);

                    let mut resp: String<16> = String::new();
                    match cmd {
                        Gs232Cmd::GetAl => {
                            write!(&mut resp, "AZ={}\r", az_actual).unwrap();
                        }
                        Gs232Cmd::GetEz => {
                            write!(&mut resp, "EL={}\r", el_actual).unwrap();
                        }
                        Gs232Cmd::GetAlEz => {
                            write!(&mut resp, "AZ={} EL={}\r", az_actual, el_actual).unwrap();
                        }
                        Gs232Cmd::MoveTo(_az, _el) => {
                            resp.push_str("\r").unwrap();
                        }
                        Gs232Cmd::Stop => {
                            resp.push_str("\r").unwrap();
                        }
                        _ => {
                            defmt::error!("Uknown command: {}", &buffer.as_str()[..line_end]);
                            resp.push_str("Unkown command!\r").unwrap();
                        }
                    }

                    match class.write_packet(resp.as_bytes()).await {
                        Ok(_) => {}
                        Err(err) => {
                            defmt::error!("Unable to write packet: {}", err);
                            break;
                        }
                    };
                }
                buffer = String::from(&buffer.as_str()[line_end + 1..]);
            }
            defmt::info!("USB disconnected");
        }
    };

    join(usb.run(), usb_handler_fut).await;
}

#[derive(Format, PartialEq)]
enum Gs232Cmd {
    Unkown,
    GetAl,
    GetEz,
    GetAlEz,
    MoveTo(u16, u16),
    Stop,
}

fn parse_command(data: &str) -> Gs232Cmd {
    match data.chars().nth(0).unwrap() {
        'B' => {
            if data.len() == 1 {
                Gs232Cmd::GetAl
            } else {
                Gs232Cmd::Unkown
            }
        }

        'C' => {
            if data.len() == 2 && data.chars().nth(1).unwrap() as char == '2' {
                Gs232Cmd::GetAlEz
            } else if data.len() == 1 {
                Gs232Cmd::GetEz
            } else {
                Gs232Cmd::Unkown
            }
        }

        'W' => {
            if data.len() == 8 {
                if let Ok(az) = data[1..4].parse::<u16>() {
                    if let Ok(el) = data[5..].parse::<u16>() {
                        Gs232Cmd::MoveTo(az, el)
                    } else {
                        Gs232Cmd::Unkown
                    }
                } else {
                    Gs232Cmd::Unkown
                }
            } else {
                Gs232Cmd::Unkown
            }
        }

        'S' => {
            if data.len() == 1 {
                Gs232Cmd::Stop
            } else {
                Gs232Cmd::Unkown
            }
        }

        _ => Gs232Cmd::Unkown,
    }
}