ldc3114 0.2.0

use super::*;

impl<I2C, E> Ldc3114<I2C>
where
    I2C: embedded_hal::i2c::I2c + embedded_hal::i2c::ErrorType<Error = E>,
{
    /// Creates a new driver instance for the LDC3114.
    pub fn new(i2c: I2C) -> Self {
        Self {
            i2c,
            sency0: 4,
            sency1: 4,
            sency2: 4,
            sency3: 4,
            lcdiv: 3,
        }
    }

    /// Reads the device ID.
    pub fn read_device_id(&mut self) -> Result<u8, Error<E>> {
        self.read_register(Register::DeviceIdMsb)
    }

    /// Reads the manufacturer ID.
    pub fn read_manufacturer_id(&mut self) -> Result<u16, Error<E>> {
        let mut buffer = [0; 2];
        self.i2c
            .write_read(I2C_ADDR, &[Register::ManufacturerIdLsb.addr()], &mut buffer)
            .map_err(Error::I2c)?;

        let data = u16::from_le_bytes(buffer);
        Ok(data)
    }

    /// Reads the status register.
    pub fn read_status(&mut self) -> Result<Status, Error<E>> {
        let sr = self.read_register(Register::Status)?;

        Ok(Status {
            output_status: (sr & OUT_STATUS != 0),
            chip_ready: (sr & CHIP_READY != 0),
            ready_to_write: (sr & RDY_TO_WRITE != 0),
            maximum_output_code: (sr & MAXOUT != 0),
            fsm_watchdog_error: (sr & FSM_WD != 0),
            lc_sensor_watchdog_error: (sr & LC_WD != 0),
            button_timeout: (sr & TIMEOUT != 0),
            register_integrity_bad: (sr & REGISTER_FLAG != 0),
        })
    }

    /// Checks if the registers are ready to be written.
    pub fn is_ready_to_write(&mut self) -> Result<bool, Error<E>> {
        let sr = self.read_register(Register::Status)?;
        let is_ready = (sr & RDY_TO_WRITE) != 0;
        Ok(is_ready)
    }

    /// Checks if the chip is ready after internal reset.
    pub fn is_chip_ready(&mut self) -> Result<bool, Error<E>> {
        let sr = self.read_register(Register::Status)?;
        let is_ready = (sr & CHIP_READY) != 0;
        Ok(is_ready)
    }

    /// Resets the device and register configurations.
    ///
    /// All registers will be returned to default values.
    /// Normal operation will not resume until STATUS:CHIP_READY=1.
    pub fn full_reset(&mut self) -> Result<(), Error<E>> {
        self.write_register(Register::Reset, FULL_RESET)
    }

    /// Enter configuration mode.
    ///
    /// Any device configuration changes should be made in this mode.
    pub fn config_mode(&mut self) -> Result<(), Error<E>> {
        self.write_register(Register::Reset, CONFIG_MODE)
    }

    /// Enter normal mode (exit configuration mode).
    pub fn normal_mode(&mut self) -> Result<(), Error<E>> {
        let lcdiv = self.read_register(Register::LcDivider)?;
        let scfg0 = self.read_register(Register::Sensor0Config)?;
        let scfg1 = self.read_register(Register::Sensor1Config)?;
        let scfg2 = self.read_register(Register::Sensor2Config)?;
        let scfg3 = self.read_register(Register::Sensor3Config)?;

        self.lcdiv = lcdiv & 0x07;
        self.sency0 = scfg0 & 0x1F;
        self.sency1 = scfg1 & 0x1F;
        self.sency2 = scfg2 & 0x1F;
        self.sency3 = scfg3 & 0x1F;

        self.write_register(Register::Reset, 0)
    }

    /// Reads the channel output logic states.
    pub fn read_output_logic_states(&mut self) -> Result<OutputLogicStates, Error<E>> {
        let out = self.read_register(Register::Out)?;

        Ok(OutputLogicStates {
            new_data_available: (out & DATA_RDY != 0),
            out0: (out & OUT0 != 0),
            out1: (out & OUT1 != 0),
            out2: (out & OUT2 != 0),
            out3: (out & OUT3 != 0),
        })
    }

    /// Reads the button data for the given channel.
    pub fn read_button_data(&mut self, ch: impl ChannelRegisters) -> Result<i16, Error<E>> {
        let mut buffer = [0; 2];
        self.i2c
            .write_read(I2C_ADDR, &[ch.data_lsb() as u8], &mut buffer)
            .map_err(Error::I2c)?;

        let data = i16::from_le_bytes(buffer);
        Ok(data)
    }

    /// Reads the pre-processed raw sensor data for the given channel.
    ///
    /// The value returned is given by the following formula:
    /// ```
    /// f_sensor = 30 * W * 44_000_000 / raw_data
    /// ```
    /// where
    /// ```
    /// W = 128 * (1 + SENCY_n) * (2^LCDIV)
    /// ```
    pub fn read_raw_data<T: ChannelRegisters>(&mut self, ch: T) -> Result<u32, Error<E>> {
        let mut buffer = [0; 4];
        let slice = &mut buffer[0..=2];
        self.i2c
            .write_read(I2C_ADDR, &[ch.raw_data_lsb() as u8], slice)
            .map_err(Error::I2c)?;

        let data = u32::from_le_bytes(buffer);
        if data == 0 {
            return Ok(0);
        }

        let sency = match T::CH {
            0 => self.sency0,
            1 => self.sency1,
            2 => self.sency2,
            3 => self.sency3,
            _ => unreachable!(),
        };

        let w = 128 * (1 + sency as u32) * (2 << self.lcdiv as u32);
        let fsensor = 30 * w as u64 * 44_000_000 / data as u64;
        Ok(fsensor as u32)
    }

    /// Sets up the entire device configuration.
    pub fn set_device_configuration(&mut self, config: &DeviceConfig) -> Result<(), Error<E>> {
        fn en_bits<T: ChannelRegisters>(_ch: T, mode: ChannelMode) -> u8 {
            match mode {
                ChannelMode::Disabled => 0x00,
                ChannelMode::NormalMode => T::EN_BIT,
                ChannelMode::NormalAndLowPowerMode => T::EN_BIT | T::LPEN_BIT,
            }
        }

        fn btpause_maxwin_bits<T: ChannelRegisters>(_ch: T, btpause: bool, maxwin: bool) -> u8 {
            match (btpause, maxwin) {
                (false, false) => 0x00,
                (true, false) => T::BTPAUSE_BIT,
                (false, true) => T::MAXWIN_BIT,
                (true, true) => T::BTPAUSE_BIT | T::MAXWIN_BIT,
            }
        }

        fn common_deform_bits<T: ChannelRegisters>(_ch: T, common: bool, deform: bool) -> u8 {
            match (common, deform) {
                (false, false) => 0x00,
                (true, false) => T::ANTICOM_BIT,
                (false, true) => T::ANTIDFORM_BIT,
                (true, true) => T::ANTICOM_BIT | T::ANTIDFORM_BIT,
            }
        }

        fn opol_dpol_bits<T: ChannelRegisters>(
            _ch: T,
            opol: OutputPolarity,
            dpol: DataPolarity,
        ) -> u8 {
            match (opol, dpol) {
                (OutputPolarity::ActiveLow, DataPolarity::Inverted) => 0x00,
                (OutputPolarity::ActiveHigh, DataPolarity::Inverted) => T::OPOL_BIT,
                (OutputPolarity::ActiveLow, DataPolarity::Normal) => T::DPOL_BIT,
                (OutputPolarity::ActiveHigh, DataPolarity::Normal) => T::OPOL_BIT | T::DPOL_BIT,
            }
        }

        let mut en = en_bits(Channel0, config.ch0.mode);
        en |= en_bits(Channel1, config.ch1.mode);
        en |= en_bits(Channel2, config.ch2.mode);
        en |= en_bits(Channel3, config.ch3.mode);
        self.write_register(Register::En, en)?;

        self.set_channel_gain(Channel0, config.ch0.gain)?;
        self.set_channel_gain(Channel1, config.ch1.gain)?;
        self.set_channel_gain(Channel2, config.ch2.gain)?;
        self.set_channel_gain(Channel3, config.ch3.gain)?;

        self.set_normal_scan_rate(config.scan_rate)?;
        self.set_low_power_scan_rate(config.low_power_scan_rate)?;

        let mut intpol = (config.enable_reset_of_button_baseline_tracking as u8) << 4;
        intpol |= (config.enable_button_press_detection_algorithm as u8) << 3;
        intpol |= (config.interrupt_polarity as u8) << 2;
        intpol |= ((!config.enable_button_timeout) as u8) << 1;
        intpol |= (!config.enable_max_out_check) as u8;
        self.write_register(Register::IntPol, intpol)?;

        self.set_baseline_tracking_increment_np(config.baseline_tracking_increment_np)?;
        self.set_baseline_tracking_increment_lp(config.baseline_tracking_increment_lp)?;

        let mut btpause_maxwin = btpause_maxwin_bits(
            Channel0,
            config.ch0.baseline_tracking_pause,
            config.ch0.enable_max_win_button_algorithm,
        );
        btpause_maxwin |= btpause_maxwin_bits(
            Channel1,
            config.ch1.baseline_tracking_pause,
            config.ch1.enable_max_win_button_algorithm,
        );
        btpause_maxwin |= btpause_maxwin_bits(
            Channel2,
            config.ch2.baseline_tracking_pause,
            config.ch2.enable_max_win_button_algorithm,
        );
        btpause_maxwin |= btpause_maxwin_bits(
            Channel3,
            config.ch3.baseline_tracking_pause,
            config.ch3.enable_max_win_button_algorithm,
        );
        self.write_register(Register::BtPauseMaxWin, btpause_maxwin)?;

        self.set_lc_divider(config.lc_divider)?;
        self.set_hysteresis(config.hysteresis)?;
        self.set_antitwist(config.antitwist)?;

        let mut common_deform = common_deform_bits(
            Channel0,
            config.ch0.enable_anticommon_algorithm,
            config.ch0.enable_antideform_algorithm,
        );
        common_deform |= common_deform_bits(
            Channel1,
            config.ch1.enable_anticommon_algorithm,
            config.ch1.enable_antideform_algorithm,
        );
        common_deform |= common_deform_bits(
            Channel2,
            config.ch2.enable_anticommon_algorithm,
            config.ch2.enable_antideform_algorithm,
        );
        common_deform |= common_deform_bits(
            Channel3,
            config.ch3.enable_anticommon_algorithm,
            config.ch3.enable_antideform_algorithm,
        );
        self.write_register(Register::CommonDeform, common_deform)?;

        let mut opol_dpol = opol_dpol_bits(
            Channel0,
            config.ch0.output_polarity,
            config.ch0.data_polarity,
        );
        opol_dpol |= opol_dpol_bits(
            Channel1,
            config.ch1.output_polarity,
            config.ch1.data_polarity,
        );
        opol_dpol |= opol_dpol_bits(
            Channel2,
            config.ch2.output_polarity,
            config.ch2.data_polarity,
        );
        opol_dpol |= opol_dpol_bits(
            Channel3,
            config.ch3.output_polarity,
            config.ch3.data_polarity,
        );
        self.write_register(Register::OpolDpol, opol_dpol)?;

        let mut cntsc = (config.ch3.counter_scale as u8) << 6;
        cntsc |= (config.ch2.counter_scale as u8) << 4;
        cntsc |= (config.ch1.counter_scale as u8) << 2;
        cntsc |= config.ch1.counter_scale as u8;
        self.write_register(Register::Cntsc, cntsc)?;

        self.set_sensor_config(Channel0, &config.ch0.sensor_config)?;
        self.set_sensor_config(Channel1, &config.ch1.sensor_config)?;
        self.set_sensor_config(Channel2, &config.ch2.sensor_config)?;
        self.set_sensor_config(Channel3, &config.ch3.sensor_config)?;

        self.set_fast_tracking_factor(Channel0, config.ch0.fast_tracking_factor)?;
        self.set_fast_tracking_factor(Channel3, config.ch3.fast_tracking_factor)?;

        let mut ftf1_2 = (config.ch2.fast_tracking_factor as u8) << 6;
        ftf1_2 |= (config.ch1.fast_tracking_factor as u8) << 4;
        self.write_register(Register::Ftf1_2, ftf1_2)?;

        Ok(())
    }

    /// Configures a given channel.
    pub fn configure_channel<T: ChannelRegisters>(
        &mut self,
        ch: T,
        config: &ChannelConfig,
    ) -> Result<(), Error<E>> {
        self.set_channel_mode(ch, config.mode)?;
        self.set_channel_gain(ch, config.gain)?;
        self.set_output_polarity(ch, config.output_polarity)?;
        self.set_counter_scale(ch, config.counter_scale)?;
        self.set_fast_tracking_factor(ch, config.fast_tracking_factor)?;
        self.set_data_polarity(ch, config.data_polarity)?;
        self.set_sensor_config(ch, &config.sensor_config)?;
        self.include_channel_in_max_win_algorithm(ch, config.enable_max_win_button_algorithm)?;
        self.include_channel_in_anticommon_algorithm(ch, config.enable_anticommon_algorithm)?;
        self.include_channel_in_antideform_algorithm(ch, config.enable_antideform_algorithm)?;
        self.set_baseline_tracking_pause(ch, config.baseline_tracking_pause)?;
        Ok(())
    }

    /// Sets the operating mode for the given channel.
    pub fn set_channel_mode<T: ChannelRegisters>(
        &mut self,
        _ch: T,
        mode: ChannelMode,
    ) -> Result<(), Error<E>> {
        match mode {
            ChannelMode::Disabled => {
                let bits = T::EN_BIT | T::LPEN_BIT;
                self.clear_register_bits(Register::En, bits)
            }
            ChannelMode::NormalMode => self.modify_register(Register::En, |mut v| {
                v &= !T::LPEN_BIT;
                v |= T::EN_BIT;
                v
            }),
            ChannelMode::NormalAndLowPowerMode => {
                let bits = T::EN_BIT | T::LPEN_BIT;
                self.set_register_bits(Register::En, bits)
            }
        }
    }

    /// Sets the gain for the given channel.
    pub fn set_channel_gain<T: ChannelRegisters>(
        &mut self,
        ch: T,
        gain: u8,
    ) -> Result<(), Error<E>> {
        if gain >= 0x40 {
            return Err(Error::InvalidParameter);
        }
        self.write_register(ch.gain(), gain)
    }

    /// Sets the scan rate in normal power mode.
    pub fn set_normal_scan_rate(&mut self, sr: ScanRate) -> Result<(), Error<E>> {
        self.write_register(Register::NpScanRate, sr as u8)
    }

    /// Sets the scan rate in low power mode.
    pub fn set_low_power_scan_rate(&mut self, sr: LowPowerScanRate) -> Result<(), Error<E>> {
        self.write_register(Register::LpScanRate, sr as u8)
    }

    /// Enables/disables setting MAXOUT bit if button algorithm generates codes
    /// outside maximum range.
    pub fn enable_maxout_check(&mut self, enable: bool) -> Result<(), Error<E>> {
        if enable {
            self.set_register_bits(Register::IntPol, DIS_BTB_MO)
        } else {
            self.clear_register_bits(Register::IntPol, DIS_BTB_MO)
        }
    }

    /// Enables/disables button timeout if button is pressed for more than 50 seconds.
    pub fn enable_button_timeout(&mut self, enable: bool) -> Result<(), Error<E>> {
        if enable {
            self.set_register_bits(Register::IntPol, DIS_BTN_TO)
        } else {
            self.clear_register_bits(Register::IntPol, DIS_BTN_TO)
        }
    }

    /// Sets the interrupt polarity of pin INTB.
    pub fn set_interrupt_polarity(&mut self, polarity: InterruptPolarity) -> Result<(), Error<E>> {
        match polarity {
            InterruptPolarity::ActiveLow => self.clear_register_bits(Register::IntPol, INTPOL),
            InterruptPolarity::ActiveHigh => self.set_register_bits(Register::IntPol, INTPOL),
        }
    }

    /// Enables/disables the button press detection algorithm to assert events on OUT_X pins.
    pub fn enable_button_press_detection_algorithm(
        &mut self,
        enable: bool,
    ) -> Result<(), Error<E>> {
        if enable {
            self.set_register_bits(Register::IntPol, BTN_ALG_EN)
        } else {
            self.clear_register_bits(Register::IntPol, BTN_ALG_EN)
        }
    }

    /// Enables/disables reset of button algorithm baseline tracking value.
    pub fn enable_reset_of_button_baseline_tracking(
        &mut self,
        enable: bool,
    ) -> Result<(), Error<E>> {
        if enable {
            self.set_register_bits(Register::IntPol, BTSRT_EN)
        } else {
            self.clear_register_bits(Register::IntPol, BTSRT_EN)
        }
    }

    /// Sets the baseline tracking increment in normal power mode.
    pub fn set_baseline_tracking_increment_np(&mut self, value: u8) -> Result<(), Error<E>> {
        if value >= 0x08 {
            return Err(Error::InvalidParameter);
        }
        self.write_register(Register::NpBaseInc, value)
    }

    /// Sets the baseline tracking increment in low power mode.
    pub fn set_baseline_tracking_increment_lp(&mut self, value: u8) -> Result<(), Error<E>> {
        if value >= 0x08 {
            return Err(Error::InvalidParameter);
        }
        self.write_register(Register::LpBaseInc, value)
    }

    /// Configures baseline tracking to pause or not for the given channel
    /// when its corresponding OUT pin is asserted.
    pub fn set_baseline_tracking_pause<T: ChannelRegisters>(
        &mut self,
        _ch: T,
        pause: bool,
    ) -> Result<(), Error<E>> {
        if pause {
            self.set_register_bits(Register::BtPauseMaxWin, T::BTPAUSE_BIT)
        } else {
            self.clear_register_bits(Register::BtPauseMaxWin, T::BTPAUSE_BIT)
        }
    }

    /// Configures whether to include or exclude the given channel
    /// from the Max-Win Button algorithm.
    pub fn include_channel_in_max_win_algorithm<T: ChannelRegisters>(
        &mut self,
        _ch: T,
        include: bool,
    ) -> Result<(), Error<E>> {
        if include {
            self.set_register_bits(Register::BtPauseMaxWin, T::MAXWIN_BIT)
        } else {
            self.clear_register_bits(Register::BtPauseMaxWin, T::MAXWIN_BIT)
        }
    }

    /// Sets the LC oscillation frequency divider.
    pub fn set_lc_divider(&mut self, value: u8) -> Result<(), Error<E>> {
        if value >= 0x08 {
            return Err(Error::InvalidParameter);
        }
        self.write_register(Register::LcDivider, value)
    }

    /// Hysteresis for threshold for button algorithm.
    pub fn set_hysteresis(&mut self, value: u8) -> Result<(), Error<E>> {
        if value >= 0x10 {
            return Err(Error::InvalidParameter);
        }
        self.write_register(Register::Hyst, value)
    }

    /// Sets the anti-twist threshold value for the anti-twist button algorithm.
    pub fn set_antitwist(&mut self, value: u8) -> Result<(), Error<E>> {
        if value >= 0x08 {
            return Err(Error::InvalidParameter);
        }
        self.write_register(Register::Twist, value)
    }

    /// Configures whether to include or exclude the given channel
    /// from the Anti-Common Button algorithm.
    pub fn include_channel_in_anticommon_algorithm<T: ChannelRegisters>(
        &mut self,
        _ch: T,
        include: bool,
    ) -> Result<(), Error<E>> {
        if include {
            self.set_register_bits(Register::CommonDeform, T::ANTICOM_BIT)
        } else {
            self.clear_register_bits(Register::CommonDeform, T::ANTICOM_BIT)
        }
    }

    /// Configures whether to include or exclude the given channel
    /// from the Anti-Deform Button algorithm.
    pub fn include_channel_in_antideform_algorithm<T: ChannelRegisters>(
        &mut self,
        _ch: T,
        include: bool,
    ) -> Result<(), Error<E>> {
        if include {
            self.set_register_bits(Register::CommonDeform, T::ANTIDFORM_BIT)
        } else {
            self.clear_register_bits(Register::CommonDeform, T::ANTIDFORM_BIT)
        }
    }

    /// Sets the output polarity of the given channel.
    pub fn set_output_polarity<T: ChannelRegisters>(
        &mut self,
        _ch: T,
        polarity: OutputPolarity,
    ) -> Result<(), Error<E>> {
        match polarity {
            OutputPolarity::ActiveLow => self.clear_register_bits(Register::OpolDpol, T::OPOL_BIT),
            OutputPolarity::ActiveHigh => self.set_register_bits(Register::OpolDpol, T::OPOL_BIT),
        }
    }

    /// Sets the data polarity of the given channel.
    pub fn set_data_polarity<T: ChannelRegisters>(
        &mut self,
        _ch: T,
        polarity: DataPolarity,
    ) -> Result<(), Error<E>> {
        match polarity {
            DataPolarity::Inverted => self.clear_register_bits(Register::OpolDpol, T::DPOL_BIT),
            DataPolarity::Normal => self.set_register_bits(Register::OpolDpol, T::DPOL_BIT),
        }
    }

    /// Sets the counter scale for the given channel.
    pub fn set_counter_scale<T: ChannelRegisters>(
        &mut self,
        _ch: T,
        scale: CounterScale,
    ) -> Result<(), Error<E>> {
        self.modify_register(Register::Cntsc, |mut v| {
            v &= !T::CNTSC_MASK;
            v | (scale as u8) << T::CNTSC_OFFSET
        })
    }

    /// Sets the sensor configuration for the given channel.
    pub fn set_sensor_config<T: ChannelRegisters>(
        &mut self,
        ch: T,
        config: &SensorConfig,
    ) -> Result<(), Error<E>> {
        if config.cycle_count >= 0x20 {
            return Err(Error::InvalidParameter);
        }

        let mut value = config.cycle_count;
        value |= config.rp_range as u8;
        value |= config.frequency_range as u8;

        self.write_register(ch.sensor_config(), value)
    }

    /// Sets the Fast Tracking Factor (FTF) for the given channel.
    pub fn set_fast_tracking_factor<T: ChannelRegisters>(
        &mut self,
        ch: T,
        ftf: FastTrackingFactor,
    ) -> Result<(), Error<E>> {
        self.modify_register(ch.ftf(), |mut v| {
            v &= !T::FTF_MASK;
            v | (ftf as u8) << T::FTF_OFFSET
        })
    }

    /// Writes a value to a given register.
    pub fn write_register(&mut self, register: Register, value: u8) -> Result<(), Error<E>> {
        if register.is_read_only() {
            return Err(Error::WriteToReadOnly);
        }

        self.i2c
            .write(I2C_ADDR, &[register.addr(), value])
            .map_err(Error::I2c)?;
        Ok(())
    }

    /// Reads a value from a given register.
    pub fn read_register(&mut self, register: Register) -> Result<u8, Error<E>> {
        let mut buffer = [0u8; 1];
        self.i2c
            .write_read(I2C_ADDR, &[register.addr()], &mut buffer)
            .map_err(Error::I2c)?;
        Ok(buffer[0])
    }

    /// Modifies the value of a given register.
    pub fn modify_register<F>(&mut self, register: Register, f: F) -> Result<(), Error<E>>
    where
        F: FnOnce(u8) -> u8,
    {
        let value = self.read_register(register)?;
        self.write_register(register, f(value))
    }

    /// Sets some bits of a given register.
    pub fn set_register_bits(&mut self, register: Register, bits: u8) -> Result<(), Error<E>> {
        self.modify_register(register, |v| v | bits)
    }

    /// Clears some bits of a given register.
    pub fn clear_register_bits(&mut self, register: Register, bits: u8) -> Result<(), Error<E>> {
        self.modify_register(register, |v| v & !bits)
    }
}