//
// lsm6ds3tr
//
use core::convert::TryFrom;
use embedded_hal::i2c::{ErrorKind, ErrorType};

use crate::lsm6ds::Error::CommunicationError;
use embedded_hal_async::i2c::I2c;
// use log::info;

/// Enum containing all possible types of errors when interacting with the IMU
#[derive(Debug)]
pub enum Error {
    CommunicationError,
    ChipDetectFailed,
    RegisterReadFailed,
}

// Value of the WHO_AM_I register
const CHIP_ID: u8 = 0x6a;

// Earth gravity constant for acceleration conversion
const EARTH_GRAVITY: f32 = 9.80665;

/// 6-DoF IMU accelerometer + gyro
pub struct Lsm6ds33<I2C> {
    i2c: I2C,
    addr: u8,
    accelerometer_scale: Option<AccelerometerScale>,
    gyroscope_scale: Option<GyroscopeFullScale>,
}

impl<I2C, I2cError> Lsm6ds33<I2C>
where
    I2C: I2c<Error = I2cError>,
{
    /// Create an instance of the Lsm6ds33 driver
    /// If the device cannot be detected on the bus, an error will be returned
    pub async fn new(i2c: I2C, addr: u8) -> Result<Self, (I2C, Error)> {
        let mut lsm = Lsm6ds33 {
            i2c,
            addr,
            accelerometer_scale: None,
            gyroscope_scale: None,
        };

        match lsm.check().await {
            Ok(true) => match lsm.set_auto_increment(true).await {
                Ok(()) => Ok(lsm),
                Err(e) => Err((lsm.release(), e)),
            },
            Ok(false) => Err((lsm.release(), Error::ChipDetectFailed)),
            Err(e) => Err((lsm.release(), e)),
        }
    }

    /// Set the accelerometer output rate
    pub async fn set_accelerometer_output(
        &mut self,
        output: AccelerometerOutput,
    ) -> Result<(), Error> {
        self.write_register_option(Register::Ctrl1XL, output).await
    }

    /// Set the accelerometer operating range
    pub async fn set_accelerometer_scale(
        &mut self,
        scale: AccelerometerScale,
    ) -> Result<(), Error> {
        match self.write_register_option(Register::Ctrl1XL, scale).await {
            Ok(()) => {
                self.accelerometer_scale = Some(scale);
                Ok(())
            }
            Err(e) => {
                self.accelerometer_scale = None;
                Err(e)
            }
        }
    }

    /// Set the accelerometer bandwidth filter
    pub async fn set_accelerometer_bandwidth(
        &mut self,
        bandwidth: AccelerometerBandwidth,
    ) -> Result<(), Error> {
        self.write_register_option(Register::Ctrl1XL, bandwidth)
            .await
    }

    /// Set the gyroscope output rate
    pub async fn set_gyroscope_output(&mut self, output: GyroscopeOutput) -> Result<(), Error> {
        self.write_register_option(Register::Ctrl2G, output).await
    }

    /// Set the gyroscope operating range
    pub async fn set_gyroscope_scale(&mut self, scale: GyroscopeFullScale) -> Result<(), Error> {
        match self.write_register_option(Register::Ctrl2G, scale).await {
            Ok(()) => {
                self.gyroscope_scale = Some(scale);
                Ok(())
            }
            Err(e) => {
                self.accelerometer_scale = None;
                Err(e)
            }
        }
    }

    /// Set the low power mode
    pub async fn set_low_power_mode(&mut self, low_power: bool) -> Result<(), Error> {
        // N.B. "1" means low-power, "0" means high-performance.
        self.write_bit(
            Register::Ctrl6C,
            low_power as u8,
            Ctrl6C::AccelHighPerformanceMode as u8,
        )
        .await?;
        self.write_bit(
            Register::Ctrl7G,
            low_power as u8,
            Ctrl7G::HighPerformanceMode as u8,
        )
        .await
    }

    /// Read the gyroscope data for each axis (RAD/s)
    pub async fn read_gyro(&mut self) -> Result<(f32, f32, f32), Error> {
        let scale = self.read_gyroscope_scale().await?;
        self.read_registers(Register::OutXLG)
            .await
            .map(|res| Self::convert_gyro_data(&res, scale))
    }

    fn convert_gyro_data(data: &[u8; 6], scale: GyroscopeFullScale) -> (f32, f32, f32) {
        // Convert raw data to float
        let (x, y, z) = Self::u8_to_f32(data);
        let scale = scale.scale();
        // Convert to RAD/s (Raw gyro data is in milli-degrees per second per bit)
        (
            (x * scale / 1000.0).to_radians(),
            (y * scale / 1000.0).to_radians(),
            (z * scale / 1000.0).to_radians(),
        )
    }

    /// Read the accelerometer data for each axis (m/s^2)
    pub async fn read_accelerometer(&mut self) -> Result<(f32, f32, f32), Error> {
        let scale = self.read_accelerometer_scale().await?;
        self.read_registers(Register::OutXLXL)
            .await
            .map(|res| Self::convert_accel_data(&res, scale))
    }

    fn convert_accel_data(data: &[u8; 6], scale: AccelerometerScale) -> (f32, f32, f32) {
        // Convert raw values to float
        let (x, y, z) = Self::u8_to_f32(data);
        let scale = scale.scale();

        // Convert to m/s^2 (Raw value is in mg/bit)
        (
            (x * scale / 1000.0) * EARTH_GRAVITY,
            (y * scale / 1000.0) * EARTH_GRAVITY,
            (z * scale / 1000.0) * EARTH_GRAVITY,
        )
    }

    /// Read the temperature (degC)
    pub async fn read_temperature(&mut self) -> Result<f32, Error> {
        let data = self.read_registers::<2>(Register::OutTempL).await?;
        Ok(Self::convert_temp_data(&data))
    }

    fn convert_temp_data(data: &[u8; 2]) -> f32 {
        let (lo, hi) = (data[0], data[1]);
        // Raw temperature as signal 16-bit number
        let temperature = ((hi as i16) << 8) | (lo as i16);

        // info!("Temperature: {:x} {:x}", hi, lo);
        // info!("Temperature: {:x}", temperature);

        // As float
        let temperature = temperature as f32;

        // Converted given the temperature sensitively value 16 bits per C
        let temperature = (temperature / 256.0) + 25.0;

        temperature
    }

    /// Check if there is new accelerometer data
    pub async fn accel_data_available(&mut self) -> Result<bool, Error> {
        self.read_status().await.map(|status| status & 0b1 != 0)
    }

    /// Check if there is new gyro scope data
    pub async fn gyro_data_available(&mut self) -> Result<bool, Error> {
        self.read_status().await.map(|status| status & 0b10 != 0)
    }

    /// Read the accelerometer scale value from the configuration register
    pub async fn read_accelerometer_scale(&mut self) -> Result<AccelerometerScale, Error> {
        match self.accelerometer_scale {
            Some(v) => Ok(v),
            None => {
                let scale = self.read_register_option(Register::Ctrl1XL).await?;
                self.accelerometer_scale = Some(scale);
                Ok(scale)
            }
        }
    }

    /// Read the gyroscope scale value from the configuration register
    pub async fn read_gyroscope_scale(&mut self) -> Result<GyroscopeFullScale, Error> {
        match self.gyroscope_scale {
            Some(v) => Ok(v),
            None => {
                let scale = self.read_register_option(Register::Ctrl2G).await?;
                self.gyroscope_scale = Some(scale);
                Ok(scale)
            }
        }
    }

    async fn check(&mut self) -> Result<bool, Error> {
        self.read_register(Register::WhoAmI).await.map(|chip_id| {
            // info!("Chip ID: {:x}", chip_id);
            return chip_id == CHIP_ID;
        })
    }

    async fn set_auto_increment(&mut self, enabled: bool) -> Result<(), Error> {
        self.write_bit(Register::Ctrl3C, enabled as u8, Ctrl3C::AutoIncrement as u8)
            .await
    }

    async fn read_status(&mut self) -> Result<u8, Error> {
        self.read_register(Register::StatusReg).await
    }

    async fn write_register_option<RO: RegisterOption>(
        &mut self,
        register: Register,
        ro: RO,
    ) -> Result<(), Error> {
        self.write_bits(register, ro.value(), RO::mask(), RO::bit_offset())
            .await
    }

    async fn read_register_option<RO: RegisterOption + TryFrom<u8>>(
        &mut self,
        register: Register,
    ) -> Result<RO, Error> {
        let value = self.read_register(register).await?;
        RO::try_from(value).map_err(|_| Error::RegisterReadFailed)
    }

    async fn write_bit(&mut self, register: Register, value: u8, shift: u8) -> Result<(), Error> {
        self.write_bits(register, value, 0x01, shift).await
    }

    async fn write_bits(
        &mut self,
        register: Register,
        new_value: u8,
        mask: u8,
        shift: u8,
    ) -> Result<(), Error> {
        let current_value = self.read_register(register).await?;
        let modified_value = (current_value & !(mask << shift)) | ((new_value & mask) << shift);
        self.write_register(register, modified_value).await
    }

    fn u8_to_f32(res: &[u8; 6]) -> (f32, f32, f32) {
        let (x, y, z) = (
            (res[0] as i16) | ((res[1] as i16) << 8),
            (res[2] as i16) | ((res[3] as i16) << 8),
            (res[4] as i16) | ((res[5] as i16) << 8),
        );
        (x as f32, y as f32, z as f32)
    }

    // Read a byte from the given register
    async fn read_register(&mut self, register: Register) -> Result<u8, Error> {
        let mut res = [0u8];
        if self
            .i2c
            .write_read(self.addr, &[register.into()], &mut res)
            .await
            .is_ok()
        {
            Ok(res[0])
        } else {
            Err(CommunicationError)
        }
    }

    async fn read_registers<const N: usize>(
        &mut self,
        start_reg: Register,
    ) -> Result<[u8; N], Error> {
        let mut res = [0u8; N];
        if self
            .i2c
            .write_read(self.addr, &[start_reg.into()], &mut res)
            .await
            .is_ok()
        {
            Ok(res)
        } else {
            Err(CommunicationError)
        }
    }

    // Write the specified value to the given register
    async fn write_register(&mut self, register: Register, value: u8) -> Result<(), Error> {
        if self
            .i2c
            .write(self.addr, &[register.into(), value])
            .await
            .is_ok()
        {
            Ok(())
        } else {
            Err(CommunicationError)
        }
    }

    /// Return the underlying I2C device
    pub fn release(self) -> I2C {
        self.i2c
    }
}

