//////////////////////////////////////////////////////////////////////////////////////////
//
//  Arduino library for the ST1VAFE3BX Biosensor with vAFE and Accelerometer
//
//  Copyright (c) 2025 ProtoCentral Electronics
//
//  Based on the official ST platform-independent driver:
//  https://github.com/STMicroelectronics/st1vafe3bx-pid
//
//  This software is licensed under the MIT License (http://opensource.org/licenses/MIT)
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
//  INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
//  PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
//  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
//  OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
//  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
//  For information on how to use, visit https://github.com/Protocentral/protocentral_st1vafe3bx_arduino
//
/////////////////////////////////////////////////////////////////////////////////////////

#include "ProtoCentral_ST1VAFE3BX.h"

// =============================================================================
// Constructor
// =============================================================================

ST1VAFE3BX::ST1VAFE3BX(TwoWire& wire)
    : _wire(&wire)
    , _address(ST1VAFE3BX_I2C_ADDR_PRIMARY)
    , _fullScale(AccelFullScale::G_2)
    , _vafeOnly(false)
    , _vafeGain(VAFE_Gain::X4)
{
}

// =============================================================================
// Initialization
// =============================================================================

bool ST1VAFE3BX::begin(uint8_t address) {
    _address = address;
    _wire->begin();

    delay(20);  // boot

    // The first I2C transaction after a host reset can fail: if the MCU was
    // reset mid-transfer while the sensor stayed powered (e.g. fed from a Qwiic
    // 3.3V rail that doesn't power-cycle on reset), the bus comes up with the
    // sensor still holding the line. On most hosts the first (failed) transfer
    // re-clocks and releases it, so a simple retry recovers (observed on the
    // Arduino UNO R4). A fully wedged controller — notably the ESP32 with SDA
    // latched low — won't recover that way, so escalate to re-initialising the
    // I2C peripheral every few attempts. The healthy path exits on the first
    // iteration, so this costs nothing when the bus is already up.
    bool connected = false;
    for (uint8_t i = 0; i < 10; i++) {
        if (isConnected()) {
            connected = true;
            break;
        }
        // Every third failed attempt, fully re-initialise the I2C controller to
        // clear a latched bus error before retrying.
        if (i > 0 && (i % 3) == 0) {
            _wire->end();
            delay(5);
            _wire->begin();
        }
        delay(20);
    }
    if (!connected) {
        return false;
    }

    if (!reset()) {
        return false;
    }
    delay(20);

    _vafeOnly = false;

    // Enable Block Data Update
    uint8_t ctrl4;
    if (!readRegister(ST1VAFE3BX_CTRL4, &ctrl4)) return false;
    ctrl4 |= ST1VAFE3BX_CTRL4_BDU;
    if (!writeRegister(ST1VAFE3BX_CTRL4, ctrl4)) return false;

    // Enable register auto-increment for burst reads
    uint8_t ctrl1;
    if (!readRegister(ST1VAFE3BX_CTRL1, &ctrl1)) return false;
    ctrl1 |= ST1VAFE3BX_CTRL1_IF_ADD_INC;
    if (!writeRegister(ST1VAFE3BX_CTRL1, ctrl1)) return false;

    // Default accelerometer configuration
    setAccelFullScale(AccelFullScale::G_2);
    setAccelODR(AccelODR::Hz_50);

    return true;
}

bool ST1VAFE3BX::isConnected() {
    uint8_t whoami;
    if (!readRegister(ST1VAFE3BX_WHO_AM_I_REG, &whoami)) {
        return false;
    }
    return (whoami == ST1VAFE3BX_WHO_AM_I_VALUE);
}

bool ST1VAFE3BX::reset() {
    // SW reset is CTRL1.sw_reset (bit 5), self-clearing. The device NACKs all
    // I2C access while it reboots. Do NOT poll it during that window: on some
    // hardware I2C peripherals (e.g. the Renesas RA4M1 on the Arduino UNO R4)
    // a NACK latches the controller into an error state it won't recover from
    // on its own, so reads keep failing well past the real reset time and the
    // reset appears to time out. Instead, issue the reset, wait a fixed settle
    // without touching the bus, then confirm the bit cleared with one read.
    uint8_t ctrl1;
    if (!readRegister(ST1VAFE3BX_CTRL1, &ctrl1)) {
        return false;
    }
    ctrl1 |= ST1VAFE3BX_CTRL1_SW_RESET;
    if (!writeRegister(ST1VAFE3BX_CTRL1, ctrl1)) {
        return false;
    }
    delay(20);  // reset/boot settle; do not access the bus during this window
    if (!readRegister(ST1VAFE3BX_CTRL1, &ctrl1)) {
        return false;
    }
    return (ctrl1 & ST1VAFE3BX_CTRL1_SW_RESET) == 0;
}

