#![no_main]
#![no_std]

mod common;
mod i2c;
mod lsm6ds;

use core::fmt::Write;
use embedded_alloc::LlffHeap;
use libm::sinf;
use lsm6ds::Lsm6ds33;
// use lsm6ds3tr::{
//     interface::{I2cInterface, SpiInterface},
//     LsmSettings, LSM6DS3TR,
// };
// use lsm6ds3tr::{interface::SpiInterface, AccelScale, LsmSettings, LSM6DS3TR};
use microflow::model;
use nalgebra::matrix;
use nrf52840_hal::{self as hal, twim, Spim, Twim};
// use nrf_softdevice::{
//     ble::{EncryptionInfo, IdentityKey, MasterId, Uuid},
//     Softdevice,
// };
use usb_device::class_prelude::UsbBusAllocator;

use nrf52840_hal::{
    gpio::Level,
    prelude::{OutputPin, PinState, _embedded_hal_timer_CountDown},
    uarte::{Baudrate, Parity, Pins, Uarte},
    Rtc,
};

use heapless::String;

// const BATTERY_SERVICE: Uuid = Uuid::new_16(0x180f);
// const BATTERY_LEVEL: Uuid = Uuid::new_16(0x2a19);

// const LED_SERVICE_UUID: Uuid = Uuid::new_16(0x112f);
// const LED_CONTROL_CHAR_UUID: Uuid = Uuid::new_16(0x321f);

const RTC_FREQ_MHZ: f32 = 0.032_768;

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

// #[embassy_executor::task]
// async fn softdevice_task(sd: &'static Softdevice) {
//     sd.run().await;
// }

// #[derive(Debug, Clone, Copy)]
// struct Peer {
//     master_id: MasterId,
//     key: EncryptionInfo,
//     peer_id: IdentityKey,
// }

#[panic_handler] // panicking behavior
fn panic(_: &core::panic::PanicInfo) -> ! {
    reset_into_dfu();
}

#[model("tflite_demo/models/sine.tflite")]
struct Sine;

#[model("tflite_demo/models/speech.tflite")]
struct Speech;

/// Resets the device into Device Firmware Update mode (DFU).
fn reset_into_dfu() -> ! {
    // Via https://devzone.nordicsemi.com/f/nordic-q-a/50839/start-dfu-mode-or-open_bootloader-from-application-by-function-call
    unsafe {
        (*hal::pac::POWER::PTR).gpregret.write(|w| w.bits(0xB1));
    };

    hal::pac::SCB::sys_reset()
}

#[derive(Clone, Copy)]
enum LightState {
    Red = 0,
    Green = 1,
    Blue = 2,
}

