///|
/// Gyroscope calibration data
pub(all) struct GyroCalibration {
  bias : Vec3
  drift_rate : Vec3
} derive(Show, Eq)

///|
/// Create default gyro calibration
pub fn GyroCalibration::default() -> GyroCalibration {
  { bias: Vec3::zero(), drift_rate: Vec3::zero() }
}

///|
/// Gyroscope driver
pub struct Gyroscope {
  mut calibration : GyroCalibration
  mut last_reading : GyroReading
  mut integrated_angle : Vec3
  mut raw_readings : Array[Vec3]
  config : SensorConfig
}

///|
/// Create new gyroscope
pub fn Gyroscope::new(config : SensorConfig) -> Gyroscope {
  {
    calibration: GyroCalibration::default(),
    last_reading: { data: Vec3::zero(), timestamp: 0L },
    integrated_angle: Vec3::zero(),
    raw_readings: [],
    config,
  }
}

///|
/// Calibrate gyroscope by measuring bias when stationary
pub fn Gyroscope::calibrate(self : Gyroscope) -> Unit {
  if self.raw_readings.length() < 20 {
    return
  }

  // Calculate average bias
  let mut sum_x = 0.0
  let mut sum_y = 0.0
  let mut sum_z = 0.0
  for reading in self.raw_readings {
    sum_x += reading.x
    sum_y += reading.y
    sum_z += reading.z
  }
  let count = self.raw_readings.length().to_double()
  let bias = Vec3::new(sum_x / count, sum_y / count, sum_z / count)
  self.calibration = { bias, drift_rate: self.calibration.drift_rate }

  // Clear readings and reset integration
  self.raw_readings = []
  self.integrated_angle = Vec3::zero()
}

///|
/// Add raw sample for calibration
pub fn Gyroscope::add_calibration_sample(self : Gyroscope, raw : Vec3) -> Unit {
  self.raw_readings.push(raw)
}

///|
/// Apply calibration to raw reading
fn apply_gyro_calibration(raw : Vec3, cal : GyroCalibration) -> Vec3 {
  Vec3::new(
    raw.x - cal.bias.x - cal.drift_rate.x,
    raw.y - cal.bias.y - cal.drift_rate.y,
    raw.z - cal.bias.z - cal.drift_rate.z,
  )
}

///|
/// Read gyroscope data
pub fn Gyroscope::read(
  self : Gyroscope,
  raw : Vec3,
  timestamp : Int64,
) -> GyroReading {
  let calibrated = apply_gyro_calibration(raw, self.calibration)
  let reading : GyroReading = { data: calibrated, timestamp }
  self.last_reading = reading
  reading
}

///|
/// Get current angular velocity
pub fn Gyroscope::get_angular_velocity(self : Gyroscope) -> Vec3 {
  self.last_reading.data
}

///|
/// Integrate angular velocity to get rotation angle
pub fn Gyroscope::integrate(self : Gyroscope, dt : Double) -> Vec3 {
  let delta = self.last_reading.data.scale(dt)
  self.integrated_angle = self.integrated_angle.add(delta)
  self.integrated_angle
}

///|
/// Reset integrated angle
pub fn Gyroscope::reset_integration(self : Gyroscope) -> Unit {
  self.integrated_angle = Vec3::zero()
}

///|
/// Detect rotation around any axis
pub fn Gyroscope::is_rotating(self : Gyroscope, threshold : Double) -> Bool {
  let mag = self.last_reading.data.magnitude()
  mag > threshold
}

///|
/// Get rotation quaternion from angular velocity
pub fn Gyroscope::get_rotation_quaternion(
  self : Gyroscope,
  dt : Double,
) -> Quaternion {
  let omega = self.last_reading.data
  let mag = omega.magnitude()
  if mag < 0.001 {
    // No significant rotation
    Quaternion::identity()
  } else {
    // Create rotation quaternion from axis-angle
    let axis = omega.scale(1.0 / mag)
    let angle = mag * dt
    Quaternion::from_axis_angle(axis, angle)
  }
}

///|
/// Detect specific rotation gesture
pub fn Gyroscope::detect_rotation_type(self : Gyroscope) -> (String, Double)? {
  let omega = self.last_reading.data
  let threshold = 2.0 // rad/s
  if omega.x.abs() > threshold &&
    omega.y.abs() < threshold / 2.0 &&
    omega.z.abs() < threshold / 2.0 {
    Some(("roll", omega.x))
  } else if omega.y.abs() > threshold &&
    omega.x.abs() < threshold / 2.0 &&
    omega.z.abs() < threshold / 2.0 {
    Some(("pitch", omega.y))
  } else if omega.z.abs() > threshold &&
    omega.x.abs() < threshold / 2.0 &&
    omega.y.abs() < threshold / 2.0 {
    Some(("yaw", omega.z))
  } else {
    None
  }
}

///|
/// Low-pass filter for gyro data
pub fn Gyroscope::apply_lowpass_filter(
  self : Gyroscope,
  previous : Vec3,
  alpha : Double,
) -> Vec3 {
  let current = self.last_reading.data
  Vec3::new(
    alpha * current.x + (1.0 - alpha) * previous.x,
    alpha * current.y + (1.0 - alpha) * previous.y,
    alpha * current.z + (1.0 - alpha) * previous.z,
  )
}