#![no_std]
#![no_main]
#![feature(type_alias_impl_trait)]

use defmt::*;
use embassy_executor::{Executor, Spawner};
use embassy_rp::adc::{Adc, Config, InterruptHandler, Pin};
use embassy_rp::bind_interrupts;
use embassy_rp::gpio::Pull;
use embassy_rp::i2c::{self, Config as I2Config, InterruptHandler as I2InterruptHandler};
use embassy_rp::multicore::{spawn_core1, Stack};
use embassy_rp::peripherals::I2C1;
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_sync::{channel::Channel, signal::Signal};
use embassy_time::Instant;
use embedded_hal_async::i2c::I2c;
use static_cell::StaticCell;
use {defmt_rtt as _, panic_probe as _};

bind_interrupts!(struct I2Crqs {
    I2C1_IRQ => I2InterruptHandler<I2C1>;
});

static mut CORE1_STACK: Stack<4096> = Stack::new();
static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
static BUF_CHANNEL: Channel<CriticalSectionRawMutex, (Instant, Buffer), 64> = Channel::new();
static SERVO_SIG: Signal<CriticalSectionRawMutex, Option<State>> = Signal::new();
bind_interrupts!(struct Irqs {
    ADC_IRQ_FIFO => InterruptHandler;
});

const BUF_SIZE: usize = 256;

#[derive(Format)]
enum State {
    Happy,
    Sad,
    Relaxed,
    Surprised,
}

// Position in degrees for each servo
#[derive(Format)]
struct ServoPosition(f32, f32, f32, f32);

impl State {
    fn servo_positions(&self) -> ServoPosition {
        // This is where we define the positions and pose for each state
        match self {
            _ => ServoPosition(0.0, 0.0, 0.0, 0.0),
        }
    }
}

#[derive(Clone)]
struct Buffer([u16; BUF_SIZE]);

impl Default for Buffer {
    fn default() -> Self {
        Buffer([0; BUF_SIZE])
    }
}

#[cortex_m_rt::entry]
fn main() -> ! {
    let p = embassy_rp::init(Default::default());

    spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
        let executor1 = EXECUTOR1.init(Executor::new());
        executor1.run(|spawner| unwrap!(spawner.spawn(process_data(spawner))));
    });

    let executor0 = EXECUTOR0.init(Executor::new());
    executor0.run(|spawner| unwrap!(spawner.spawn(read_eeg())));
}

#[embassy_executor::task]
async fn read_eeg() {
    let p = embassy_rp::init(Default::default());
    let mut adc = Adc::new(p.ADC, Irqs, Config::default());
    let mut eeg = Pin::new(p.PIN_26, Pull::None);

    let mut buf = Buffer([0; BUF_SIZE]);
    let mut count = 0;

    info!("Hello from core 0");
    // Sample EEG data, then append it to buffer
    // When full, send buffer to the channel queue
    loop {
        if count >= BUF_SIZE {
            BUF_CHANNEL.send((Instant::now(), buf.clone())).await;
            count = 0;
        } else {
            buf.0[count] = adc.read(&mut eeg).await.unwrap();
            count += 1;
        }
    }
}

// TODO: async task that goes through queue on timer and merges buffers of similar timestamp

#[embassy_executor::task]
async fn process_data(spawner: Spawner) {
    unwrap!(spawner.spawn(servo_control()));

    loop {
        let (time, mut buffer) = BUF_CHANNEL.recv().await;
        let diff = Instant::now().as_secs() - time.as_secs();
        if diff >= 10 {
            // Test value, if buffer is too old throw out
            break;
        }
        // Low-pass filter
        // FFT (w/ hanning window)
        info!("Running FFT...");
        SERVO_SIG.signal(None);
    }
}

#[embassy_executor::task]
async fn servo_control() {
    // TODO: make this work
    let p = embassy_rp::init(Default::default());

    let sda = p.PIN_14;
    let scl = p.PIN_15;

    // What the fuck
    // I hate this
    // Fuck you I2C
    info!("set up i2c");
    let mut i2c = i2c::I2c::new_async(p.I2C1, scl, sda, I2Crqs, I2Config::default());
    // Completely guessing at these values
    let addr: u8 = 0x05;
    let val: u16 = 2047;
    // Will this work? Who knows!!
    i2c.write(addr, &[0, val as u8, (val >> 8) as u8])
        .await
        .unwrap();

    use nanorand::{Rng, WyRand};
    let mut rng = WyRand::new();

    loop {
        let state = SERVO_SIG.wait().await;
        // Use a map of positions for each state, and move the servos towards it
        // Use lerp and randomness
        // I2C control board manages servos
        match state {
            Some(s) => {
                info!("moving servos");
            }
            None => warn!("No state given"),
        }
    }
}

#[inline]
fn lerp(v0: f32, v1: f32, t: f32) -> f32 {
    return (1. - t) * v0 + t * v1;
}