//////////////////////////////////////////////////////////////////////////////////////////
//
//  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
//
/////////////////////////////////////////////////////////////////////////////////////////

#ifndef _ST1VAFE3BX_H_
#define _ST1VAFE3BX_H_

#include <Arduino.h>
#include <Wire.h>

/////////////////////////////////////////////////////////////////////////////////////////
// Important Note: Operating Modes
/////////////////////////////////////////////////////////////////////////////////////////
//
// The ST1VAFE3BX runs in one of two mutually exclusive states:
//
//   - Accelerometer mode (default after begin()): CTRL5.odr selects a standard
//     accel ODR, AH_BIO_CFG2.ah_bio_en = 0.
//
//   - vAFE-only mode: AH_BIO_CFG2.ah_bio_en = 1, CTRL5.odr = 0x0B, and
//     CTRL3.hp_en selects the sub-mode (1 = 800 Hz high-performance,
//     0 = 3200 Hz low-power). Accel output registers (0x28-0x2D) return
//     zeros; only 0x2E-0x2F carry valid data.
//
// Use configureVAFE() / configureForECG() to enter vAFE-only mode and
// disableVAFE() to leave it.
//
/////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////
// I2C Addresses
/////////////////////////////////////////////////////////////////////////////////////////

#define ST1VAFE3BX_I2C_ADDR_PRIMARY   0x21  // SA0 = VCC
#define ST1VAFE3BX_I2C_ADDR_SECONDARY 0x20  // SA0 = GND

/////////////////////////////////////////////////////////////////////////////////////////
// Device Identification
/////////////////////////////////////////////////////////////////////////////////////////

#define ST1VAFE3BX_WHO_AM_I_REG       0x0F
#define ST1VAFE3BX_WHO_AM_I_VALUE     0x48

/////////////////////////////////////////////////////////////////////////////////////////
// Register Map
/////////////////////////////////////////////////////////////////////////////////////////

// Pin and Function Control
#define ST1VAFE3BX_PIN_CTRL           0x02
// FUNC_CFG_ACCESS.emb_func_reg_access (bit 7) is the memory-bank switch:
// 0 = main page, 1 = embedded-function page (where MLC1..4_SRC, EMB_FUNC_EN_*,
// FSM_*, etc. live).
#define ST1VAFE3BX_FUNC_CFG_ACCESS    0x3F
#define ST1VAFE3BX_EMB_FUNC_ACCESS    0x80

// FIFO Configuration / Status / Data (per ST PID driver)
#define ST1VAFE3BX_FIFO_CTRL          0x15
#define ST1VAFE3BX_FIFO_WTM           0x16
#define ST1VAFE3BX_FIFO_STATUS1       0x26
#define ST1VAFE3BX_FIFO_STATUS2       0x27
#define ST1VAFE3BX_FIFO_DATA_OUT_TAG  0x40
#define ST1VAFE3BX_FIFO_DATA_OUT_X_L  0x41
#define ST1VAFE3BX_FIFO_DATA_OUT_X_H  0x42
#define ST1VAFE3BX_FIFO_DATA_OUT_Y_L  0x43
#define ST1VAFE3BX_FIFO_DATA_OUT_Y_H  0x44
#define ST1VAFE3BX_FIFO_DATA_OUT_Z_L  0x45
#define ST1VAFE3BX_FIFO_DATA_OUT_Z_H  0x46
#define ST1VAFE3BX_FIFO_BATCH_DEC     0x47

// Interrupt Configuration
#define ST1VAFE3BX_INT1_CTRL          0x0D
#define ST1VAFE3BX_INT2_CTRL          0x0E

// Control Registers
#define ST1VAFE3BX_CTRL1              0x10
#define ST1VAFE3BX_CTRL2              0x11
#define ST1VAFE3BX_CTRL3              0x12
#define ST1VAFE3BX_CTRL4              0x13
#define ST1VAFE3BX_CTRL5              0x14
#define ST1VAFE3BX_CTRL6              0x15
#define ST1VAFE3BX_CTRL7              0x16