uint8_t ST1VAFE3BX::getDeviceID() {
    uint8_t id = 0;
    readRegister(ST1VAFE3BX_WHO_AM_I_REG, &id);
    return id;
}

// =============================================================================
// Accelerometer Configuration
// =============================================================================

bool ST1VAFE3BX::setAccelODR(AccelODR odr) {
    uint8_t ctrl5;
    if (!readRegister(ST1VAFE3BX_CTRL5, &ctrl5)) {
        return false;
    }
    ctrl5 = (ctrl5 & ~ST1VAFE3BX_CTRL5_ODR_MASK) |
            (static_cast<uint8_t>(odr) << ST1VAFE3BX_CTRL5_ODR_SHIFT);
    return writeRegister(ST1VAFE3BX_CTRL5, ctrl5);
}

bool ST1VAFE3BX::setAccelFullScale(AccelFullScale fs) {
    uint8_t ctrl5;
    if (!readRegister(ST1VAFE3BX_CTRL5, &ctrl5)) {
        return false;
    }
    ctrl5 = (ctrl5 & ~ST1VAFE3BX_CTRL5_FS_MASK) | static_cast<uint8_t>(fs);
    if (!writeRegister(ST1VAFE3BX_CTRL5, ctrl5)) {
        return false;
    }
    _fullScale = fs;
    return true;
}

AccelFullScale ST1VAFE3BX::getAccelFullScale() {
    return _fullScale;
}

float ST1VAFE3BX::getAccelSensitivity() {
    // mg/LSB for 16-bit data
    switch (_fullScale) {
        case AccelFullScale::G_2:  return 0.061f;
        case AccelFullScale::G_4:  return 0.122f;
        case AccelFullScale::G_8:  return 0.244f;
        case AccelFullScale::G_16: return 0.488f;
        default: return 0.061f;
    }
}

// =============================================================================
// Accelerometer Data
// =============================================================================

bool ST1VAFE3BX::isAccelDataReady() {
    uint8_t status;
    if (!readRegister(ST1VAFE3BX_STATUS, &status)) {
        return false;
    }
    return (status & ST1VAFE3BX_STATUS_DRDY) != 0;
}

bool ST1VAFE3BX::readAccelRaw(AccelRawData* data) {
    uint8_t buffer[6];
    if (!readRegisters(ST1VAFE3BX_OUT_X_L, buffer, 6)) {
        return false;
    }
    data->x = combineBytes(buffer[0], buffer[1]);
    data->y = combineBytes(buffer[2], buffer[3]);
    data->z = combineBytes(buffer[4], buffer[5]);
    return true;
}

bool ST1VAFE3BX::readAccel(AccelData* data) {
    AccelRawData raw;
    if (!readAccelRaw(&raw)) {
        return false;
    }
    float sensitivity = getAccelSensitivity();
    data->x = raw.x * sensitivity / 1000.0f;
    data->y = raw.y * sensitivity / 1000.0f;
    data->z = raw.z * sensitivity / 1000.0f;
    return true;
}

float ST1VAFE3BX::readAccelX() {
    AccelData data;
    return readAccel(&data) ? data.x : 0.0f;
}

float ST1VAFE3BX::readAccelY() {
    AccelData data;
    return readAccel(&data) ? data.y : 0.0f;
}

float ST1VAFE3BX::readAccelZ() {
    AccelData data;
    return readAccel(&data) ? data.z : 0.0f;
}

// =============================================================================
// vAFE Configuration
// =============================================================================

bool ST1VAFE3BX::powerDown() {
    uint8_t ctrl5;
    if (!readRegister(ST1VAFE3BX_CTRL5, &ctrl5)) return false;
    ctrl5 &= ~ST1VAFE3BX_CTRL5_ODR_MASK;
    return writeRegister(ST1VAFE3BX_CTRL5, ctrl5);
}

