use embassy_time::Duration;
use enum_map::Enum;
use enumset::EnumSetType;
use figments::liber8tion::interpolate::Fract8;
use nalgebra::{Vector2, Vector3};

use crate::ego::engine::MotionState;

#[derive(Clone, Copy, Default, Debug)]
pub enum Scene {
    #[default]
    Ready, // Default state when booting up, but not when coming out of sleep
    Accelerating,
    Decelerating,
    Idle, // Default state when waking up from sleep, or entered when accelerometers and GPS both show zero motion for ~30 seconds
}

#[derive(Clone, Copy, Default, Debug)]
pub enum SensorState {
    #[default]
    // There is no connection to the sensor
    Offline,

    // Sensor is starting up
    AcquiringFix,

    // Sensor is ready and is generating data
    Online,

    // Sensor was previously fully functioning but currently is not (eg, gps fix lost)
    Degraded,
}

#[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> },

    // Hardware status updates
    SensorHardwareStatus(SensorSource, SensorState),

    // Simulation metadata updates
    SimulationProgress(SensorSource, Duration, Fract8),
    Annotation
}

#[derive(Default, Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)]
pub enum Personality {
    Sleeping, // System should be using as low power as possible, displays off, etc
    #[default]
    Waking, // System is resuming from sleep, or it is the first boot. A transient state that quickly turns into Active.
    Parked, // System should be acting like an art piece with some idle animations, maybe it can search for some wifi/bluetooth
    Active // System is almost likely on the road and must provide lighting
}

#[derive(Clone, Copy, Debug)]
pub enum Prediction {
    Motion { prev: MotionState, next: MotionState },
    Velocity(f32),
    Location(Vector2<f64>),
    // States of external connections to the world
    SensorStatus(SensorSource, SensorState),
    // The system should enter into one of the four personalities
    SetPersonality(Personality)
}

// GPS data = 2, motion data = 1
#[derive(Debug, EnumSetType, Enum)]
pub enum SensorSource {
    // Real hardware
    IMU,
    GPS,

    // Connectivity related
    Wifi,

    // Fusion outputs
    MotionFrame,
    Location,
    Cloud,

    // Simulated sensors
    Demo,
    Simulation,
    Annotations
}

#[cfg(feature="simulation")]
use crate::simdata::StreamType;
#[cfg(feature="simulation")]
impl From<StreamType> for SensorSource {
    fn from(value: StreamType) -> Self {
        match value {
            StreamType::Annotations => Self::Annotations,
            StreamType::GPS => Self::GPS,
            StreamType::IMU => Self::IMU
        }
    }
}