use nalgebra::{Vector3, Rotation3, Matrix3, Unit};
use log::*;

use crate::CircularBuffer;

#[derive(Default, Debug, Clone, Copy)]
pub struct OrientationEstimator {
    sensor_bias: Vector3<f32>, // Bias of the sensors at rest, eg, this will include the pull of gravity and needs applied prior to rotating
    down: Option<Unit<Vector3<f32>>>, 
    forward: Option<Unit<Vector3<f32>>>,
    rotation_sb: Rotation3<f32>, // sensor -> body
    accel_history: CircularBuffer<Vector3<f32>, 200>,
    forwards_history: CircularBuffer<Vector3<f32>, 200>
}

impl OrientationEstimator {
    pub fn is_ready(&self) -> bool {
        self.forward.is_some()
    }

    pub fn has_down(&self) -> bool {
        self.down.is_some()
    }

    pub fn bias(&self) -> Vector3<f32> {
        self.sensor_bias
    }

    pub fn insert(&mut self, accel: Vector3<f32>) {
        self.accel_history.insert(accel);
        match self.down {
            None => {
                if self.accel_history.is_filled() {
                    // Estimate down from stationary accel
                    let avg = self.accel_history.data().iter().sum::<Vector3<f32>>()
                        / (self.accel_history.data().len() as f32);
                    if avg.norm() >= 9.1 && avg.norm() <= 9.9 {
                        self.down = Some(Unit::new_normalize(-avg));
                        self.sensor_bias = avg;
                        info!("Found down={:?} bias={:?}", self.down, self.sensor_bias);
                    }
                }
            },
            Some(down) => {
                // Project motion into horizontal plane
                let avg = self.accel_history.data().iter().sum::<Vector3<f32>>() - self.sensor_bias;
                let horiz = avg - avg.dot(&down) * down.into_inner();

                if horiz.norm() < 9.0 {
                    return; // not enough motion
                }
                //warn!("motion accel={accel:?} accel.norm={} horiz={horiz:?} horiz.norm={}", accel.norm(), horiz.norm());

                self.forwards_history.insert(horiz);
                if self.forwards_history.is_filled() {

                    let avg_forwards = self.forwards_history.data().iter().sum::<Vector3<f32>>() / (self.forwards_history.data().len() as f32);
                    let unit_forward = Unit::new_normalize(avg_forwards);

                    // If we already had a forward, allow reversal check
                    if let Some(prev) = &self.forward {
                        // Positive dot means similar direction, negative means opposite direction
                        if unit_forward.dot(prev) < -0.7 {
                            // Strong opposite -> flip
                            //warn!("Forwards is very different!!! prev={prev:?} forward={unit_forward:?} dot={}", unit_forward.dot(prev));
                            self.forward = Some(Unit::new_unchecked(-unit_forward.into_inner()));
                        }
                    } else {
                        info!("Motion frame established with forwards={unit_forward:?} avg.norm={} down={:?} bias={:?}", horiz.norm(), self.down, self.sensor_bias);
                        self.forward = Some(unit_forward);
                    }

                    // Build body axes
                    let x = unit_forward.into_inner();
                    let z = down.into_inner();
                    let y = z.cross(&x).normalize();
                    let x = y.cross(&z).normalize(); // re-orthogonalize

                    let mat = Matrix3::from_columns(&[x, y, z]);
                    self.rotation_sb = Rotation3::from_matrix_unchecked(mat);
                }
            }
        }
    }

    pub fn apply(&self, v_sensor: Vector3<f32>) -> Vector3<f32> {
        self.rotation_sb * (v_sensor - self.sensor_bias)
    }
}
