// src/mpu/driver.rs
//! MPU6050 driver using raw register I/O.

use embedded_hal::i2c::I2c;

/// MPU6050 register addresses
mod reg {
    pub const PWR_MGMT_1: u8 = 0x6B;
    pub const CONFIG: u8 = 0x1A;
    pub const GYRO_CONFIG: u8 = 0x1B;
    pub const ACCEL_CONFIG: u8 = 0x1C;
    pub const ACCEL_XOUT_H: u8 = 0x3B; // Start of 14-byte burst read
    pub const WHO_AM_I: u8 = 0x75;
}

/// Expected WHO_AM_I value for MPU6050
const MPU6050_WHO_AM_I: u8 = 0x68;

/// MPU6050 accelerometer full-scale range
#[derive(Clone, Copy, Debug, Default)]
pub enum AccelRange {
    #[default]
    G2 = 0,  // ±2g  -> 16384 LSB/g
    G4 = 1,  // ±4g  -> 8192 LSB/g
    G8 = 2,  // ±8g  -> 4096 LSB/g
    G16 = 3, // ±16g -> 2048 LSB/g
}

impl AccelRange {
    /// LSB per g for this range
    pub fn sensitivity(self) -> f32 {
        match self {
            AccelRange::G2 => 16384.0,
            AccelRange::G4 => 8192.0,
            AccelRange::G8 => 4096.0,
            AccelRange::G16 => 2048.0,
        }
    }
}

/// MPU6050 gyroscope full-scale range
#[derive(Clone, Copy, Debug, Default)]
pub enum GyroRange {
    #[default]
    Dps250 = 0,  // ±250°/s  -> 131 LSB/°/s
    Dps500 = 1,  // ±500°/s  -> 65.5 LSB/°/s
    Dps1000 = 2, // ±1000°/s -> 32.8 LSB/°/s
    Dps2000 = 3, // ±2000°/s -> 16.4 LSB/°/s
}

impl GyroRange {
    /// LSB per degree/second for this range
    pub fn sensitivity(self) -> f32 {
        match self {
            GyroRange::Dps250 => 131.0,
            GyroRange::Dps500 => 65.5,
            GyroRange::Dps1000 => 32.8,
            GyroRange::Dps2000 => 16.4,
        }
    }
}

/// Raw sensor data from a single read
#[derive(Clone, Copy, Debug, Default)]
pub struct RawReading {
    pub accel: [i16; 3], // X, Y, Z
    pub gyro: [i16; 3],  // X, Y, Z
    pub temp: i16,
}

/// MPU6050 driver
pub struct Mpu6050<I> {
    i2c: I,
    addr: u8,
    accel_range: AccelRange,
    gyro_range: GyroRange,
}

impl<I> Mpu6050<I> {
    /// Create a new MPU6050 driver.
    ///
    /// Default I2C address is 0x68. Use 0x69 if AD0 pin is high.
    pub fn new(i2c: I, addr: u8) -> Self {
        Self {
            i2c,
            addr,
            accel_range: AccelRange::default(),
            gyro_range: GyroRange::default(),
        }
    }

    /// Create with default address 0x68
    pub fn new_default(i2c: I) -> Self {
        Self::new(i2c, 0x68)
    }

    /// Release the I2C peripheral
    pub fn release(self) -> I {
        self.i2c
    }

    /// Get current accelerometer range
    pub fn accel_range(&self) -> AccelRange {
        self.accel_range
    }

    /// Get current gyroscope range
    pub fn gyro_range(&self) -> GyroRange {
        self.gyro_range
    }
}