#[derive(Debug)]
pub enum RegisterError {
    ConversionError,
}

// Device registers
#[derive(Debug, Clone, Copy)]
pub enum Register {
    FifoCtrl1 = 0x06,
    FifoCtrl2 = 0x07,
    FifoCtrl3 = 0x08,
    FifoCtrl4 = 0x09,
    FifoCtrl5 = 0x0A,

    OrientCfgG = 0x0B,

    Int1Ctrl = 0x0D,
    Int2Ctrl = 0x0E,

    WhoAmI = 0x0F,

    Ctrl1XL = 0x10,
    Ctrl2G = 0x11,
    Ctrl3C = 0x12,
    Ctrl4C = 0x13,
    Ctrl5C = 0x14,
    Ctrl6C = 0x15,
    Ctrl7G = 0x16,
    Ctrl8Xl = 0x18,
    Ctrl10C = 0x19,

    WakeUpSrc = 0x1B,
    TapSrc = 0x1C,
    D6dSrc = 0x1D,

    StatusReg = 0x1E,

    OutTempL = 0x20,
    OutTempH = 0x21,

    OutXLG = 0x22,
    OutXHG = 0x23,
    OutYLG = 0x24,
    OutYHG = 0x25,
    OutZLG = 0x26,
    OutZHG = 0x27,
    OutXLXL = 0x28,
    OutXHXL = 0x29,
    OutYLXL = 0x2A,
    OutYHXL = 0x2B,
    OutZLXL = 0x2C,
    OutZHXL = 0x2D,

