///|
/// Magnetometer calibration data
pub(all) struct MagCalibration {
  hard_iron : Vec3 // Offset
  soft_iron : Array[Double] // 3x3 matrix (row-major)
} derive(Show, Eq)

///|
/// Create default magnetometer calibration
pub fn MagCalibration::default() -> MagCalibration {
  {
    hard_iron: Vec3::zero(),
    soft_iron: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], // Identity matrix
  }
}

///|
/// Magnetometer driver
pub struct Magnetometer {
  mut calibration : MagCalibration
  mut last_reading : MagReading
  mut raw_readings : Array[Vec3]
  config : SensorConfig
}

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

///|
/// Simple hard-iron calibration (offset only)
pub fn Magnetometer::calibrate_simple(self : Magnetometer) -> Unit {
  if self.raw_readings.length() < 50 {
    return
  }

  // Find min and max for each axis
  let mut min_x = self.raw_readings[0].x
  let mut max_x = self.raw_readings[0].x
  let mut min_y = self.raw_readings[0].y
  let mut max_y = self.raw_readings[0].y
  let mut min_z = self.raw_readings[0].z
  let mut max_z = self.raw_readings[0].z
  for reading in self.raw_readings {
    if reading.x < min_x {
      min_x = reading.x
    }
    if reading.x > max_x {
      max_x = reading.x
    }
    if reading.y < min_y {
      min_y = reading.y
    }
    if reading.y > max_y {
      max_y = reading.y
    }
    if reading.z < min_z {
      min_z = reading.z
    }
    if reading.z > max_z {
      max_z = reading.z
    }
  }

  // Offset is the center of the range
  let hard_iron = Vec3::new(
    (min_x + max_x) / 2.0,
    (min_y + max_y) / 2.0,
    (min_z + max_z) / 2.0,
  )
  self.calibration = { hard_iron, soft_iron: self.calibration.soft_iron }
  self.raw_readings = []
}

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

///|
/// Apply calibration to raw reading
fn apply_mag_calibration(raw : Vec3, cal : MagCalibration) -> Vec3 {
  // Apply hard-iron correction (offset)
  let corrected = Vec3::new(
    raw.x - cal.hard_iron.x,
    raw.y - cal.hard_iron.y,
    raw.z - cal.hard_iron.z,
  )

  // Apply soft-iron correction (matrix multiplication)
  // M = [[m0, m1, m2], [m3, m4, m5], [m6, m7, m8]]
  let m = cal.soft_iron
  Vec3::new(
    m[0] * corrected.x + m[1] * corrected.y + m[2] * corrected.z,
    m[3] * corrected.x + m[4] * corrected.y + m[5] * corrected.z,
    m[6] * corrected.x + m[7] * corrected.y + m[8] * corrected.z,
  )
}

///|
/// Read magnetometer data
pub fn Magnetometer::read(
  self : Magnetometer,
  raw : Vec3,
  timestamp : Int64,
) -> MagReading {
  let calibrated = apply_mag_calibration(raw, self.calibration)
  let reading : MagReading = { data: calibrated, timestamp }
  self.last_reading = reading
  reading
}

///|
/// Get magnetic field vector
pub fn Magnetometer::get_magnetic_field(self : Magnetometer) -> Vec3 {
  self.last_reading.data
}

///|
/// Calculate heading (yaw) from magnetometer
/// Returns angle in radians from magnetic north
pub fn Magnetometer::get_heading(self : Magnetometer) -> Double {
  let mag = self.last_reading.data
  @math.atan2(mag.y, mag.x)
}

///|
/// Calculate tilt-compensated heading
pub fn Magnetometer::get_tilt_compensated_heading(
  self : Magnetometer,
  pitch : Double,
  roll : Double,
) -> Double {
  let mag = self.last_reading.data

  // Rotate magnetometer readings by pitch and roll
  let cos_pitch = @math.cos(pitch)
  let sin_pitch = @math.sin(pitch)
  let cos_roll = @math.cos(roll)
  let sin_roll = @math.sin(roll)

  // Tilt compensation
  let mag_x = mag.x * cos_pitch + mag.z * sin_pitch
  let mag_y = mag.x * sin_roll * sin_pitch +
    mag.y * cos_roll -
    mag.z * sin_roll * cos_pitch
  @math.atan2(mag_y, mag_x)
}

///|
/// Detect magnetic interference
pub fn Magnetometer::detect_interference(self : Magnetometer) -> Bool {
  let mag = self.last_reading.data.magnitude()

  // Earth's magnetic field is typically 25-65 μT
  // Interference if significantly outside this range
  mag < 20.0 || mag > 70.0
}

///|
/// Get field strength
pub fn Magnetometer::get_field_strength(self : Magnetometer) -> Double {
  self.last_reading.data.magnitude()
}

///|
/// Calculate magnetic declination offset
pub fn Magnetometer::apply_declination(
  self : Magnetometer,
  declination : Double,
) -> Double {
  let heading = self.get_heading()
  heading + declination
}