use crate::serial_core::multi_dim_force::PztProcessor;

const FINGER_SAMPLE_COUNT: usize = 84;
const MIN_TANGENTIAL_MAGNITUDE: f32 = 0.02;

#[derive(Debug, Clone, Copy)]
pub struct HudSpatialForce {
    pub angle_deg: f32,
    pub magnitude: f32,
}

pub struct ForceEstimatorState {
    pzt_processor: PztProcessor,
}

impl ForceEstimatorState {
    pub fn new() -> Self {
        Self {
            pzt_processor: PztProcessor::new(),
        }
    }

    pub fn reset(&mut self) {
        self.pzt_processor.reset_baseline();
    }

    pub fn analyze(&mut self, values: &[u32]) -> Option<HudSpatialForce> {
        if values.len() != FINGER_SAMPLE_COUNT {
            return None;
        }

        let pzt_values = values.iter().map(|value| *value as f32).collect::<Vec<_>>();

        self.pzt_processor
            .get_pzt_analysis(&pzt_values)
            .ok()
            .filter(|analysis| analysis.magnitude > MIN_TANGENTIAL_MAGNITUDE)
            .map(|analysis| HudSpatialForce {
                angle_deg: analysis.angle_deg,
                magnitude: analysis.magnitude,
            })
    }
}

impl Default for ForceEstimatorState {
    fn default() -> Self {
        Self::new()
    }
}
