#![no_std]
#![no_main]

use defmt::info;
use embassy_executor::Spawner;
use embassy_time::{Timer, Duration};
use embassy_futures::{select::select, select::Either};
use embassy_stm32::gpio::{Level, Output, Speed};
use embassy_stm32::{i2c, spi, Config};
use embassy_stm32::time::Hertz;

use crate::ad5940_registers::{AFECON, AFEGENINTSTA};

use {defmt_rtt as _, panic_probe as _};

// mod ad5940;
// use ad5940::AD5940;

// mod adg2128;
// use adg2128::State;

// mod electrodes;
// use electrodes::{Electrodes, Electrode, AD5940Pin};

mod ad5940_registers;

use embassy_stm32::usb::Driver;
use embassy_stm32::{bind_interrupts, peripherals, usb};

mod communication;
use communication::{init_communication, LED_FREQUENCY_SIGNAL};

bind_interrupts!(struct Irqs {
    USB_DRD_FS => usb::InterruptHandler<peripherals::USB>;
});

#[embassy_executor::main]
async fn main(spawner: Spawner) {

    let mut config = Config::default();
    {
        use embassy_stm32::rcc::*;
        config.rcc.hsi = None;
        config.rcc.hsi48 = Some(Hsi48Config { sync_from_usb: true }); // needed for USB
        config.rcc.hse = Some(Hse {
            freq: Hertz(24_000_000),
            mode: HseMode::Oscillator,
        });
        config.rcc.pll1 = Some(Pll {
            source: PllSource::HSE,
            prediv: PllPreDiv::DIV6,
            mul: PllMul::MUL125,
            divp: Some(PllDiv::DIV2),
            divq: Some(PllDiv::DIV2),
            divr: Some(PllDiv::DIV2),
        });
        config.rcc.sys = Sysclk::PLL1_P;
        config.rcc.mux.usbsel = mux::Usbsel::HSI48;
    }    

    let p = embassy_stm32::init(config);
    info!("Hello World!");

    let mut led = Output::new(p.PA5, Level::High, Speed::Low);

    // // Set up SPI for AD5940
    // let cs = Output::new(p.PC9, Level::High, Speed::Low);
    // let rst = Output::new(p.PB3, Level::High, Speed::Low);

    // let spi = spi::Spi::new_blocking(
    //     p.SPI1,
    //     p.PA5, // SCK
    //     p.PA7, // MOSI
    //     p.PA6, // MISO
    //     spi::Config::default()
    // );

    // let mut ad5940 = AD5940::new(spi, cs, rst);
    // ad5940.reset().await.unwrap();
    // Timer::after_millis(1).await;
    // ad5940.system_init().await.unwrap();

    // ad5940.init_temperature().await.unwrap();
    // ad5940.init_waveform().await.unwrap();

    // // Set up I2C for ADG2128
    // let i2c = i2c::I2c::new_blocking(
    //     p.I2C1, 
    //     p.PB6, 
    //     p.PB7, 
    //     Hertz(400_000), 
    //     i2c::Config::default()
    // );

    // // Initialize electrodes
    // let mut electrodes = Electrodes::new(i2c);
    // electrodes.reset_all();
    // electrodes.set(Electrode::E1, AD5940Pin::CE0, State::ENABLED);
    // electrodes.set(Electrode::E3, AD5940Pin::AIN1, State::ENABLED);
    // // electrodes.set(Electrode::E12, AD5940Pin::RE0, State::ENABLED);

    // // Sequencer test
    // ad5940.sequencer_enable(true).await;
    // ad5940.afecon(AFECON::WAVEGENEN, true).await;
    // ad5940.wgfcw(1000).await;
    // ad5940.sequencer_wait(600_000).await;
    // ad5940.wgfcw(2000).await;
    // ad5940.sequencer_trigger_interrupt(AFEGENINTSTA::CUSTOMINT0).await;
    // ad5940.sequencer_trigger_interrupt(AFEGENINTSTA::CUSTOMINT1).await;
    // ad5940.sequencer_wait(160_000).await;
    // ad5940.afecon(AFECON::WAVEGENEN, false).await;
    // ad5940.sequencer_enable(false).await;

    // // Configure the sequencer cmd data sram
    // ad5940.cmddatacon().await;

    // let start_address = 100; // 20 works
    // ad5940.sequencer_cmd_write(start_address).await;

    // ad5940.sequencer_info_configure(0, ad5940.seq_len, start_address).await;
    // info!("{}", ad5940.seq_len);

    // Create USB driver and start postcard-rpc server
    let driver = Driver::new(p.USB, Irqs, p.PA12, p.PA11);
    init_communication(driver, spawner);

    // Green led task
    spawner.must_spawn(green_led(led));

    loop {
        // Read chip id
        // let chip_id = ad5940.get_chipid().await.unwrap();
        // info!("Chip ID: 0x{:04X}", chip_id);
        
        // Read temperature
        // let temp = ad5940.get_temperature().await.unwrap();
        // info!("Temperature: {}°C", temp);

        // let result = electrodes.get_all();
        // info!("Electrodes states: {:?}", result);        

        // info!("high");

        



        info!("Mainloop still running!");

        Timer::after_millis(5000).await;

        // ad5940.sequencer_trigger().await;        
    }
}

#[embassy_executor::task]
async fn green_led(mut led: Output<'static>) {
    let mut delay = Duration::from_millis(500);

    loop {
        // Wait for either a frequency change or a timer timeout
        match select(LED_FREQUENCY_SIGNAL.wait(), Timer::after(delay)).await {
            Either::First(frequency) => {
                if frequency > 0.0 {
                    // Avoid divide-by-zero or negative delay
                    let millis = (1000.0 / frequency) as u64 / 2;
                    delay = Duration::from_millis(millis.max(1)); // enforce minimum delay of 1ms
                }
            }
            Either::Second(_) => {
                // Timer expired, proceed to toggle
            }
        }

        led.toggle();
    }
}