// Status Registers
#define ST1VAFE3BX_STATUS             0x25
#define ST1VAFE3BX_ALL_INT_SRC        0x1A
#define ST1VAFE3BX_WAKE_UP_SRC        0x1B
#define ST1VAFE3BX_TAP_SRC            0x1C
#define ST1VAFE3BX_SIXD_SRC           0x1D

// Temperature Output
#define ST1VAFE3BX_OUT_TEMP_L         0x20
#define ST1VAFE3BX_OUT_TEMP_H         0x21

// Accelerometer Output (16-bit, 2's complement)
#define ST1VAFE3BX_OUT_X_L            0x28
#define ST1VAFE3BX_OUT_X_H            0x29
#define ST1VAFE3BX_OUT_Y_L            0x2A
#define ST1VAFE3BX_OUT_Y_H            0x2B
#define ST1VAFE3BX_OUT_Z_L            0x2C
#define ST1VAFE3BX_OUT_Z_H            0x2D

// vAFE / Analog Hub Bio Output (left-justified, 2's complement; 12-bit in LP
// mode, 14-bit in HP mode)
#define ST1VAFE3BX_OUT_AH_BIO_L       0x2E
#define ST1VAFE3BX_OUT_AH_BIO_H       0x2F

// vAFE / Analog Hub Configuration
#define ST1VAFE3BX_AH_BIO_CFG1        0x30
#define ST1VAFE3BX_AH_BIO_CFG2        0x31
#define ST1VAFE3BX_AH_BIO_CFG3        0x32

// Motion Detection Configuration
#define ST1VAFE3BX_TAP_CFG0           0x56
#define ST1VAFE3BX_TAP_CFG1           0x57
#define ST1VAFE3BX_TAP_CFG2           0x58
#define ST1VAFE3BX_TAP_THS_6D         0x59
#define ST1VAFE3BX_TAP_DUR            0x5A
#define ST1VAFE3BX_WAKE_UP_THS        0x5B
#define ST1VAFE3BX_WAKE_UP_DUR        0x5C
#define ST1VAFE3BX_FREE_FALL          0x5D
#define ST1VAFE3BX_MD1_CFG            0x5E
#define ST1VAFE3BX_MD2_CFG            0x5F

// User Offset Registers
#define ST1VAFE3BX_X_OFS_USR          0x73
#define ST1VAFE3BX_Y_OFS_USR          0x74
#define ST1VAFE3BX_Z_OFS_USR          0x75

/////////////////////////////////////////////////////////////////////////////////////////
// Register Bit Definitions
/////////////////////////////////////////////////////////////////////////////////////////

// CTRL1 (0x10) — bit positions per ST PID driver st1vafe3bx_ctrl1_t
//   bit 0: wu_z_en
//   bit 1: wu_y_en
//   bit 2: wu_x_en
//   bit 3: drdy_pulsed
//   bit 4: if_add_inc
//   bit 5: sw_reset      (self-clearing)
//   bit 6: int_pin_en
//   bit 7: smart_power_en
#define ST1VAFE3BX_CTRL1_DRDY_PULSED  0x08
#define ST1VAFE3BX_CTRL1_IF_ADD_INC   0x10
#define ST1VAFE3BX_CTRL1_SW_RESET     0x20

// CTRL3 (0x12) — bit positions per ST PID driver st1vafe3bx_ctrl3_t
//   bit 0: st_sign_x
//   bit 1: st_sign_y
//   bit 2: hp_en        (1 = HP mode; in vafe-only state this selects 800 Hz HP)
#define ST1VAFE3BX_CTRL3_HP_EN        0x04