impl<I, E> Mpu6050<I>
where
    I: I2c<Error = E>,
{
    /// Initialize the sensor with default configuration.
    ///
    /// - Wakes up the device (exits sleep mode)
    /// - Sets DLPF to ~44Hz bandwidth
    /// - Sets accel range to ±2g
    /// - Sets gyro range to ±250°/s
    ///
    /// Call this once after creating the driver.
    /// Consider adding a ~50ms delay after init before first read.
    pub fn init(&mut self) -> Result<(), E> {
        // Wake up (clear sleep bit in PWR_MGMT_1)
        self.write_reg(reg::PWR_MGMT_1, 0x00)?;

        // Set DLPF (Digital Low Pass Filter) to ~44Hz
        // CONFIG register, bits 2:0 = DLPF_CFG
        self.write_reg(reg::CONFIG, 0x03)?;

        // Set accelerometer range (default ±2g)
        self.write_reg(reg::ACCEL_CONFIG, (self.accel_range as u8) << 3)?;

        // Set gyroscope range (default ±250°/s)
        self.write_reg(reg::GYRO_CONFIG, (self.gyro_range as u8) << 3)?;

        Ok(())
    }

    /// Verify the device is responding and is an MPU6050.
    ///
    /// Returns true if WHO_AM_I register returns expected value.
    pub fn verify(&mut self) -> Result<bool, E> {
        let who = self.read_reg(reg::WHO_AM_I)?;
        Ok(who == MPU6050_WHO_AM_I)
    }

    /// Set accelerometer full-scale range.
    ///
    /// Takes effect immediately.
    pub fn set_accel_range(&mut self, range: AccelRange) -> Result<(), E> {
        self.accel_range = range;
        self.write_reg(reg::ACCEL_CONFIG, (range as u8) << 3)
    }

    /// Set gyroscope full-scale range.
    ///
    /// Takes effect immediately.
    pub fn set_gyro_range(&mut self, range: GyroRange) -> Result<(), E> {
        self.gyro_range = range;
        self.write_reg(reg::GYRO_CONFIG, (range as u8) << 3)
    }

    /// Read all sensor data in a single burst.
    ///
    /// This reads 14 bytes starting at ACCEL_XOUT_H:
    /// - Accelerometer X, Y, Z (6 bytes)
    /// - Temperature (2 bytes)
    /// - Gyroscope X, Y, Z (6 bytes)
    ///
    /// Returns raw values; use `convert_reading` for physical units.
    pub fn read_raw(&mut self) -> Result<RawReading, E> {
        let mut buf = [0u8; 14];
        self.i2c.write_read(self.addr, &[reg::ACCEL_XOUT_H], &mut buf)?;

        Ok(RawReading {
            accel: [
                i16::from_be_bytes([buf[0], buf[1]]),
                i16::from_be_bytes([buf[2], buf[3]]),
                i16::from_be_bytes([buf[4], buf[5]]),
            ],
            temp: i16::from_be_bytes([buf[6], buf[7]]),
            gyro: [
                i16::from_be_bytes([buf[8], buf[9]]),
                i16::from_be_bytes([buf[10], buf[11]]),
                i16::from_be_bytes([buf[12], buf[13]]),
            ],
        })
    }

    /// Convert raw reading to physical units.
    ///
    /// Returns (accel_g, gyro_dps, temp_c)
    pub fn convert_reading(&self, raw: &RawReading) -> ([f32; 3], [f32; 3], f32) {
        let accel_sens = self.accel_range.sensitivity();
        let gyro_sens = self.gyro_range.sensitivity();

        let accel_g = [
            raw.accel[0] as f32 / accel_sens,
            raw.accel[1] as f32 / accel_sens,
            raw.accel[2] as f32 / accel_sens,
        ];

        let gyro_dps = [
            raw.gyro[0] as f32 / gyro_sens,
            raw.gyro[1] as f32 / gyro_sens,
            raw.gyro[2] as f32 / gyro_sens,
        ];

        // Temperature formula from datasheet:
        // Temp in °C = (TEMP_OUT / 340.0) + 36.53
        let temp_c = (raw.temp as f32 / 340.0) + 36.53;

        (accel_g, gyro_dps, temp_c)
    }

    /// Read and convert in one call.
    ///
    /// Convenience method that combines `read_raw` and `convert_reading`.
    pub fn read(&mut self) -> Result<([f32; 3], [f32; 3], f32), E> {
        let raw = self.read_raw()?;
        Ok(self.convert_reading(&raw))
    }

    // ─────────────────────────────────────────────────────────────────
    // Low-level register access
    // ─────────────────────────────────────────────────────────────────

    fn write_reg(&mut self, reg: u8, val: u8) -> Result<(), E> {
        self.i2c.write(self.addr, &[reg, val])
    }

    fn read_reg(&mut self, reg: u8) -> Result<u8, E> {
        let mut buf = [0u8];
        self.i2c.write_read(self.addr, &[reg], &mut buf)?;
        Ok(buf[0])
    }
}