bool ST1VAFE3BX::configureVAFE(const VAFE_Config& config) {
    // Per ST PID driver (st1vafe3bx_ah_bio_config_set / enter_vafe_only /
    // mode_set with ST1VAFE3BX_800Hz_VAFE_HP or _3200Hz_VAFE_LP):
    //
    //   1. Power down by clearing CTRL5.odr.
    //   2. Clear AH_BIO_CFG1 / CFG3 (leave HPF, self-test off).
    //   3. Write AH_BIO_CFG2 with mode/zin/gain AND ah_bio_en = 1. This is
    //      what puts the chip into vafe-only state.
    //   4. Set CTRL3.hp_en per VAFE_Mode (1 = HP_800Hz, 0 = LP_3200Hz).
    //   5. Write CTRL5 with odr = 0x0B, bw from config, fs preserved.

    if (!powerDown()) return false;
    delay(25);

    if (!writeRegister(ST1VAFE3BX_AH_BIO_CFG1, 0x00)) return false;
    if (!writeRegister(ST1VAFE3BX_AH_BIO_CFG3, 0x00)) return false;
    delay(5);

    uint8_t cfg2 = ST1VAFE3BX_AH_BIO_CFG2_EN;
    cfg2 |= (static_cast<uint8_t>(config.gain)      << ST1VAFE3BX_AH_BIO_CFG2_GAIN_SHIFT)
            & ST1VAFE3BX_AH_BIO_CFG2_GAIN_MASK;
    cfg2 |= (static_cast<uint8_t>(config.impedance) << ST1VAFE3BX_AH_BIO_CFG2_ZIN_SHIFT)
            & ST1VAFE3BX_AH_BIO_CFG2_ZIN_MASK;
    cfg2 |= (static_cast<uint8_t>(config.inputMode) << ST1VAFE3BX_AH_BIO_CFG2_MODE_SHIFT)
            & ST1VAFE3BX_AH_BIO_CFG2_MODE_MASK;
    if (!writeRegister(ST1VAFE3BX_AH_BIO_CFG2, cfg2)) return false;
    delay(10);

    uint8_t ctrl3;
    if (!readRegister(ST1VAFE3BX_CTRL3, &ctrl3)) return false;
    if (config.mode == VAFE_Mode::HP_800Hz) {
        ctrl3 |= ST1VAFE3BX_CTRL3_HP_EN;
    } else {
        ctrl3 &= ~ST1VAFE3BX_CTRL3_HP_EN;
    }
    if (!writeRegister(ST1VAFE3BX_CTRL3, ctrl3)) return false;

    uint8_t ctrl5;
    if (!readRegister(ST1VAFE3BX_CTRL5, &ctrl5)) return false;
    ctrl5 = (ctrl5 & ST1VAFE3BX_CTRL5_FS_MASK)
          | (ST1VAFE3BX_ODR_VAFE_ONLY << ST1VAFE3BX_CTRL5_ODR_SHIFT)
          | ((static_cast<uint8_t>(config.bandwidth) << ST1VAFE3BX_CTRL5_BW_SHIFT)
             & ST1VAFE3BX_CTRL5_BW_MASK);
    if (!writeRegister(ST1VAFE3BX_CTRL5, ctrl5)) return false;
    delay(10);

    _vafeOnly = true;
    _vafeGain = config.gain;
    return true;
}

bool ST1VAFE3BX::getVAFE_Config(VAFE_Config* config) {
    uint8_t cfg2;
    if (!readRegister(ST1VAFE3BX_AH_BIO_CFG2, &cfg2)) return false;

    config->gain      = static_cast<VAFE_Gain>(
        (cfg2 & ST1VAFE3BX_AH_BIO_CFG2_GAIN_MASK) >> ST1VAFE3BX_AH_BIO_CFG2_GAIN_SHIFT);
    config->impedance = static_cast<VAFE_Impedance>(
        (cfg2 & ST1VAFE3BX_AH_BIO_CFG2_ZIN_MASK) >> ST1VAFE3BX_AH_BIO_CFG2_ZIN_SHIFT);
    config->inputMode = static_cast<VAFE_InputMode>(
        (cfg2 & ST1VAFE3BX_AH_BIO_CFG2_MODE_MASK) >> ST1VAFE3BX_AH_BIO_CFG2_MODE_SHIFT);

    uint8_t ctrl3;
    if (!readRegister(ST1VAFE3BX_CTRL3, &ctrl3)) return false;
    config->mode = (ctrl3 & ST1VAFE3BX_CTRL3_HP_EN)
                   ? VAFE_Mode::HP_800Hz : VAFE_Mode::LP_3200Hz;

    uint8_t ctrl5;
    if (!readRegister(ST1VAFE3BX_CTRL5, &ctrl5)) return false;
    config->bandwidth = static_cast<VAFE_Bandwidth>(
        (ctrl5 & ST1VAFE3BX_CTRL5_BW_MASK) >> ST1VAFE3BX_CTRL5_BW_SHIFT);

    return true;
}

