use defmt::*;
use heapless::Vec;
use static_cell::StaticCell;

use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel};

use embassy_rp::{peripherals::USB, usb::Driver as UsbDriver};
use embassy_usb::class::cdc_acm::{CdcAcmClass, State as UsbState};
use usb_communication_shared::request::Packet as RequestPacket;
use usb_communication_shared::response::Packet as ResponsePacket;
static USB_SEND_CHANNEL: Channel<CriticalSectionRawMutex, ResponsePacket, 4> = Channel::new();
static USB_RECIEVE_CHANNEL: Channel<CriticalSectionRawMutex, RequestPacket, 4> = Channel::new();

#[embassy_executor::task]
pub async fn handle_usb(usb_driver: UsbDriver<'static, USB>) {
    let mut config = embassy_usb::Config::new(0xc0de, 0xcafe);
    config.manufacturer = Some("Melfely");
    config.product = Some("USB-Parrot");
    config.serial_number = Some("1234");
    config.max_power = 100;
    config.max_packet_size_0 = 64;

    let mut usb_builder = {
        static CONFIG_DESCRIPTOR: StaticCell<[u8; 256]> = StaticCell::new();
        static BOS_DESCRIPTOR: StaticCell<[u8; 256]> = StaticCell::new();
        static CONTROL_BUF: StaticCell<[u8; 64]> = StaticCell::new();

        let embassy = embassy_usb::Builder::new(
            usb_driver,
            config,
            CONFIG_DESCRIPTOR.init([0; 256]),
            BOS_DESCRIPTOR.init([0; 256]),
            &mut [],
            CONTROL_BUF.init([0; 64]),
        );
        embassy
    };

    let usb_class = {
        static STATE: StaticCell<UsbState> = StaticCell::new();
        let state = STATE.init(UsbState::new());
        CdcAcmClass::new(&mut usb_builder, state, 64)
    };

    let (mut usb_tx, mut usb_rx, _usb_control) = usb_class.split_with_control();

    let mut usb = usb_builder.build();

    let usb_fut = usb.run();

    let channel = Channel::<CriticalSectionRawMutex, Vec<u8, 64>, 4>::new();

    let mut usb_buf = [0; 64];
    let usb_rx_future = async {
        let sender = channel.sender();
        loop {
            usb_rx.wait_connection().await;
            info!("USB RX Connected");
            loop {
                match usb_rx.read_packet(&mut usb_buf).await {
                    Err(e) => {
                        error!(
                            "Usb read error: {:?}. Assume Discconection",
                            Debug2Format(&e)
                        );
                        break;
                    }
                    Ok(_) => {
                        let vec: Vec<u8, 64> = usb_buf.into();
                        sender.send(vec).await;
                    }
                }
            }
        }
    };

    let usb_tx_future = async {
        let reciever = channel.receiver();
        loop {
            usb_tx.wait_connection().await;
            info!("USB TX Connected");
            loop {
                let packet = reciever.receive().await;
                match usb_tx.write_packet(&packet).await {
                    Ok(_) => {}
                    Err(err) => {
                        error!(
                            "Usb write error: {:?}, Assume Disconnection",
                            Debug2Format(&err)
                        );
                        break;
                    }
                }
            }
        }
    };

    embassy_futures::join::join3(usb_fut, usb_rx_future, usb_tx_future).await;
}