#![no_main]
#![no_std]

use assign_resources::assign_resources;
use core::fmt::Write;
use defmt::{dbg, info, unwrap};
use embassy_executor::Spawner;
use embassy_nrf::gpio::{Level, Output, OutputDrive};
use embassy_nrf::usb::vbus_detect::HardwareVbusDetect;
use embassy_nrf::usb::Driver;
use embassy_nrf::{bind_interrupts, peripherals, twim, usb, Peripherals};
use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
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 heapless::String;
use lsm6ds3tr::interface::I2cInterface;
use lsm6ds3tr::{
    registers::{GyroSampleRate, GyroScale},
    AccelSampleRate, AccelScale, AccelSettings, GyroSettings, LsmSettings, LSM6DS3TR,
};
use static_cell::StaticCell;
use {defmt_rtt as _, panic_probe as _};

mod usb_dfu;

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

assign_resources! {
    imu: ImuResources {
        spi: SPI2,
    }
}

bind_interrupts!(struct IrqsTest {
    // SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0 => twim::InterruptHandler<TWISPI0>;
    TWISPI0 => twim::InterruptHandler<peripherals::TWISPI0>;
});

static I2C_BUS: StaticCell<Mutex<ThreadModeRawMutex, twim::Twim<peripherals::TWISPI0>>> =
    StaticCell::new();

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

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

    // setup_dfu_over_usb(&spawner, p.USBD);

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

    let mut config = Config::new(0xc0de, 0xcafe);
    config.manufacturer = Some("Umbrella Corporation");
    config.product = Some("Secret Project");
    config.serial_number = Some("11880");
    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 mut class = CdcAcmClass::new(&mut builder, state, 64);

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

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

    let mut led_red = Output::new(p.P0_06, Level::Low, OutputDrive::Standard);

    let mut pin = Output::new(p.P1_08, Level::High, OutputDrive::HighDrive);
    pin.set_high();

    Timer::after_millis(100).await;

    let sda_pin = p.P0_07;
    let scl_pin = p.P0_27;

    let mut imuConfig = twim::Config::default();
    // This limits the ADXL355 ODR to 200Hz max.
    imuConfig.frequency = twim::Frequency::K400;
    // Internal pullups for SCL and SDA must be enabled.
    imuConfig.scl_pullup = true;
    imuConfig.sda_pullup = true;

    let i2c = twim::Twim::new(p.TWISPI0, IrqsTest, sda_pin, scl_pin, imuConfig);

    let settings = LsmSettings::basic()
        .with_gyro(
            GyroSettings::new()
                .with_sample_rate(GyroSampleRate::_104Hz)
                .with_scale(GyroScale::_2000DPS),
        )
        .with_accel(
            AccelSettings::new()
                .with_sample_rate(AccelSampleRate::_104Hz)
                .with_scale(AccelScale::_4G),
        );

    let mut imu = LSM6DS3TR::new(I2cInterface::new(i2c)).with_settings(settings);
    imu.init().expect("LSM6DS3TR-C initialization failure!");

    let blink_fut = async {
        loop {
            Timer::after_millis(500).await;

            if let (Ok(xyz_a), Ok(xyz_g)) = (imu.read_accel_raw(), imu.read_gyro()) {
                class.wait_connection().await;
                info!("Connected");

                let temp = imu.read_temp().expect("Error reading temperature data.");
                // imu.read_temp()

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

                let mut temp_data = String::<32>::new();
                if write!(temp_data, "Degrees C1 = {}\r\n", temp).is_ok() {
                    if let Err(e) = class.write_packet(temp_data.as_bytes()).await {
                        info!("Failed to write to serial console: {:?}", e);
                    }
                }
            } else {
                class.wait_connection().await;
                info!("Connected");

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

    blink_fut.await;
}

#[embassy_executor::task]
async fn led_blinking_task(resources: ImuResources) {
    loop {}
}

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

#[embassy_executor::task]
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::task]
async fn gyro_task(mut class: CdcAcmClass<'static, MyDriver>, p: &'static Peripherals) {
    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::task]
async fn print_task(mut class: CdcAcmClass<'static, MyDriver>, text: &'static str) {
    class.wait_connection().await;
    info!("Connected");

    let mut data = String::<32>::new();
    if write!(data, "Count: {}\r\n", text).is_ok() {
        if let Err(e) = class.write_packet(data.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");
}

#[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 :(");
    }
}

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