// CTRL4 (0x13) — bit positions per ST PID driver st1vafe3bx_ctrl4_t
//   bit 0: boot
//   bit 1: soc
//   bit 3: fifo_en
//   bit 4: emb_func_en
//   bit 5: bdu
//   bits 7:6: inact_odr
#define ST1VAFE3BX_CTRL4_BOOT         0x01
#define ST1VAFE3BX_CTRL4_SOC          0x02
#define ST1VAFE3BX_CTRL4_FIFO_EN      0x08
#define ST1VAFE3BX_CTRL4_EMB_FUNC_EN  0x10
#define ST1VAFE3BX_CTRL4_BDU          0x20

// STATUS (0x25) — per ST PID driver, there is only one DRDY bit (bit 0). It
// reflects the active channel: accel data in accel mode, vAFE data in
// vafe-only mode.
#define ST1VAFE3BX_STATUS_DRDY        0x01

// AH_BIO_CFG2 (0x31) bit layout (matches ST PID driver)
//   bit 0   : ah_bio_en       (1 = vAFE channel enabled, chip in vafe-only state)
//   bits 2:1: ah_bio_gain     (0=2x, 1=4x, 2=8x, 3=16x)
//   bits 4:3: ah_bio_c_zin    (0=100M, 1=200M, 2=500M, 3=1G ohm)
//   bits 6:5: ah_bio_mode     (0=differential, 1=single+, 2=single-, 3=forced reset)
#define ST1VAFE3BX_AH_BIO_CFG2_EN         0x01
#define ST1VAFE3BX_AH_BIO_CFG2_GAIN_MASK  0x06
#define ST1VAFE3BX_AH_BIO_CFG2_GAIN_SHIFT 1
#define ST1VAFE3BX_AH_BIO_CFG2_ZIN_MASK   0x18
#define ST1VAFE3BX_AH_BIO_CFG2_ZIN_SHIFT  3
#define ST1VAFE3BX_AH_BIO_CFG2_MODE_MASK  0x60
#define ST1VAFE3BX_AH_BIO_CFG2_MODE_SHIFT 5

// AH_BIO_CFG3 (0x32) — per ST PID driver st1vafe3bx_ah_bio_cfg3_t
//   bit 0   : ah_bio_active (latches configuration changes while in vafe-only)
//   bits 5:4: st            (self-test: 0=off, 1=step1, 2=step2)
#define ST1VAFE3BX_AH_BIO_CFG3_ACTIVE   0x01
#define ST1VAFE3BX_AH_BIO_CFG3_ST_MASK  0x30
#define ST1VAFE3BX_AH_BIO_CFG3_ST_SHIFT 4

// CTRL5 (0x14) field positions
//   bits 7:4: odr
//   bits 3:2: bw
//   bits 1:0: fs
#define ST1VAFE3BX_CTRL5_ODR_MASK  0xF0
#define ST1VAFE3BX_CTRL5_ODR_SHIFT 4
#define ST1VAFE3BX_CTRL5_BW_MASK   0x0C
#define ST1VAFE3BX_CTRL5_BW_SHIFT  2
#define ST1VAFE3BX_CTRL5_FS_MASK   0x03

// Encoded ODR value for vAFE-only mode (same for HP_800 and LP_3200; CTRL3.hp_en
// selects the sub-mode).
#define ST1VAFE3BX_ODR_VAFE_ONLY   0x0B

/////////////////////////////////////////////////////////////////////////////////////////
// Enumerations
/////////////////////////////////////////////////////////////////////////////////////////

/**
 * @brief Accelerometer Output Data Rate (ODR, CTRL5.odr, bits 7:4)
 *
 * The ST1VAFE3BX top speed is 800 Hz. The same 4-bit value in CTRL5.odr
 * maps to a low-power (LP) rate when CTRL3.hp_en=0 and a high-performance
 * (HP) rate when CTRL3.hp_en=1; the numeric rates are identical in both
 * cases (6 / 12.5 / 25 / 50 / 100 / 200 / 400 / 800 Hz), but HP mode has
 * better noise at the cost of current draw. Values 0x01..0x03 are
 * ultra-low-power modes and are only valid with hp_en=0.
 *
 * CTRL5.odr = 0x0B with CFG2.ah_bio_en=1 puts the chip into vafe-only
 * state (see configureVAFE()). In dual mode (accel + vAFE via FIFO) use
 * one of the other rates.
 */
