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

use defmt::*;
use embassy_executor::Executor;
use embassy_rp::adc::{Adc, Config, InterruptHandler, Pin};
use embassy_rp::bind_interrupts;
use embassy_rp::gpio::Pull;
use embassy_rp::gpio::{Level, Output};
use embassy_rp::multicore::{spawn_core1, Stack};
use embassy_rp::peripherals::PIN_25;
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_sync::channel::Channel;
use static_cell::StaticCell;
use {defmt_rtt as _, panic_probe as _};

static mut CORE1_STACK: Stack<4096> = Stack::new();
static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
static CHANNEL: Channel<CriticalSectionRawMutex, Buffer, 1> = Channel::new();
bind_interrupts!(struct Irqs {
    ADC_IRQ_FIFO => InterruptHandler;
});

const BUF_SIZE: usize = 64;

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

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

#[embassy_executor::task]
async fn core0_task() {
    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);

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

    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 be added to the queue
    loop {
        if count >= BUF_SIZE {
            CHANNEL.send(buf.clone()).await;
            count = 0;
        } else {
            buf.0[count] = adc.read(&mut eeg).await.unwrap();
            count += 1;
        }
    }
}

#[embassy_executor::task]
async fn core1_task() {
    use tinyvec::ArrayVec;
    let mut queue = ArrayVec::<[Buffer; 64]>::new();

    info!("Hello from core 1");
    loop {
        // Shouldn't block on waiting for message?
        // Need to test
        queue.push(CHANNEL.recv().await);

        match process_data(&queue[0]).await {
            Some(s) => {
                info!("Parsed '{}' state from EEG data", &s);
                set_servo_position(s).await;
            }
            None => warn!("Unable to match EEG data to state"),
        }
        queue.remove(0);
    }
}

async fn process_data(buf: &Buffer) -> Option<State> {
    // Low-pass filter
    // FFT (w/ hanning window)
    info!("Running FFT...");
    return None;
}

async fn set_servo_position(state: State) {
    // Use a map of positions for each state, and move the servos towards it
    // Use lerp and randomness
    // I2C control board manages servos
    info!("Setting position to {}", state.servo_positions());
}

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