bool ST1VAFE3BX::configureForECG() {
    VAFE_Config config;
    config.mode      = VAFE_Mode::HP_800Hz;
    config.bandwidth = VAFE_Bandwidth::BW_3;   // 45 Hz LPF (ECG)
    config.inputMode = VAFE_InputMode::DIFFERENTIAL;
    config.impedance = VAFE_Impedance::GOHM_1;
    config.gain      = VAFE_Gain::X4;
    return configureVAFE(config);
}

bool ST1VAFE3BX::disableVAFE() {
    // Fully unwind the vafe-only configuration so the subsequent accel
    // setup starts from a clean slate. Keep CTRL3.hp_en = 1 so accel can
    // run at HP rates (up to 800 Hz) without triggering an HP↔LP
    // transition that would cost another power-down settle.
    if (!powerDown()) return false;
    delay(25);  // ST required power-down settle for ODR ≥ 50 Hz

    if (!writeRegister(ST1VAFE3BX_AH_BIO_CFG2, 0x00)) return false;
    if (!writeRegister(ST1VAFE3BX_AH_BIO_CFG1, 0x00)) return false;
    if (!writeRegister(ST1VAFE3BX_AH_BIO_CFG3, 0x00)) return false;

    // Clear BW so a subsequent setAccelODR picks a sane default.
    uint8_t ctrl5;
    if (!readRegister(ST1VAFE3BX_CTRL5, &ctrl5)) return false;
    ctrl5 &= ~ST1VAFE3BX_CTRL5_BW_MASK;
    if (!writeRegister(ST1VAFE3BX_CTRL5, ctrl5)) return false;

    _vafeOnly = false;
    return true;
}

bool ST1VAFE3BX::isVAFE_Enabled() {
    uint8_t cfg2;
    if (!readRegister(ST1VAFE3BX_AH_BIO_CFG2, &cfg2)) {
        return false;
    }
    return (cfg2 & ST1VAFE3BX_AH_BIO_CFG2_EN) != 0;
}

// =============================================================================
// vAFE Data
// =============================================================================

bool ST1VAFE3BX::isVAFE_DataReady() {
    // STATUS.drdy (bit 0) is the active-channel data-ready flag. In
    // vafe-only mode it signals new vAFE samples.
    uint8_t status;
    if (!readRegister(ST1VAFE3BX_STATUS, &status)) {
        return false;
    }
    return (status & ST1VAFE3BX_STATUS_DRDY) != 0;
}

int16_t ST1VAFE3BX::readVAFE_Raw() {
    // AH_BIO output is left-justified: 12 bits in LP mode, 14 bits in HP mode.
    // When vAFE is enabled, burst reads starting from 0x28 return zeros for
    // the accel registers, so always start the read at 0x2E.
    uint8_t buffer[2];
    if (!readRegisters(ST1VAFE3BX_OUT_AH_BIO_L, buffer, 2)) {
        return 0;
    }
    return combineBytes(buffer[0], buffer[1]);
}

float ST1VAFE3BX::readVAFE_mV() {
    int16_t raw = readVAFE_Raw();
    // Gain-dependent sensitivity, matching st1vafe3bx_from_lsb_to_mv() in the
    // ST PID driver. Values in LSB per mV at the input pin.
    float lsb_per_mv;
    switch (_vafeGain) {
        case VAFE_Gain::X2:  lsb_per_mv = 164.0f;  break;
        case VAFE_Gain::X4:  lsb_per_mv = 328.0f;  break;
        case VAFE_Gain::X8:  lsb_per_mv = 655.0f;  break;
        case VAFE_Gain::X16: lsb_per_mv = 1311.0f; break;
        default:             lsb_per_mv = 328.0f;  break;
    }
    return static_cast<float>(raw) / lsb_per_mv;
}

// =============================================================================
// vAFE Self-Test
// =============================================================================