enum class AccelODR : uint8_t
{
    OFF      = 0x00,
    Hz_1p6_ULP  = 0x01,  // ULP: only valid with hp_en=0
    Hz_3_ULP    = 0x02,
    Hz_25_ULP   = 0x03,
    Hz_6     = 0x04,
    Hz_12p5  = 0x05,
    Hz_25    = 0x06,
    Hz_50    = 0x07,
    Hz_100   = 0x08,
    Hz_200   = 0x09,
    Hz_400   = 0x0A,
    Hz_800   = 0x0B,
};

/**
 * @brief Accelerometer Full Scale Selection
 */
enum class AccelFullScale : uint8_t
{
    G_2  = 0x00,
    G_4  = 0x01,
    G_8  = 0x02,
    G_16 = 0x03,
};

/**
 * @brief vAFE-only Operating Mode
 *
 * Selects the high-performance (800 Hz, 14-bit output) vs low-power
 * (3200 Hz, 12-bit output) sub-mode. Maps to CTRL3.hp_en.
 */
enum class VAFE_Mode : uint8_t
{
    LP_3200Hz = 0,  // Low-power, 3200 Hz ODR, 12-bit output
    HP_800Hz  = 1,  // High-performance, 800 Hz ODR, 14-bit output
};

/**
 * @brief vAFE Bandwidth (CTRL5.bw, meaning depends on VAFE_Mode)
 *
 * In HP_800Hz mode:  BW_0=360Hz, BW_1=180Hz, BW_2=90Hz,  BW_3=45Hz
 * In LP_3200Hz mode: BW_0=1600Hz, BW_1=700Hz, BW_2=360Hz, BW_3=180Hz
 */
enum class VAFE_Bandwidth : uint8_t
{
    BW_0 = 0,  // widest
    BW_1 = 1,
    BW_2 = 2,
    BW_3 = 3,  // narrowest (best for ECG in HP mode: 45 Hz)
};

/**
 * @brief vAFE Input Mode Selection (AH_BIO_CFG2 bits 6:5)
 */
enum class VAFE_InputMode : uint8_t
{
    DIFFERENTIAL     = 0x00,
    SINGLE_VIN_PLUS  = 0x01,
    SINGLE_VIN_MINUS = 0x02,
    FORCED_RESET     = 0x03,
};

/**
 * @brief vAFE Input Impedance Selection (AH_BIO_CFG2 bits 4:3)
 */
enum class VAFE_Impedance : uint8_t
{
    MOHM_100 = 0x00,
    MOHM_200 = 0x01,
    MOHM_500 = 0x02,
    GOHM_1   = 0x03,
};

/**
 * @brief vAFE Gain Selection (AH_BIO_CFG2 bits 2:1)
 */
enum class VAFE_Gain : uint8_t
{
    X2  = 0x00,
    X4  = 0x01,
    X8  = 0x02,
    X16 = 0x03,
};

/**
 * @brief FIFO Operation Mode (FIFO_CTRL.fifo_mode, bits 2:0)
 */
enum class FIFO_Mode : uint8_t
{
    BYPASS           = 0x00,  // FIFO disabled
    FIFO             = 0x01,  // fills once, stops when full
    STREAM_TO_FIFO   = 0x03,  // stream, freezes on trigger
    BYPASS_TO_STREAM = 0x04,  // starts bypassed, switches to stream on trigger
    STREAM           = 0x06,  // continuous, newest overwrites oldest
    BYPASS_TO_FIFO   = 0x07,  // starts bypassed, switches to FIFO on trigger
    OFF              = 0x08,  // FIFO gated off (low power)
};