    FifoStatus1 = 0x3A,
    FifoStatus2 = 0x3B,
    FifoStatus3 = 0x3C,
    FifoStatus4 = 0x3D,

    FifoDataOutL = 0x3E,
    FifoDataOutH = 0x3F,

    Timestamp1Reg = 0x41,
    Timestamp2Reg = 0x42,

    StepTimestampL = 0x49,
    StepTimestampH = 0x4A,

    StepCounterL = 0x4B,
    StepCounterH = 0x4C,

    FuncSrc = 0x53,

    TapCfg = 0x58,
    TapThs6d = 0x59,

    IntDur2 = 0x5A,

    WakeUpThs = 0x5B,
    WakeUpDur = 0x5C,

    FreeFall = 0x5D,

    Md1Cfg = 0x5E,
    Md2Cfg = 0x5F,
}

impl From<Register> for u8 {
    fn from(r: Register) -> u8 {
        r as u8
    }
}

pub trait RegisterOption {
    fn value(&self) -> u8;
    fn bit_offset() -> u8;
    fn mask() -> u8;
}

// ---------------------------------------------------------------------------------------------------------------------
// --- CTRL1_XL --------------------------------------------------------------------------------------------------------
// ---------------------------------------------------------------------------------------------------------------------

#[derive(Debug, Clone, Copy)]
pub enum AccelerometerOutput {
    PowerDown = 0b0000,
    Rate13 = 0b0001,
    Rate26 = 0b0010,
    Rate52 = 0b0011,
    Rate104 = 0b0100,
    Rate208 = 0b0101,
    Rate416 = 0b0110,
    Rate833 = 0b0111,
    Rate1_66k = 0b1000,
    Rate3_33k = 0b1001,
    Rate6_66k = 0b1010,
}

