use core::sync::atomic::{AtomicBool, AtomicU8};

use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::Channel, signal::Signal};
use figments::hardware::Brightness;
use nalgebra::{Vector2, Vector3};

#[derive(Clone, Copy, Default, Debug)]
pub enum Scene {
    #[default]
    Startup, // Default state when booting up
    ParkedIdle, // Default state when waking up from long term (overnight, eg) parking, or entered when accelerometers and GPS both show zero motion for ~30 seconds
    StoplightIdle, // Entered when GPS speed is zero after decelerating
    Accelerating, // GPS speed is increasing
    Decelerating, // GPS speed is decreasing
    ParkedLongTerm, // GPS has not changed location in the last ~5 minutes
}

#[derive(Clone, Copy, Debug)]
pub enum Measurement {
    // GPS coordinates
    GPS(Option<Vector2<f64>>),
    // Accelerometer values in body frame where x=forwards
    IMU { accel: Vector3<f32>, gyro: Vector3<f32> },
}

#[derive(Clone, Copy, Debug)]
pub enum Prediction {
    Location(Vector2<f32>),
    Velocity(Vector2<f32>)
}

#[derive(Clone, Copy, Debug)]
pub enum Telemetry {
    Prediction(Prediction),
    Measurement(Measurement),
    Notification(Notification)
}

#[derive(Clone, Copy, Debug)]
pub enum Notification {
    SceneChange(Scene),
    WifiConnected,
    WifiDisconnected,
    GPSLost,
    GPSAcquired,
    CalibrationComplete,
    SetHeadlight(bool),
    SetBrakelight(bool)
}

// TODO: Make this clone() able, so multiple threads can point to the same underlying atomics for the hardware controls
pub struct DisplayControls {
    pub on: AtomicBool,
    pub brightness: AtomicU8,
    pub render_is_running: Signal<CriticalSectionRawMutex, bool>
}

impl Brightness for DisplayControls {
    fn set_brightness(&mut self, brightness: u8) {
        self.brightness.store(brightness, core::sync::atomic::Ordering::Relaxed);
    }

    fn set_on(&mut self, is_on: bool) {
        self.on.store(is_on, core::sync::atomic::Ordering::Relaxed);
    }

    // TODO: Split gamma out from brightness controls
    fn set_gamma(&mut self, _gamma: f32) {
        unimplemented!()
    }
}

impl core::fmt::Debug for DisplayControls {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> Result<(), core::fmt::Error> {
        f.debug_struct("DisplayControls")
            .field("on", &self.on)
            .field("brightness", &self.brightness)
            .field("render_is_running", &self.render_is_running.signaled())
            .finish()
    }
}

impl Default for DisplayControls {
    fn default() -> Self {
        Self {
            on: AtomicBool::new(true),
            brightness: AtomicU8::new(255),
            render_is_running: Signal::new()
        }
    }
}

#[derive(Debug)]
pub struct BusGarage {
    pub motion: Channel<NoopRawMutex, Measurement, 5>,
    pub scenes: Channel<CriticalSectionRawMutex, Notification, 5>,
    pub telemetry: Channel<CriticalSectionRawMutex, Telemetry, 15>,
    pub display: DisplayControls
}

impl Default for BusGarage {
    fn default() -> Self {
        Self {
            motion: Channel::new(),
            scenes: Channel::new(),
            telemetry: Channel::new(),
            display: Default::default()
        }
    }
}