/**
 * @brief Accelerometer batch rate for FIFO (FIFO_BATCH_DEC.bdr_xl, bits 2:0)
 *
 * Relative to the chip ODR (in vafe-only state, ODR = 800 Hz HP or 3200 Hz LP).
 * OFF disables accel batching (FIFO contains only vAFE frames).
 */
enum class FIFO_XLBatchRate : uint8_t
{
    ODR        = 0x00,
    ODR_DIV_2  = 0x01,
    ODR_DIV_4  = 0x02,
    ODR_DIV_8  = 0x03,
    ODR_DIV_16 = 0x04,
    ODR_DIV_32 = 0x05,
    ODR_DIV_64 = 0x06,
    OFF        = 0x07,
};

/**
 * @brief FIFO frame sensor tag (5-bit tag_sensor in FIFO_DATA_OUT_TAG bits 7:3)
 */
enum class FIFO_Tag : uint8_t
{
    EMPTY              = 0x00,
    XL_ONLY            = 0x02,
    XL_ONLY_2X         = 0x03,
    TIMESTAMP_CFG_CHG  = 0x04,
    STEP_COUNTER       = 0x12,
    MLC_RESULT         = 0x1A,
    MLC_FILTER         = 0x1B,
    MLC_FEATURE        = 0x1C,
    FSM_RESULT         = 0x1D,
    AH_VAFE_ONLY       = 0x1E,  // vAFE-only sample
    XL_AND_AH_VAFE     = 0x1F,  // combined XL + vAFE in one frame
};

/////////////////////////////////////////////////////////////////////////////////////////
// Data Structures
/////////////////////////////////////////////////////////////////////////////////////////

struct AccelRawData
{
    int16_t x;
    int16_t y;
    int16_t z;
};

struct AccelData
{
    float x;
    float y;
    float z;
};

struct VAFE_Config
{
    VAFE_Mode      mode;
    VAFE_Bandwidth bandwidth;
    VAFE_InputMode inputMode;
    VAFE_Impedance impedance;
    VAFE_Gain      gain;
};

/**
 * @brief Parsed FIFO frame
 *
 * Depending on the tag, `hasXL` / `hasVAFE` indicate which fields are valid.
 * XL raw values are left-justified 16-bit two's-complement. In combined
 * (XL_AND_AH_VAFE) frames when xl_only_fifo=0 the XL and vAFE values are
 * packed as 12-bit each and the parser left-shifts to produce the same
 * int16 representation the direct-register read path uses.
 */
struct FifoFrame
{
    FIFO_Tag tag;
    bool     hasXL;
    bool     hasVAFE;
    int16_t  x;
    int16_t  y;
    int16_t  z;
    int16_t  vafe;
};

/////////////////////////////////////////////////////////////////////////////////////////
// ST1VAFE3BX Class
/////////////////////////////////////////////////////////////////////////////////////////

class ST1VAFE3BX
{
public:
    ST1VAFE3BX(TwoWire& wire = Wire);

    //-----------------------------------------------------------------------------------
    // Initialization
    //-----------------------------------------------------------------------------------

    bool begin(uint8_t address = ST1VAFE3BX_I2C_ADDR_PRIMARY);
    bool isConnected();
    bool reset();
    uint8_t getDeviceID();

    //-----------------------------------------------------------------------------------
    // Accelerometer Configuration (accelerometer mode only)
    //-----------------------------------------------------------------------------------

    bool setAccelODR(AccelODR odr);
    bool setAccelFullScale(AccelFullScale fs);
    AccelFullScale getAccelFullScale();

    //-----------------------------------------------------------------------------------
    // Accelerometer Data
    //-----------------------------------------------------------------------------------

