bmp581 0.1.0

use embedded_hal::delay;
use crate::{interface::{I2cAddr, I2cInterface, ReadData, WriteData, SpiInterface}, registers::Registers, types::{Error, Odr, PowerMode, IntConfig, IntSource, IntStatus, Status, OsrConfig, DeepDis, OdrConfig}};

/// BMP581 driver
pub struct Bmp581<I> {
    interface: I,
}

impl<I2C> Bmp581<I2cInterface<I2C>>
{
    /// Creates a new BMP581 device with I2C communication
    pub fn new_i2c(i2c: I2C, address: I2cAddr) -> Self {
        Bmp581 {
            interface: I2cInterface {
                i2c, 
                address: address.addr(),
            },
        }
    }

    pub fn release(self) -> I2C {
        self.interface.i2c
    }    
}    

impl<SPI> Bmp581<SpiInterface<SPI>>
{
    /// Creates a new BMP581 device with I2C communication
    pub fn new_spi(spi: SPI) -> Self {
        Bmp581 {
            interface: SpiInterface {
                spi, 
            },
        }
    }

    pub fn release(self) -> SPI {
        self.interface.spi
    }    
}

impl<I, CommE> Bmp581<I>
where 
    I: ReadData<Error = Error<CommE>> + WriteData<Error = Error<CommE>>,  
{
    /// Initializes the device
    pub fn init<D>(&mut self, delay: &mut D) -> Result<(), Error<CommE>> 
    where 
    D: delay::DelayNs,
    {
        self.soft_reset()?;
        delay.delay_us(2000);
        let chip_id = self.get_chip_id()?;
        if chip_id != 0x50 {
            return Err(Error::InvalidChipId(chip_id));
        }
        // TODO: Check that status_nvm_rdy==1, status_nvm_err == 0, otherwise throw error
        let _status = self.get_status()?;
        // TODO: Check INT_STATUS.por register field and check that it is set to 1; that means INT_STATUS==0x10, otherwise throw error
        let _int_status = self.get_int_status()?;
        let mut odr_config = self.get_odr_config()?;
        odr_config.deep_dis = DeepDis::Disabled;
        self.set_odr_config(odr_config)?;
        Ok(())
    }

    /// Get Chip ID
    pub fn get_chip_id(&mut self) -> Result<u8, Error<CommE>> {
        self.interface.read_reg(Registers::CHIP_ID)
    }

    /// Enable/disable pressure sensor
    pub fn set_pressure_enabled(&mut self, enable: bool) -> Result<(), Error<CommE>> {
        let mut current = self.interface.read_reg(Registers::OSR_CONFIG)?;
        
        if enable {
            current |= 1 << 6;  // Set bit 6 for pressure enable
        } else {
            current &= !(1 << 6);  // Clear bit 6 to disable
        }
        
        self.interface.write_reg(Registers::OSR_CONFIG, current)
    }

    /// Perform soft reset
    pub fn soft_reset(&mut self) -> Result<(), Error<CommE>> {
        // Write reset command (0xB6) to CMD register
        self.interface.write_reg(Registers::CMD, 0xB6)
    }


    /// Get ODR configuration
    pub fn get_odr_config(&mut self) -> Result<OdrConfig, Error<CommE>> {
        let register = self.interface.read_reg(Registers::ODR_CONFIG)?;
        Ok(OdrConfig::from_reg(register))
    }

    /// Set power mode
    pub fn set_power_mode(&mut self, mode: PowerMode) -> Result<(), Error<CommE>> {
        // Read current register value
        let mut current = self.interface.read_reg(Registers::ODR_CONFIG)?;
        // Clear power mode bits [1:0]
        current &= !0x03;
        // Set new power mode in bits [1:0]
        current |= mode as u8 & 0x03;
        // Setting deep_dis = 1 disables the deep standby mode
        current |= 0b1 << 7;
        // Write back while preserving other bits
        self.interface.write_reg(Registers::ODR_CONFIG, current)
    }

    /// Set output data rate (ODR)
    pub fn set_odr(&mut self, odr: Odr) -> Result<(), Error<CommE>> {
        // Read current register value
        let mut current = self.interface.read_reg(Registers::ODR_CONFIG)?;
        // Clear ODR bits (bits [6:2])
        current &= !(0x1F << 2);
        // Set new ODR value in bits [6:2]
        current |= (odr as u8) << 2;
        // Preserve bits 7 (deep_dis) and [1:0] (power mode)
        self.interface.write_reg(Registers::ODR_CONFIG, current)
    }

    /// Set OSR configuration
    pub fn set_osr_config(&mut self, config: OsrConfig) -> Result<(), Error<CommE>> {
        self.interface.write_reg(Registers::OSR_CONFIG, config.to_reg())
    }


    /// Set ODR configuration
    pub fn set_odr_config(&mut self, config: OdrConfig) -> Result<(), Error<CommE>> {
        self.interface.write_reg(Registers::ODR_CONFIG, config.to_reg())
    }

    /// Get OSR configuration
    pub fn get_osr_config(&mut self) -> Result<OsrConfig, Error<CommE>> {
        let reg = self.interface.read_reg(Registers::OSR_CONFIG)?;
        Ok(OsrConfig::from_reg(reg))
    }

    /// Get Status
    pub fn get_status(&mut self) -> Result<Status, Error<CommE>> {
        let status = self.interface.read_reg(Registers::STATUS)?;
        Ok(Status::from_reg(status))
    }

    /// Get Interrupt Status
    pub fn get_int_status(&mut self) -> Result<IntStatus, Error<CommE>> {
        let status = self.interface.read_reg(Registers::INT_STATUS)?;
        Ok(IntStatus::from_reg(status))
    }

    /// Set Interrupt Source
    pub fn set_int_source(&mut self, source: IntSource) -> Result<(), Error<CommE>> {
        self.interface.write_reg(Registers::INT_SOURCE, source.to_reg())
    }

    /// Get Interrupt Source
    pub fn get_int_source(&mut self) -> Result<IntSource, Error<CommE>> {
        let status = self.interface.read_reg(Registers::INT_SOURCE)?;
        Ok(IntSource::from_reg(status))
    }

    /// Get Interrupt Configuration
    pub fn get_int_config(&mut self) -> Result<IntConfig, Error<CommE>> {
        let status = self.interface.read_reg(Registers::INT_CONFIG)?;
        Ok(IntConfig::from_reg(status))
    }


    /// Get FIFO configuration
    pub fn get_fifo_config(&mut self) -> Result<u8, Error<CommE>> {
        self.interface.read_reg(Registers::FIFO_CONFIG)
        // TODO: Implement FifoConfig type
    }

    /// Get FIFO Selection configuration
    pub fn get_fifo_sel_config(&mut self) -> Result<u8, Error<CommE>> {
        self.interface.read_reg(Registers::FIFO_SEL)
        // TODO: Implement FifoSel type
    }

    /// Set Interrupt Configuration
    pub fn set_int_config(&mut self, config: IntConfig) -> Result<(), Error<CommE>> {
        self.interface.write_reg(Registers::INT_CONFIG, config.to_reg())
    }

    /// Get DSP configuration
    pub fn get_dsp_config(&mut self) -> Result<u8, Error<CommE>> {
        self.interface.read_reg(Registers::DSP_CONFIG)
        // TODO: Implement DspConfig type
    }


    /// Get DSP IIR configuration
    pub fn get_dsp_iir(&mut self) -> Result<u8, Error<CommE>> {
        self.interface.read_reg(Registers::DSP_IIR)
        // TODO: Implement DspIir type
    }

    /// Read temperature in degrees Celsius
    pub fn read_temperature(&mut self) -> Result<f32, Error<CommE>> {
        let mut buffer = [Registers::TEMP_DATA_XLSB, 0, 0, 0];
        self.interface.read(&mut buffer)?;
        let raw_temp = ((buffer[3] as u32) << 16) | ((buffer[2] as u32) << 8) | (buffer[1] as u32);
        Ok((raw_temp as f32) / 65536.0)
    }

    /// Read pressure in Pascal
    pub fn read_pressure(&mut self) -> Result<f32, Error<CommE>> {
        // Read 3 bytes from Pressure XLSB (XLSB → LSB → MSB)
        let xlsb = self.interface.read_reg(Registers::PRESS_DATA_XLSB)?;
        let lsb = self.interface.read_reg(Registers::PRESS_DATA_LSB)?;
        let msb = self.interface.read_reg(Registers::PRESS_DATA_MSB)?;

        // Construct the 24-bit pressure value (Big Endian format)
        let raw_press = u32::from_be_bytes([0, msb, lsb, xlsb]);

        #[cfg(feature = "defmt")]
        defmt::debug!("Pressure: {}, {}, {}", msb, lsb, xlsb);

        // Convert to Pascal (divide by 64 as per BMP581 datasheet)
        Ok((raw_press as f32) / 64.0)
    }

}