#![no_main]
#![no_std]

mod lsm6ds3tr;

use core::fmt::Write;
use defmt::{info, panic, unwrap};
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
use embassy_executor::Spawner;
use embassy_nrf::peripherals::TWISPI0;
use embassy_nrf::twim::Twim;
use embassy_nrf::usb::vbus_detect::HardwareVbusDetect;
use embassy_nrf::usb::Driver;
use embassy_nrf::{bind_interrupts, pac, peripherals, twim, usb};
use embassy_sync::blocking_mutex::raw::NoopRawMutex;
use embassy_sync::mutex::Mutex;
use embassy_time::{Duration, Timer};
use embassy_usb::class::cdc_acm::{CdcAcmClass, State};
use embassy_usb::driver::EndpointError;
use embassy_usb::{Builder, Config, UsbDevice};
use embedded_alloc::LlffHeap;

use lsm6ds3tr::interface::i2c::I2cInterface;
// use embassy_embedded_hal::shared_bus::blocking::i2c::I2cDevice;
use static_cell::StaticCell;

use heapless::String;

use {defmt_serial as _, panic_probe as _};

static I2C_BUS: StaticCell<Mutex<NoopRawMutex, Twim<TWISPI0>>> = StaticCell::new();

bind_interrupts!(struct Irqs {
    USBD => usb::InterruptHandler<peripherals::USBD>;
    CLOCK_POWER => usb::vbus_detect::InterruptHandler;
});

type MyDriver = Driver<'static, peripherals::USBD, HardwareVbusDetect>;

#[global_allocator]
static HEAP: LlffHeap = LlffHeap::empty();

#[embassy_executor::task]
async fn usb_task(mut device: UsbDevice<'static, MyDriver>) {
    device.run().await;
}

#[embassy_executor::task]
async fn echo_task(mut class: CdcAcmClass<'static, MyDriver>) {
    loop {
        class.wait_connection().await;
        info!("Connected");
        let _ = echo(&mut class).await;
        info!("Disconnected");
    }
}

#[embassy_executor::task]
// async fn write_task(
//     mut class: CdcAcmClass<'static, MyDriver>,
//     mut imu: lsm6ds3tr::LSM6DS3TR<
//         I2cInterface<I2cDevice<'static, NoopRawMutex, Twim<'static, TWISPI0>>>,
//     >,
// ) {
//     loop {
//         class.wait_connection().await;
//         info!("Connected");

//         // Read accelerometer data from the IMU
//         let accel_data = imu.read_accel().await.unwrap();

//         let mut data = String::<32>::new();
//         if write!(
//             data,
//             "Accel: x={}, y={}, z={}\r\n",
//             accel_data.x, accel_data.y, accel_data.z
//         )
//         .is_ok()
//         {
//             if let Err(e) = class.write_packet(data.as_bytes()).await {
//                 info!("Failed to write to serial console: {:?}", e);
//             }
//         }

//         // // Format the accelerometer data
//         // let message = format!(
//         //     "Accel: x={}, y={}, z={}\r\n",
//         //     accel_data.x, accel_data.y, accel_data.z
//         // );
//         // if let Err(e) = class.write_packet(message.as_bytes()).await {
//         //     info!("Failed to write to serial console: {:?}", e);
//         // }

//         // Add a delay of 1 second
//         Timer::after(Duration::from_secs(1)).await;

//         info!("Disconnected");
//     }
// }
async fn write_task(mut class: CdcAcmClass<'static, MyDriver>) {
    let mut count = 1;
    loop {
        class.wait_connection().await;
        info!("Connected");

        let mut data = String::<32>::new();
        if write!(data, "Count: {}\r\n", count).is_ok() {
            if let Err(e) = class.write_packet(data.as_bytes()).await {
                info!("Failed to write to serial console: {:?}", e);
            }
        }
        count += 1;

        // Add a delay of 1 second
        Timer::after(Duration::from_secs(1)).await;

        info!("Disconnected");
    }
}

#[embassy_executor::main]
async fn main(spawner: Spawner) {
    let p = embassy_nrf::init(Default::default());

    info!("Enabling ext hfosc...");
    pac::CLOCK.tasks_hfclkstart().write_value(1);
    while pac::CLOCK.events_hfclkstarted().read() != 1 {}

    // Create the driver, from the HAL.
    let driver = Driver::new(p.USBD, Irqs, HardwareVbusDetect::new(Irqs));

    // Create embassy-usb Config
    let mut config = Config::new(0xc0de, 0xcafe);
    config.manufacturer = Some("Embassy");
    config.product = Some("USB-serial example");
    config.serial_number = Some("12345678");
    config.max_power = 100;
    config.max_packet_size_0 = 64;

    static STATE: StaticCell<State> = StaticCell::new();
    let state = STATE.init(State::new());

    // Create embassy-usb DeviceBuilder using the driver and config.
    static CONFIG_DESC: StaticCell<[u8; 256]> = StaticCell::new();
    static BOS_DESC: StaticCell<[u8; 256]> = StaticCell::new();
    static MSOS_DESC: StaticCell<[u8; 128]> = StaticCell::new();
    static CONTROL_BUF: StaticCell<[u8; 128]> = StaticCell::new();
    let mut builder = Builder::new(
        driver,
        config,
        &mut CONFIG_DESC.init([0; 256])[..],
        &mut BOS_DESC.init([0; 256])[..],
        &mut MSOS_DESC.init([0; 128])[..],
        &mut CONTROL_BUF.init([0; 128])[..],
    );

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

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

    unwrap!(spawner.spawn(usb_task(usb)));

    let i2c_bus = {
        use embassy_nrf::{
            bind_interrupts,
            peripherals::{self},
        };
        // bind_interrupts!(struct Irqs {
        //     SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0 => twim::InterruptHandler<peripherals::TWISPI0>;
        // });
        bind_interrupts!(struct Irqs2 {
            TWISPI0 => twim::InterruptHandler<peripherals::TWISPI0>;
        });
        let config = twim::Config::default();
        let i2c = Twim::new(p.TWISPI0, Irqs2, p.P0_07, p.P0_27, config);
        let i2c_bus = Mutex::<NoopRawMutex, _>::new(i2c);
        I2C_BUS.init(i2c_bus)
    };

    let i2c = I2cDevice::new(i2c_bus);

    let interface = I2cInterface::new(i2c);
    let mut imu = lsm6ds3tr::LSM6DS3TR::new(interface);

    // imu.init().await.unwrap();

    // imu.read_accel().await.unwrap();

    unwrap!(spawner.spawn(write_task(class)));
}

struct Disconnected {}

impl From<EndpointError> for Disconnected {
    fn from(val: EndpointError) -> Self {
        match val {
            EndpointError::BufferOverflow => panic!("Buffer overflow"),
            EndpointError::Disabled => Disconnected {},
        }
    }
}

async fn echo(class: &mut CdcAcmClass<'static, MyDriver>) -> Result<(), Disconnected> {
    let mut buf = [0; 64];
    loop {
        let n = class.read_packet(&mut buf).await?;
        let data = &buf[..n];
        info!("data: {:x}", data);
        class.write_packet(data).await?;
    }
}