impl RegisterOption for AccelerometerOutput {
    fn value(&self) -> u8 {
        *self as u8
    }
    fn mask() -> u8 {
        0xF
    }
    fn bit_offset() -> u8 {
        4
    }
}

/// Accelerometer full-scale selection
#[derive(Debug, Clone, Copy)]
pub enum AccelerometerScale {
    G02 = 0b00,
    G16 = 0b01,
    G04 = 0b10,
    G08 = 0b11,
}

impl RegisterOption for AccelerometerScale {
    fn value(&self) -> u8 {
        *self as u8
    }
    fn mask() -> u8 {
        0x03
    }
    fn bit_offset() -> u8 {
        2
    }
}

impl AccelerometerScale {
    pub fn scale(&self) -> f32 {
        match *self {
            Self::G02 => 0.061,
            Self::G04 => 0.122,
            Self::G08 => 0.244,
            Self::G16 => 0.488,
        }
    }
}

impl TryFrom<u8> for AccelerometerScale {
    type Error = RegisterError;

    fn try_from(value: u8) -> Result<Self, Self::Error> {
        let value = (value >> Self::bit_offset()) & Self::mask();
        match value {
            0b00 => Ok(Self::G02),
            0b01 => Ok(Self::G16),
            0b10 => Ok(Self::G04),
            0b11 => Ok(Self::G08),
            _ => Err(RegisterError::ConversionError),
        }
    }
}

#[derive(Debug, Clone, Copy)]
pub enum AccelerometerBandwidth {
    Freq400 = 0b00,
    Freq200 = 0b01,
    Freq100 = 0b10,
    Freq50 = 0b11,
}

