///|
/// Accelerometer calibration data
pub(all) struct AccelCalibration {
  offset : Vec3
  scale : Vec3
  gravity : Double
} derive(Show, Eq)

///|
/// Create default calibration (no offset, unity scale, Earth's gravity)
pub fn AccelCalibration::default() -> AccelCalibration {
  { offset: Vec3::zero(), scale: Vec3::new(1.0, 1.0, 1.0), gravity: 9.81 }
}

///|
/// Accelerometer driver
pub struct Accelerometer {
  mut calibration : AccelCalibration
  mut last_reading : AccelReading
  mut raw_readings : Array[Vec3]
  config : SensorConfig
}

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

///|
/// Calibrate accelerometer using collected samples
/// Assumes the sensor is stationary during calibration
pub fn Accelerometer::calibrate(self : Accelerometer) -> Unit {
  if self.raw_readings.length() < 10 {
    return
  }

  // Calculate average of all samples
  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 avg_x = sum_x / count
  let avg_y = sum_y / count
  let avg_z = sum_z / count

  // Offset is the difference from expected gravity (assuming Z-up)
  let offset = Vec3::new(avg_x, avg_y, avg_z - self.calibration.gravity)
  self.calibration = {
    offset,
    scale: self.calibration.scale,
    gravity: self.calibration.gravity,
  }

  // Clear readings after calibration
  self.raw_readings = []
}

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

///|
/// Apply calibration to raw reading
fn apply_calibration(raw : Vec3, cal : AccelCalibration) -> Vec3 {
  Vec3::new(
    (raw.x - cal.offset.x) * cal.scale.x,
    (raw.y - cal.offset.y) * cal.scale.y,
    (raw.z - cal.offset.z) * cal.scale.z,
  )
}

///|
/// Read accelerometer data
pub fn Accelerometer::read(
  self : Accelerometer,
  raw : Vec3,
  timestamp : Int64,
) -> AccelReading {
  let calibrated = apply_calibration(raw, self.calibration)
  let reading : AccelReading = { data: calibrated, timestamp }
  self.last_reading = reading
  reading
}

///|
/// Get gravity vector from acceleration
pub fn Accelerometer::get_gravity(self : Accelerometer) -> Vec3 {
  self.last_reading.data
}

///|
/// Calculate linear acceleration (remove gravity component)
pub fn Accelerometer::get_linear_acceleration(
  self : Accelerometer,
  gravity : Vec3,
) -> Vec3 {
  self.last_reading.data.sub(gravity)
}

///|
/// Detect if device is in free fall
pub fn Accelerometer::is_freefall(self : Accelerometer) -> Bool {
  let mag = self.last_reading.data.magnitude()
  // Free fall when total acceleration is close to zero
  mag < 2.0 // Less than 2 m/s²
}

///|
/// Detect impact/shock
pub fn Accelerometer::detect_impact(self : Accelerometer) -> Double? {
  let mag = self.last_reading.data.magnitude()
  // Impact when acceleration exceeds threshold
  if mag > 30.0 {
    // More than 3g
    Some(mag)
  } else {
    None
  }
}

///|
/// Calculate tilt angles (pitch and roll) from gravity
pub fn Accelerometer::get_tilt(self : Accelerometer) -> (Double, Double) {
  let acc = self.last_reading.data

  // Roll (rotation around X-axis)
  let roll = @math.atan2(acc.y, acc.z)

  // Pitch (rotation around Y-axis)
  let pitch = @math.atan2(-acc.x, (acc.y * acc.y + acc.z * acc.z).sqrt())
  (pitch, roll)
}

///|
/// Detect shake motion using acceleration variance
pub fn Accelerometer::detect_shake(
  _self : Accelerometer,
  recent_readings : ArrayView[AccelReading],
) -> Bool {
  if recent_readings.length() < 5 {
    return false
  }

  // Calculate variance of acceleration magnitude
  let mut sum_mag = 0.0
  let mut sum_sq_mag = 0.0
  for reading in recent_readings {
    let mag = reading.data.magnitude()
    sum_mag += mag
    sum_sq_mag += mag * mag
  }
  let count = recent_readings.length().to_double()
  let mean = sum_mag / count
  let variance = sum_sq_mag / count - mean * mean

  // High variance indicates shaking
  variance > 100.0 // Threshold for shake detection
}