    bool isAccelDataReady();
    bool readAccelRaw(AccelRawData* data);
    bool readAccel(AccelData* data);
    float readAccelX();
    float readAccelY();
    float readAccelZ();

    //-----------------------------------------------------------------------------------
    // vAFE Configuration
    //
    // configureVAFE() powers the chip down, writes the AH_BIO_CFG registers,
    // sets CTRL3.hp_en and CTRL5 odr/bw, and enters vafe-only state. After
    // this call the accelerometer output is not usable until disableVAFE()
    // is called.
    //-----------------------------------------------------------------------------------

    bool configureVAFE(const VAFE_Config& config);
    bool getVAFE_Config(VAFE_Config* config);
    bool configureForECG();
    bool disableVAFE();
    bool isVAFE_Enabled();

    //-----------------------------------------------------------------------------------
    // Note: the ST1VAFE3BX cannot stream raw accelerometer and vAFE samples
    // simultaneously. Setting CFG2.ah_bio_en=1 forces the chip into
    // vafe-only state regardless of the configured ODR, and the accel
    // output is physically off. The FIFO_Tag::XL_AND_AH_VAFE combined
    // frame exists in the register map (inherited from the ST1VAFE6AX
    // architecture) but the 3BX never emits it — empirically verified.
    //
    // To present both streams, toggle modes (see Example4-ecg-accel-openview).
    //-----------------------------------------------------------------------------------

    //-----------------------------------------------------------------------------------
    // vAFE Data
    //-----------------------------------------------------------------------------------

    bool isVAFE_DataReady();
    int16_t readVAFE_Raw();
    float readVAFE_mV();

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

    bool startVAFE_SelfTest(uint8_t step);  // step: 1 or 2
    bool stopVAFE_SelfTest();

    //-----------------------------------------------------------------------------------
    // Temperature
    //-----------------------------------------------------------------------------------

    float readTemperature();

    //-----------------------------------------------------------------------------------
    // FIFO
    //
    // Typical dual-channel streaming flow (see Example4-ecg-accel-openview):
    //   configureForECG();                        // vafe-only state
    //   setFIFO_XLOnly(false);                    // combined XL+vAFE frames
    //   setFIFO_BatchRate(FIFO_XLBatchRate::ODR); // XL batched at vAFE ODR
    //   setFIFO_Mode(FIFO_Mode::STREAM);          // continuous
    //   ... loop:
    //     while (getFIFO_Count()) {
    //         FifoFrame f; readFIFO_Frame(&f);
    //         dispatch on f.tag
    //     }
    //-----------------------------------------------------------------------------------

    bool setFIFO_Mode(FIFO_Mode mode);
    bool setFIFO_Watermark(uint8_t watermark);   // 0..127 samples
    bool setFIFO_XLOnly(bool xlOnly);            // FIFO_WTM.xl_only_fifo
    bool setFIFO_BatchRate(FIFO_XLBatchRate bdr); // FIFO_BATCH_DEC.bdr_xl
    uint8_t getFIFO_Count();                     // FIFO_STATUS2.fss, unread samples
    bool isFIFO_WatermarkReached();              // FIFO_STATUS1.fifo_wtm_ia
    bool isFIFO_Overrun();                       // FIFO_STATUS1.fifo_ovr_ia
    bool readFIFO_Frame(FifoFrame* frame);
    bool flushFIFO();                            // sets BYPASS mode

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

    bool readRegister(uint8_t reg, uint8_t* value);
    bool readRegisters(uint8_t reg, uint8_t* buffer, uint8_t length);
    bool writeRegister(uint8_t reg, uint8_t value);

private:
    TwoWire* _wire;
    uint8_t _address;
    AccelFullScale _fullScale;
    bool _vafeOnly;
    VAFE_Gain _vafeGain;

    float getAccelSensitivity();
    int16_t combineBytes(uint8_t lsb, uint8_t msb);
    bool powerDown();
};

#endif // _ST1VAFE3BX_H_
