use as5600::As5600;
use embassy_executor::Spawner;
use embassy_rp::{
    Peri,
    gpio::{AnyPin, Input},
    i2c::{Config, I2c},
    peripherals::{I2C0, PIN_4, PIN_5},
};
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel, mutex::Mutex};
use embassy_time::{Duration, Timer};
use ufmt::uwrite;

use crate::{Irqs, WHEEL_VALUE, screen::SCREEN_BUFFER, unwrap};

/// Button input notification channel
pub static INPUT: Channel<CriticalSectionRawMutex, u8, 64> = Channel::new();

#[embassy_executor::task(pool_size = 4)]
/// Polls a single pin for falling edge (assumes buttons are pulled up and shorted when pressed)
/// Sends [INPUT] with given id
pub async fn button_poll_task(mut button: Input<'static>, id: u8) {
    loop {
        button.wait_for_falling_edge().await;
        INPUT.send(id).await;
        // uwrite!(SCREEN_BUFFER.lock().await.line1(), "btn {}", id);
        Timer::after(Duration::from_millis(50)).await;
    }
}

pub static ANGLE: Mutex<CriticalSectionRawMutex, u16> = Mutex::new(0);

#[embassy_executor::task]
pub async fn rotation_read_task(
    i2c: Peri<'static, I2C0>,
    scl: Peri<'static, PIN_5>,
    sda: Peri<'static, PIN_4>,
) {
    let i2c = I2c::new_async(i2c, scl, sda, Irqs, Config::default());
    let mut as5600 = As5600::new(i2c);

    loop {
        Timer::after(Duration::from_millis(50)).await;
        let angle = as5600.angle().unwrap_or(0);
        let mut locked = ANGLE.lock().await;
        let old = *locked as i32;
        *locked = angle;
        drop(locked);
        let left_diff = old - angle as i32;
        let right_diff = angle as i32 - old;
        let diff = if left_diff.abs() < right_diff.abs() {
            -left_diff
        } else {
            right_diff
        };
        let mut wheel = WHEEL_VALUE.lock().await;
        if wheel.max != wheel.min {
            let wheel_range = wheel.max - wheel.min;
            wheel.value += diff * wheel_range / 4096;
            if wheel.value < wheel.min {
                wheel.value = wheel.min;
            } else if wheel.value > wheel.max {
                wheel.value = wheel.max;
            }
        }
        drop(wheel);
    }
}

pub struct InputConfig {
    pub i2c0: Peri<'static, I2C0>,
    pub scl: Peri<'static, PIN_5>,
    pub sda: Peri<'static, PIN_4>,

    pub button_pins: [(Peri<'static, AnyPin>, u8); 4],
}

pub fn setup(spawner: Spawner, config: InputConfig) {
    spawner.spawn(unwrap!(rotation_read_task(
        config.i2c0,
        config.scl,
        config.sda,
    )));

    for (pin, id) in config.button_pins {
        spawner.spawn(unwrap!(button_poll_task(
            Input::new(pin, embassy_rp::gpio::Pull::Up),
            id,
        )));
    }
}