use core::f32::consts::PI;
use nalgebra::ComplexField;
use nalgebra::RealField;

const G: f32 = 9.80665;

/// Complementary filter state
pub struct TiltEstimator {
    roll: f32,   // phi
    pitch: f32,  // theta
    yaw: f32,    // psi (integrated gyro yaw, will drift)
    alpha: f32,  // complementary weight for gyro integration (0..1)
}

impl TiltEstimator {
    pub fn new(alpha: f32) -> Self {
        Self { roll: 0.0, pitch: 0.0, yaw: 0.0, alpha }
    }

    /// dt in seconds, accel in m/s^2 body frame, gyro in rad/s body rates (gx, gy, gz)
    /// body axes assumed: x forward, y right, z down
    pub fn update(&mut self, dt: f32, accel: (f32, f32, f32), gyro: (f32, f32, f32)) {
        let (ax, ay, az) = accel;
        let (gx, gy, gz) = gyro;

        // 1) accel-based tilt estimates (avoid NaN by small epsilon)
        let eps = 1e-6;
        let roll_acc = (ay).atan2(az.max(eps)); // phi = atan2(ay, az)
        let pitch_acc = (-ax).atan2((ay*ay + az*az).sqrt().max(eps)); // theta = atan2(-ax, sqrt(ay^2+az^2))

        // 2) integrate gyro for angles (body rates -> approx angle rates)
        // Note: for small angles, roll_dot ≈ gx, pitch_dot ≈ gy, yaw_dot ≈ gz in body frame if axes aligned.
        // For better accuracy you'd convert rates to inertial derivatives but this is common and simple.
        let roll_from_gyro = self.roll + gx * dt;
        let pitch_from_gyro = self.pitch + gy * dt;
        let yaw_from_gyro = self.yaw + gz * dt;

        // 3) complementary combine
        let alpha = self.alpha;
        self.roll  = alpha * roll_from_gyro  + (1.0 - alpha) * roll_acc;
        self.pitch = alpha * pitch_from_gyro + (1.0 - alpha) * pitch_acc;
        self.yaw   = yaw_from_gyro; // we typically don't correct yaw with accel (no observability)

        // normalize yaw to -pi..pi
        if self.yaw > PI { self.yaw -= 2.0*PI; }
        if self.yaw < -PI { self.yaw += 2.0*PI; }
    }

    pub fn angles(&self) -> (f32, f32, f32) {
        (self.roll, self.pitch, self.yaw)
    }
}

/// Rotate body accel into ENU and remove gravity; returns (east, north, up) linear accel in m/s^2
pub fn accel_body_to_enu_lin(body_acc: (f32,f32,f32), roll: f32, pitch: f32, yaw: f32) -> (f32,f32,f32) {
    let (ax, ay, az) = body_acc;

    let (sr, cr) = roll.sin_cos();
    let (sp, cp) = pitch.sin_cos();
    let (sy, cy) = yaw.sin_cos();

    // Build R = Rz(yaw) * Ry(pitch) * Rx(roll)
    // multiply R * a_body
    let r11 =  cy*cp;
    let r12 =  cy*sp*sr - sy*cr;
    let r13 =  cy*sp*cr + sy*sr;

    let r21 =  sy*cp;
    let r22 =  sy*sp*sr + cy*cr;
    let r23 =  sy*sp*cr - cy*sr;

    let r31 = -sp;
    let r32 =  cp*sr;
    let r33 =  cp*cr;

    // a_nav = R * a_body
    let ax_nav = r11*ax + r12*ay + r13*az; // east
    let ay_nav = r21*ax + r22*ay + r23*az; // north
    let az_nav = r31*ax + r32*ay + r33*az; // up

    // remove gravity (nav frame gravity points down in NED; in our ENU 'up' axis positive up)
    // If accel measured gravity as +9.8 on sensor when static and z down convention used above,
    // then az_nav will include +g in 'up' direction; subtract G.
    let ax_lin = ax_nav;
    let ay_lin = ay_nav;
    let az_lin = az_nav - G; // linear acceleration in up axis

    (ax_lin, ay_lin, az_lin)
}