///|
/// Complementary filter for sensor fusion
pub struct ComplementaryFilter {
  mut orientation : Quaternion
  alpha : Double // Accelerometer/mag weight (0-1)
}

///|
/// Create new complementary filter
pub fn ComplementaryFilter::new(alpha : Double) -> ComplementaryFilter {
  { orientation: Quaternion::identity(), alpha }
}

///|
/// Update orientation using complementary filter
pub fn ComplementaryFilter::update(
  self : ComplementaryFilter,
  accel : Vec3,
  gyro : Vec3,
  mag : Vec3,
  dt : Double,
) -> Quaternion {
  // Gyro integration (high-pass)
  let gyro_quat = Quaternion::from_axis_angle(gyro, gyro.magnitude() * dt)
  let gyro_orientation = self.orientation.multiply(gyro_quat)

  // Accelerometer-based orientation (low-pass)
  let accel_orientation = orientation_from_accel_mag(accel, mag)

  // Complementary filter: blend gyro and accel/mag
  self.orientation = slerp(gyro_orientation, accel_orientation, self.alpha).normalize()
  self.orientation
}

///|
/// Get current orientation
pub fn ComplementaryFilter::get_orientation(
  self : ComplementaryFilter,
) -> Quaternion {
  self.orientation
}

///|
/// Spherical linear interpolation for quaternions
fn slerp(q1 : Quaternion, q2 : Quaternion, t : Double) -> Quaternion {
  let dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z

  // Ensure shortest path
  let (q2_w, q2_x, q2_y, q2_z) = if dot < 0.0 {
    (-q2.w, -q2.x, -q2.y, -q2.z)
  } else {
    (q2.w, q2.x, q2.y, q2.z)
  }
  let dot_abs = dot.abs()

  // If quaternions are very close, use linear interpolation
  if dot_abs > 0.9995 {
    return {
      w: q1.w + t * (q2_w - q1.w),
      x: q1.x + t * (q2_x - q1.x),
      y: q1.y + t * (q2_y - q1.y),
      z: q1.z + t * (q2_z - q1.z),
    }.normalize()
  }
  let theta = @math.acos(dot_abs)
  let sin_theta = @math.sin(theta)
  let ratio_a = @math.sin((1.0 - t) * theta) / sin_theta
  let ratio_b = @math.sin(t * theta) / sin_theta
  {
    w: q1.w * ratio_a + q2_w * ratio_b,
    x: q1.x * ratio_a + q2_x * ratio_b,
    y: q1.y * ratio_a + q2_y * ratio_b,
    z: q1.z * ratio_a + q2_z * ratio_b,
  }
}

///|
/// Calculate orientation from accelerometer and magnetometer
fn orientation_from_accel_mag(accel : Vec3, mag : Vec3) -> Quaternion {
  // Normalize inputs
  let acc_norm = accel.normalize()
  let mag_norm = mag.normalize()

  // Calculate roll and pitch from accelerometer
  let roll = @math.atan2(acc_norm.y, acc_norm.z)
  let pitch = @math.atan2(
    -acc_norm.x,
    (acc_norm.y * acc_norm.y + acc_norm.z * acc_norm.z).sqrt(),
  )

  // Tilt-compensated magnetometer for yaw
  let cos_roll = @math.cos(roll)
  let sin_roll = @math.sin(roll)
  let cos_pitch = @math.cos(pitch)
  let sin_pitch = @math.sin(pitch)
  let mag_x = mag_norm.x * cos_pitch + mag_norm.z * sin_pitch
  let mag_y = mag_norm.x * sin_roll * sin_pitch +
    mag_norm.y * cos_roll -
    mag_norm.z * sin_roll * cos_pitch
  let yaw = @math.atan2(mag_y, mag_x)

  // Convert Euler to quaternion
  euler_to_quaternion(roll, pitch, yaw)
}

///|
/// Convert Euler angles to quaternion
fn euler_to_quaternion(
  roll : Double,
  pitch : Double,
  yaw : Double,
) -> Quaternion {
  let cr = @math.cos(roll / 2.0)
  let sr = @math.sin(roll / 2.0)
  let cp = @math.cos(pitch / 2.0)
  let sp = @math.sin(pitch / 2.0)
  let cy = @math.cos(yaw / 2.0)
  let sy = @math.sin(yaw / 2.0)
  {
    w: cr * cp * cy + sr * sp * sy,
    x: sr * cp * cy - cr * sp * sy,
    y: cr * sp * cy + sr * cp * sy,
    z: cr * cp * sy - sr * sp * cy,
  }
}

///|
/// Simplified Kalman filter for orientation
pub struct KalmanFilter {
  mut orientation : Quaternion
  mut error_covariance : Double
  process_noise : Double
  measurement_noise : Double
}

