use std::{collections::VecDeque, ops::{Div, Sub}};

use common::{CamState, ControlPacket, SensorData};
use anyhow::Result;

struct AutoConfig {
    turn_gain: f32,
    auto_fire_distance: f32,
}
const AUTO_GAP: i16 = 140; /// mm tofs must drop to count as a hit
const AUTO_SELF_OCCLUSION: u16 = 130; /// minimum 

struct AutoInterface {
}

impl AutoInterface {
    /// change active command, fails if not in control
    fn run_command(&self, command: ControlPacket) -> Result<()> {unimplemented!()}
    fn sensor_data(&self) -> SensorData {unimplemented!()}
    fn cam_state(&self) -> CamState {unimplemented!()}
    async fn sensor_update(&self) -> SensorData {unimplemented!()}
    /// disable auto
    fn disable(&self) {unimplemented!()}
    /// request auto enable, fails if the driver does not grant it
    fn enable(&self) -> Result<()> {unimplemented!()}
    fn enabled(&self) -> bool {unimplemented!()}
    fn auto_conf(&self) -> AutoConfig {unimplemented!()}
    fn send_message(&self, message: String) {unimplemented!()}
}

struct Configurable {
    name: &'static str,
    default: f32,
    min: f32,
    max: f32,
}

/// A fake trait in the sense that these methods are exposed as symbols, not trait methods
trait Auto {
    /// entrypoint
    async fn run(interface: &AutoInterface);
    /// register
    fn default_configs() -> [Configurable];
    fn name() -> &'static str;
}

async fn auto (interface: &AutoInterface) {
    let mut tof_l = Stats::new();
    let mut tof_r = Stats::new();
    loop {
        let data = interface.sensor_update().await;
        let cam = interface.cam_state();
        let CamState::Charged = cam else {
            continue;
        };
        
        let Some(latest_tof_l) = data.tof_l else {
            continue;
        };
        tof_l.update(latest_tof_l as i16);
        let Some(latest_tof_r) = data.tof_r else {
            continue;
        };
        tof_r.update(latest_tof_r as i16);

        let detection = |latest: u16, delta: i16| {
            delta < AUTO_GAP && latest > AUTO_SELF_OCCLUSION
        };

        if detection(latest_tof_l, tof_l.delta()) || detection(latest_tof_l, tof_l.delta()) {
            if let Ok(()) = interface.enable() {
                println!("found, now seek")
            }
        }
    }
}


struct Stats<T> {
    table: VecDeque<T>
}

impl<T> Stats<T> {
    pub fn new() -> Self {
        Self { table: VecDeque::new() }

    }

    const MAX_ELEMENTS: usize = 3;

    pub fn update(&mut self, elem: T) {
        self.table.push_front(elem);
        if self.table.len() > Self::MAX_ELEMENTS {
            self.table.pop_back();
        }
    }
}

impl<T: Ord + Sub<Output = T> + Copy + From<u8>> Stats<T> {
    pub fn delta(&self) -> T {
        *self.table.get(0).unwrap_or(&T::from(0)) - *self.table.get(1).unwrap_or(&T::from(0))
    }

    pub fn max(&self) -> T {
        *self.table.iter().max().unwrap_or(&T::from(0))
    }

    pub fn min(&self) -> T {
        *self.table.iter().max().unwrap_or(&T::from(0))
    }
}