///|
/// Motion controller integrating all sensors
pub struct MotionController {
  accel : Accelerometer
  gyro : Gyroscope
  mag : Magnetometer
  mut filter : ComplementaryFilter
  mut state : MotionState
  mut recent_accels : Array[AccelReading]
  max_history : Int
}

///|
/// Create new motion controller
pub fn MotionController::new(config : SensorConfig) -> MotionController {
  {
    accel: Accelerometer::new(config),
    gyro: Gyroscope::new(config),
    mag: Magnetometer::new(config),
    filter: ComplementaryFilter::new(0.02), // 2% accel/mag weight
    state: MotionState::new(),
    recent_accels: [],
    max_history: 20,
  }
}

///|
/// Update all sensors and state
pub fn MotionController::update(
  self : MotionController,
  accel_raw : Vec3,
  gyro_raw : Vec3,
  mag_raw : Vec3,
  timestamp : Int64,
  dt : Double,
) -> MotionState {
  // Read sensors
  let accel_reading = self.accel.read(accel_raw, timestamp)
  let gyro_reading = self.gyro.read(gyro_raw, timestamp)
  let mag_reading = self.mag.read(mag_raw, timestamp)

  // Update sensor fusion
  let orientation = self.filter.update(
    accel_reading.data,
    gyro_reading.data,
    mag_reading.data,
    dt,
  )

  // Update state
  self.state = {
    acceleration: accel_reading.data,
    angular_velocity: gyro_reading.data,
    magnetic_field: mag_reading.data,
    orientation,
    timestamp,
  }

  // Keep recent accelerometer readings for gesture detection
  self.recent_accels.push(accel_reading)
  if self.recent_accels.length() > self.max_history {
    self.recent_accels = self.recent_accels[1:].to_array()
  }
  self.state
}

///|
/// Get current motion state
pub fn MotionController::get_state(self : MotionController) -> MotionState {
  self.state
}

///|
/// Get orientation as Euler angles (roll, pitch, yaw) in radians
pub fn MotionController::get_euler_angles(
  self : MotionController,
) -> (Double, Double, Double) {
  self.state.orientation.to_euler()
}

///|
/// Get orientation as Euler angles in degrees
pub fn MotionController::get_euler_degrees(
  self : MotionController,
) -> (Double, Double, Double) {
  let (roll, pitch, yaw) = self.get_euler_angles()
  (roll * 180.0 / @math.PI, pitch * 180.0 / @math.PI, yaw * 180.0 / @math.PI)
}

///|
/// Detect current gesture
pub fn MotionController::detect_gesture(self : MotionController) -> Gesture {
  // Check for free fall
  if self.accel.is_freefall() {
    return FreeFall
  }

  // Check for impact
  match self.accel.detect_impact() {
    Some(force) => return Impact(force)
    None => ()
  }

  // Check for shake
  if self.recent_accels.length() >= 5 {
    let view = self.recent_accels[:]
    if self.accel.detect_shake(view) {
      return Shaking
    }
  }

  // Check for significant rotation
  let angular_vel = self.gyro.get_angular_velocity()
  let rotation_mag = angular_vel.magnitude()
  if rotation_mag > 3.0 {
    // Significant rotation
    return Rotating(rotation_mag)
  }

  // Check for tilt
  let (pitch, roll) = self.accel.get_tilt()
  let tilt_threshold = 0.5 // ~28 degrees
  if pitch.abs() > tilt_threshold || roll.abs() > tilt_threshold {
    return Tilting(pitch, roll)
  }

  // Default to stationary
  Stationary
}

///|
/// Calibrate all sensors
pub fn MotionController::calibrate(self : MotionController) -> Unit {
  self.accel.calibrate()
  self.gyro.calibrate()
  self.mag.calibrate_simple()
}

///|
/// Add calibration samples for all sensors
pub fn MotionController::add_calibration_sample(
  self : MotionController,
  accel_raw : Vec3,
  gyro_raw : Vec3,
  mag_raw : Vec3,
) -> Unit {
  self.accel.add_calibration_sample(accel_raw)
  self.gyro.add_calibration_sample(gyro_raw)
  self.mag.add_calibration_sample(mag_raw)
}

///|
/// Get compass heading in radians
pub fn MotionController::get_heading(self : MotionController) -> Double {
  let (pitch, roll) = self.accel.get_tilt()
  self.mag.get_tilt_compensated_heading(pitch, roll)
}

///|
/// Get compass heading in degrees
pub fn MotionController::get_heading_degrees(self : MotionController) -> Double {
  let heading = self.get_heading()
  let degrees = heading * 180.0 / @math.PI
  if degrees < 0.0 {
    degrees + 360.0
  } else {
    degrees
  }
}

///|
/// Detect if device is moving
pub fn MotionController::is_moving(self : MotionController) -> Bool {
  let accel_mag = self.state.acceleration.magnitude()
  let gyro_mag = self.state.angular_velocity.magnitude()

  // Device is moving if acceleration or rotation exceeds threshold
  let accel_threshold = 11.0 // Above gravity + some margin
  let gyro_threshold = 0.1 // rad/s
  accel_mag > accel_threshold || gyro_mag > gyro_threshold
}

///|
/// Reset sensor fusion
pub fn MotionController::reset(self : MotionController) -> Unit {
  self.filter = ComplementaryFilter::new(0.02)
  self.state = MotionState::new()
  self.recent_accels = []
}