bool ST1VAFE3BX::startVAFE_SelfTest(uint8_t step) {
    if (step != 1 && step != 2) {
        return false;
    }
    uint8_t cfg3;
    if (!readRegister(ST1VAFE3BX_AH_BIO_CFG3, &cfg3)) {
        return false;
    }
    // ST field (bits 2:1): step 1 = 0b01, step 2 = 0b10
    cfg3 = (cfg3 & ~ST1VAFE3BX_AH_BIO_CFG3_ST_MASK)
         | ((step << ST1VAFE3BX_AH_BIO_CFG3_ST_SHIFT) & ST1VAFE3BX_AH_BIO_CFG3_ST_MASK);
    return writeRegister(ST1VAFE3BX_AH_BIO_CFG3, cfg3);
}

bool ST1VAFE3BX::stopVAFE_SelfTest() {
    uint8_t cfg3;
    if (!readRegister(ST1VAFE3BX_AH_BIO_CFG3, &cfg3)) {
        return false;
    }
    cfg3 &= ~ST1VAFE3BX_AH_BIO_CFG3_ST_MASK;
    return writeRegister(ST1VAFE3BX_AH_BIO_CFG3, cfg3);
}

// =============================================================================
// Temperature
// =============================================================================

float ST1VAFE3BX::readTemperature() {
    uint8_t buffer[2];
    if (!readRegisters(ST1VAFE3BX_OUT_TEMP_L, buffer, 2)) {
        return 0.0f;
    }
    int16_t raw = combineBytes(buffer[0], buffer[1]);
    return 25.0f + (static_cast<float>(raw) / 256.0f);
}

// =============================================================================
// FIFO Operations
// =============================================================================

bool ST1VAFE3BX::setFIFO_Mode(FIFO_Mode mode) {
    uint8_t ctrl;
    if (!readRegister(ST1VAFE3BX_FIFO_CTRL, &ctrl)) return false;
    ctrl = (ctrl & 0xF8) | (static_cast<uint8_t>(mode) & 0x07);
    return writeRegister(ST1VAFE3BX_FIFO_CTRL, ctrl);
}

bool ST1VAFE3BX::setFIFO_Watermark(uint8_t watermark) {
    if (watermark > 127) watermark = 127;
    uint8_t wtm;
    if (!readRegister(ST1VAFE3BX_FIFO_WTM, &wtm)) return false;
    wtm = (wtm & 0x80) | (watermark & 0x7F);
    return writeRegister(ST1VAFE3BX_FIFO_WTM, wtm);
}

bool ST1VAFE3BX::setFIFO_XLOnly(bool xlOnly) {
    uint8_t wtm;
    if (!readRegister(ST1VAFE3BX_FIFO_WTM, &wtm)) return false;
    if (xlOnly) wtm |= 0x80;
    else        wtm &= ~0x80;
    return writeRegister(ST1VAFE3BX_FIFO_WTM, wtm);
}

bool ST1VAFE3BX::setFIFO_BatchRate(FIFO_XLBatchRate bdr) {
    uint8_t batch;
    if (!readRegister(ST1VAFE3BX_FIFO_BATCH_DEC, &batch)) return false;
    batch = (batch & ~0x07) | (static_cast<uint8_t>(bdr) & 0x07);
    return writeRegister(ST1VAFE3BX_FIFO_BATCH_DEC, batch);
}

uint8_t ST1VAFE3BX::getFIFO_Count() {
    uint8_t count = 0;
    readRegister(ST1VAFE3BX_FIFO_STATUS2, &count);
    return count;
}

bool ST1VAFE3BX::isFIFO_WatermarkReached() {
    uint8_t status;
    if (!readRegister(ST1VAFE3BX_FIFO_STATUS1, &status)) return false;
    return (status & 0x80) != 0;
}

bool ST1VAFE3BX::isFIFO_Overrun() {
    uint8_t status;
    if (!readRegister(ST1VAFE3BX_FIFO_STATUS1, &status)) return false;
    return (status & 0x40) != 0;
}