///|
/// Create new Kalman filter
pub fn KalmanFilter::new(
  process_noise : Double,
  measurement_noise : Double,
) -> KalmanFilter {
  {
    orientation: Quaternion::identity(),
    error_covariance: 1.0,
    process_noise,
    measurement_noise,
  }
}

///|
/// Update Kalman filter
pub fn KalmanFilter::update(
  self : KalmanFilter,
  accel : Vec3,
  gyro : Vec3,
  mag : Vec3,
  dt : Double,
) -> Quaternion {
  // Prediction step (using gyroscope)
  let gyro_quat = Quaternion::from_axis_angle(gyro, gyro.magnitude() * dt)
  let predicted_orientation = self.orientation.multiply(gyro_quat).normalize()
  let predicted_covariance = self.error_covariance + self.process_noise

  // Measurement step (using accelerometer and magnetometer)
  let measured_orientation = orientation_from_accel_mag(accel, mag)

  // Kalman gain
  let kalman_gain = predicted_covariance /
    (predicted_covariance + self.measurement_noise)

  // Update estimate
  self.orientation = slerp(
    predicted_orientation, measured_orientation, kalman_gain,
  ).normalize()
  self.error_covariance = (1.0 - kalman_gain) * predicted_covariance
  self.orientation
}

///|
/// Get current orientation
pub fn KalmanFilter::get_orientation(self : KalmanFilter) -> Quaternion {
  self.orientation
}

///|
/// Madgwick filter (gradient descent orientation filter)
pub struct MadgwickFilter {
  mut orientation : Quaternion
  beta : Double // Filter gain
}

///|
/// Create new Madgwick filter
pub fn MadgwickFilter::new(beta : Double) -> MadgwickFilter {
  { orientation: Quaternion::identity(), beta }
}

///|
/// Update Madgwick filter
pub fn MadgwickFilter::update(
  self : MadgwickFilter,
  accel : Vec3,
  gyro : Vec3,
  mag : Vec3,
  dt : Double,
) -> Quaternion {
  let q = self.orientation

  // Normalize accelerometer
  let a = accel.normalize()

  // Normalize magnetometer
  let _m = mag.normalize()

  // Gradient descent algorithm corrective step
  let f1 = 2.0 * (q.x * q.z - q.w * q.y) - a.x
  let f2 = 2.0 * (q.w * q.x + q.y * q.z) - a.y
  let f3 = 2.0 * (0.5 - q.x * q.x - q.y * q.y) - a.z

  // Jacobian
  let j11 = -2.0 * q.y
  let j12 = 2.0 * q.z
  let j13 = -2.0 * q.w
  let j14 = 2.0 * q.x
  let j21 = 2.0 * q.x
  let j22 = 2.0 * q.w
  let j23 = 2.0 * q.z
  let j24 = 2.0 * q.y
  let j31 = 0.0
  let j32 = -4.0 * q.x
  let j33 = -4.0 * q.y
  let j34 = 0.0

  // Gradient
  let gradient_w = j11 * f1 + j21 * f2 + j31 * f3
  let gradient_x = j12 * f1 + j22 * f2 + j32 * f3
  let gradient_y = j13 * f1 + j23 * f2 + j33 * f3
  let gradient_z = j14 * f1 + j24 * f2 + j34 * f3

  // Normalize gradient
  let norm = (gradient_w * gradient_w +
  gradient_x * gradient_x +
  gradient_y * gradient_y +
  gradient_z * gradient_z).sqrt()
  let (grad_w, grad_x, grad_y, grad_z) = if norm > 0.0 {
    (gradient_w / norm, gradient_x / norm, gradient_y / norm, gradient_z / norm)
  } else {
    (0.0, 0.0, 0.0, 0.0)
  }

  // Compute rate of change of quaternion
  let q_dot_w = 0.5 * (-q.x * gyro.x - q.y * gyro.y - q.z * gyro.z) -
    self.beta * grad_w
  let q_dot_x = 0.5 * (q.w * gyro.x + q.y * gyro.z - q.z * gyro.y) -
    self.beta * grad_x
  let q_dot_y = 0.5 * (q.w * gyro.y - q.x * gyro.z + q.z * gyro.x) -
    self.beta * grad_y
  let q_dot_z = 0.5 * (q.w * gyro.z + q.x * gyro.y - q.y * gyro.x) -
    self.beta * grad_z

  // Integrate to yield quaternion
  self.orientation = {
    w: q.w + q_dot_w * dt,
    x: q.x + q_dot_x * dt,
    y: q.y + q_dot_y * dt,
    z: q.z + q_dot_z * dt,
  }.normalize()
  self.orientation
}

///|
/// Get current orientation
pub fn MadgwickFilter::get_orientation(self : MadgwickFilter) -> Quaternion {
  self.orientation
}