#[cortex_m_rt::entry]
fn main() -> ! {
    {
        use core::mem::MaybeUninit;
        const HEAP_SIZE: usize = 1024;
        static mut HEAP_MEM: [MaybeUninit<u8>; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE];
        unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) }
    }

    let p = hal::pac::Peripherals::take().unwrap();
    let port0 = hal::gpio::p0::Parts::new(p.P0);
    let mut led_red = port0.p0_26.into_push_pull_output(Level::Low);
    let mut led_green = port0.p0_30.into_push_pull_output(Level::Low);
    let mut led_blue = port0.p0_06.into_push_pull_output(Level::Low);

    let clocks = hal::clocks::Clocks::new(p.CLOCK);
    let clocks = clocks.enable_ext_hfosc();
    let usb_peripheral = hal::usbd::UsbPeripheral::new(p.USBD, &clocks);
    let usb_bus = UsbBusAllocator::new(hal::usbd::Usbd::new(usb_peripheral));
    let mut serial_port = usbd_serial::SerialPort::new(&usb_bus);

    // let spiclk = port0.p0_24.into_push_pull_output(Level::Low).degrade();
    // let spimosi = port0.p0_23.into_push_pull_output(Level::Low).degrade();
    // let spimiso = port0.p0_22.into_floating_input().degrade();

    // let pins = nrf52840_hal::spim::Pins {
    //     sck: Some(spiclk),
    //     miso: Some(spimiso),
    //     mosi: Some(spimosi),
    // };

    // let spim = Spim::new(
    //     p.SPIM2,
    //     pins,
    //     nrf52840_hal::spim::Frequency::K125,
    //     nrf52840_hal::spim::MODE_0,
    //     0,
    // );

    // let spi_interface = SpiInterface::new(spim);

    // let settings = LsmSettings::basic();
    // let mut imu = LSM6DS3TR::new(spi_interface);
    // imu.init().expect("Couldn't initialize the LSM6 sensor!");

    // let scl = port0.p0_27.into_floating_input().degrade();
    // let sda = port0.p0_07.into_floating_input().degrade();

    // let pins = twim::Pins { scl: scl, sda: sda };

    // let i2c = Twim::new(p.TWIM0, pins, twim::Frequency::K100);

    // let i2c_interface = I2cInterface::new(i2c);

    // let mut imu = LSM6DS3TR::new(I2cInterface::new(""));

    // let mut imu = Lsm6ds33::new(i2c, addr);

    let rtc = Rtc::new(p.RTC0, 0).unwrap();
    rtc.enable_counter();

    let mut usb_device = usb_device::device::UsbDeviceBuilder::new(
        &usb_bus,
        usb_device::device::UsbVidPid(0x16c0, 0x27dd),
    )
    .manufacturer("HSRW")
    .product("HSRW Pet Tracker nRF52840")
    .serial_number("pet1")
    .device_class(usbd_serial::USB_CLASS_CDC)
    .max_packet_size_0(64) // makes control transfers 8x faster says https://github.com/nrf-rs/nrf-hal/blob/master/examples/usb/src/bin/serial.rs
    .build();

    let x = 0.5;
    let start = rtc.get_counter();
    let y_predicted = Sine::predict(matrix![x])[0];
    let end = rtc.get_counter();
    let y_exact = sinf(x);

    // TIMER0 is reserved by Softdevice
    // https://devzone.nordicsemi.com/f/nordic-q-a/1160/soft-device-and-timers---how-do-they-work-together
    let mut timer = hal::Timer::new(p.TIMER1).into_periodic();
    timer.start(hal::Timer::<hal::pac::TIMER0>::TICKS_PER_SECOND);

    let mut light = LightState::Red;

    loop {
        light = match light {
            LightState::Red => LightState::Green,
            LightState::Green => LightState::Blue,
            LightState::Blue => LightState::Red,
        };
        match light {
            LightState::Red => {
                led_red.set_state(PinState::Low).unwrap();
                led_green.set_state(PinState::High).unwrap();
                led_blue.set_state(PinState::High).unwrap();
            }
            LightState::Green => {
                led_red.set_state(PinState::High).unwrap();
                led_green.set_state(PinState::Low).unwrap();
                led_blue.set_state(PinState::High).unwrap();
            }
            LightState::Blue => {
                led_red.set_state(PinState::High).unwrap();
                led_green.set_state(PinState::High).unwrap();
                led_blue.set_state(PinState::Low).unwrap();
                //reset_into_dfu();
            }
        }

        // if !usb_dev.poll(&mut [&mut serial]) {
        //     continue;
        // }

        let mut buf = [0u8; 64];

        match serial_port.read(&mut buf) {
            Ok(count) if count > 0 => {
                // Check if received data equals to 'reset'
                for i in 0..count - 4 {
                    if buf[i] == b'r'
                        && buf[i + 1] == b'e'
                        && buf[i + 2] == b's'
                        && buf[i + 3] == b'e'
                        && buf[i + 4] == b't'
                    {
                        reset_into_dfu();
                    }
                }

                if buf[..12].iter().all(|&x| x == 0) {
                    reset_into_dfu();
                }

                // Echo back in upper case
                for c in buf[0..count].iter_mut() {
                    if 0x61 <= *c && *c <= 0x7a {
                        *c &= !0x20;
                    }
                }

                let mut write_offset = 0;
                while write_offset < count {
                    match serial_port.write(&buf[write_offset..count]) {
                        Ok(len) if len > 0 => {
                            write_offset += len;
                        }
                        _ => {}
                    }
                }
            }
            _ => {}
        }

        let text = y_predicted.to_bits();
        let mut data = String::<32>::new();

        let text2 = y_exact.to_bits();
        let mut data2 = String::<32>::new();

        let _ = serial_port.write("Hello World\n".as_bytes());

        let _ = serial_port.write("Predicted Y: ".as_bytes());
        let _ = write!(data, "data:{text}");
        let _ = serial_port.write(data.as_bytes());
        let _ = serial_port.write("\n".as_bytes());

        let _ = serial_port.write("Exact Y: ".as_bytes());
        let _ = write!(data2, "data:{text2}");
        let _ = serial_port.write(data2.as_bytes());
        let _ = serial_port.write("\n".as_bytes());

        while timer.wait().is_err() {
            // TODO: sleep.
            // Spec says poll needs to be called at least every 10ms.
            usb_device.poll(&mut [&mut serial_port]);
            continue;
        }
    }
}