bool ST1VAFE3BX::readFIFO_Frame(FifoFrame* frame) {
    if (frame == nullptr) return false;

    frame->hasXL = false;
    frame->hasVAFE = false;
    frame->x = frame->y = frame->z = frame->vafe = 0;

    uint8_t tagReg;
    if (!readRegister(ST1VAFE3BX_FIFO_DATA_OUT_TAG, &tagReg)) return false;
    frame->tag = static_cast<FIFO_Tag>((tagReg >> 3) & 0x1F);

    uint8_t raw[6];
    if (!readRegisters(ST1VAFE3BX_FIFO_DATA_OUT_X_L, raw, 6)) return false;

    // Check whether FIFO is storing combined 12-bit frames or XL-only 16-bit.
    uint8_t wtm;
    if (!readRegister(ST1VAFE3BX_FIFO_WTM, &wtm)) return false;
    bool xlOnlyFifo = (wtm & 0x80) != 0;

    switch (frame->tag) {
        case FIFO_Tag::AH_VAFE_ONLY: {
            // 16-bit vAFE in raw[0..1], rest undefined
            frame->vafe = (int16_t)(((uint16_t)raw[1] << 8) | raw[0]);
            frame->hasVAFE = true;
            break;
        }
        case FIFO_Tag::XL_AND_AH_VAFE: {
            if (xlOnlyFifo) {
                // XL-only 16-bit per axis (this tag shouldn't really appear here)
                frame->x = (int16_t)(((uint16_t)raw[1] << 8) | raw[0]);
                frame->y = (int16_t)(((uint16_t)raw[3] << 8) | raw[2]);
                frame->z = (int16_t)(((uint16_t)raw[5] << 8) | raw[4]);
                frame->hasXL = true;
            } else {
                // Combined 12-bit XL + 12-bit vAFE packed in 6 bytes.
                // Layout (ST PID st1vafe3bx_reg.c):
                //   raw[0] = X[7:0]
                //   raw[1] = Y[3:0] | X[11:8]
                //   raw[2] = Y[11:4]
                //   raw[3] = Z[7:0]
                //   raw[4] = V[3:0] | Z[11:8]
                //   raw[5] = V[11:4]
                uint16_t x12 = ((uint16_t)(raw[1] & 0x0F) << 8) | raw[0];
                uint16_t y12 = ((uint16_t)raw[2] << 4) | (raw[1] >> 4);
                uint16_t z12 = ((uint16_t)(raw[4] & 0x0F) << 8) | raw[3];
                uint16_t v12 = ((uint16_t)raw[5] << 4) | (raw[4] >> 4);
                // Left-shift by 4 so the int16 sign bit and magnitude match
                // the direct-register left-justified format.
                frame->x    = (int16_t)(x12 << 4);
                frame->y    = (int16_t)(y12 << 4);
                frame->z    = (int16_t)(z12 << 4);
                frame->vafe = (int16_t)(v12 << 4);
                frame->hasXL = true;
                frame->hasVAFE = true;
            }
            break;
        }
        case FIFO_Tag::XL_ONLY: {
            // 16-bit per axis
            frame->x = (int16_t)(((uint16_t)raw[1] << 8) | raw[0]);
            frame->y = (int16_t)(((uint16_t)raw[3] << 8) | raw[2]);
            frame->z = (int16_t)(((uint16_t)raw[5] << 8) | raw[4]);
            frame->hasXL = true;
            break;
        }
        default:
            // Other tags carry metadata or MLC/FSM results; leave fields cleared.
            break;
    }
    return true;
}

bool ST1VAFE3BX::flushFIFO() {
    return setFIFO_Mode(FIFO_Mode::BYPASS);
}

// =============================================================================
// Low-Level Register Access
// =============================================================================

bool ST1VAFE3BX::readRegister(uint8_t reg, uint8_t* value) {
    _wire->beginTransmission(_address);
    _wire->write(reg);
    if (_wire->endTransmission(false) != 0) {
        return false;
    }
    if (_wire->requestFrom(_address, (uint8_t)1) != 1) {
        return false;
    }
    *value = _wire->read();
    return true;
}

bool ST1VAFE3BX::readRegisters(uint8_t reg, uint8_t* buffer, uint8_t length) {
    _wire->beginTransmission(_address);
    _wire->write(reg);
    if (_wire->endTransmission(false) != 0) {
        return false;
    }
    if (_wire->requestFrom(_address, length) != length) {
        return false;
    }
    for (uint8_t i = 0; i < length; i++) {
        buffer[i] = _wire->read();
    }
    return true;
}

bool ST1VAFE3BX::writeRegister(uint8_t reg, uint8_t value) {
    _wire->beginTransmission(_address);
    _wire->write(reg);
    _wire->write(value);
    return (_wire->endTransmission() == 0);
}

int16_t ST1VAFE3BX::combineBytes(uint8_t lsb, uint8_t msb) {
    return static_cast<int16_t>((msb << 8) | lsb);
}