impl RegisterOption for AccelerometerBandwidth {
    fn value(&self) -> u8 {
        *self as u8
    }
    fn mask() -> u8 {
        0x03
    }
    fn bit_offset() -> u8 {
        0
    }
}

// ---------------------------------------------------------------------------------------------------------------------
// --- CTRL2_G --------------------------------------------------------------------------------------------------------
// ---------------------------------------------------------------------------------------------------------------------

#[derive(Debug, Clone, Copy)]
pub enum GyroscopeOutput {
    PowerDown = 0b0000,
    Rate13 = 0b0001,
    Rate26 = 0b0010,
    Rate52 = 0b0011,
    Rate104 = 0b0100,
    Rate208 = 0b0101,
    Rate416 = 0b0110,
    Rate833 = 0b0111,
    Rate1_66k = 0b1000,
    Rate3_33k = 0b1001,
    Rate6_66k = 0b1010,
}

impl RegisterOption for GyroscopeOutput {
    fn value(&self) -> u8 {
        *self as u8
    }
    fn mask() -> u8 {
        0xF
    }
    fn bit_offset() -> u8 {
        4
    }
}

#[derive(Debug, Clone, Copy)]
pub enum GyroscopeFullScale {
    Dps125 = 0b001,
    Dps245 = 0b000,
    Dps500 = 0b010,
    Dps1000 = 0b100,
    Dps2000 = 0b110,
}

impl RegisterOption for GyroscopeFullScale {
    fn value(&self) -> u8 {
        *self as u8
    }
    fn mask() -> u8 {
        0b111
    }
    fn bit_offset() -> u8 {
        1
    }
}

impl GyroscopeFullScale {
    pub fn scale(&self) -> f32 {
        match *self {
            Self::Dps125 => 4.375,
            Self::Dps245 => 8.75,
            Self::Dps500 => 17.50,
            Self::Dps1000 => 35.0,
            Self::Dps2000 => 70.0,
        }
    }
}

impl TryFrom<u8> for GyroscopeFullScale {
    type Error = RegisterError;

    fn try_from(value: u8) -> Result<Self, Self::Error> {
        let value = (value >> Self::bit_offset()) & Self::mask();
        match value {
            0b001 => Ok(GyroscopeFullScale::Dps125),
            0b000 => Ok(GyroscopeFullScale::Dps245),
            0b010 => Ok(GyroscopeFullScale::Dps500),
            0b100 => Ok(GyroscopeFullScale::Dps1000),
            0b110 => Ok(GyroscopeFullScale::Dps2000),
            _ => Err(RegisterError::ConversionError),
        }
    }
}

// #[derive(Debug, Clone, Copy)]
// pub enum GyroscopeFullScale125Dps {
//     Disabled = 0,
//     Enabled = 1,
// }

// impl RegisterOption for GyroscopeFullScale125Dps {
//     fn value(&self) -> u8 {
//         *self as u8
//     }
//     fn mask() -> u8 {
//         0b1
//     }
//     fn bit_offset() -> u8 {
//         1
//     }
// }

// pub fn option_mask<T: RegisterOption>(opt: &T) -> u8 {
//     0x01 << opt.bit_offset()
// }

/// Bit fields for CTRL3_C
pub enum Ctrl3C {
    Boot = 7,
    BlockDataUpdate = 6,
    InterruptActivationLevel = 5,
    InterruptPadOutput = 4,
    SpiSerialInterfaceMode = 3,
    AutoIncrement = 2,
    Endian = 1,
    SoftwareReset = 0,
}

/// Bit fields for CTRL6_C
pub enum Ctrl6C {
    GyroEdgeTrigge = 7,
    GyroLevelTrigger = 6,
    GyroLevelLatched = 5,
    AccelHighPerformanceMode = 4,
}

/// Bit fields for CTRL6_G
pub enum Ctrl7G {
    HighPerformanceMode = 7,
    HighPassFilter = 6,
    SourceRegisterRounding = 3,
}