//////////////////////////////////////////////////////////////////////////////////////////
//
//  Demo code for the ST1VAFE3BX Biosensor Breakout Board
//
//  Copyright (c) 2025 ProtoCentral Electronics
//
//  This example streams ECG data in ProtoCentral OpenView format (MAX30001 compatible).
//  Use with ProtoCentral OpenView software for real-time waveform visualization.
//
//  Download OpenView: https://github.com/Protocentral/protocentral_openview
//
//  Hardware Connections:
//    VCC    -> 3.3V
//    GND    -> Ground
//    SDA    -> I2C Data
//    SCL    -> I2C Clock
//    AH_IN+ -> ECG electrode (Left Arm / LA)
//    AH_IN- -> ECG electrode (Right Arm / RA)
//
//  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 <Wire.h>
#include "ProtoCentral_ST1VAFE3BX.h"

ST1VAFE3BX st1vafe3bx;

// ProtoCentral OpenView packet format constants
#define CES_CMDIF_PKT_START_1  0x0A
#define CES_CMDIF_PKT_START_2  0xFA
#define CES_CMDIF_TYPE_DATA    0x02
#define CES_CMDIF_PKT_STOP     0x0B
#define DATA_LEN               0x0C
#define ZERO                   0

// OpenView packet buffers (same structure as MAX30001 library)
volatile char dataPacket[DATA_LEN];
const char dataPacketFooter[2] = {ZERO, CES_CMDIF_PKT_STOP};
const char dataPacketHeader[5] = {CES_CMDIF_PKT_START_1, CES_CMDIF_PKT_START_2,
                                  DATA_LEN, ZERO, CES_CMDIF_TYPE_DATA};

void setup()
{
    // OpenView uses 57600 baud
    Serial.begin(57600);

    // Wait for USB but with timeout
    unsigned long startTime = millis();
    while (!Serial && (millis() - startTime < 3000))
    {
        delay(10);
    }

    Wire.begin();

    // Initialize sensor
    if (!st1vafe3bx.begin())
    {
        Serial.println("ST1VAFE3BX not detected. Check wiring, 3.3V power, and I2C address.");
        while (1)
        {
            delay(500);
        }
    }

    // Configure vAFE for ECG: HP_800Hz mode, 45 Hz LPF, differential,
    // 1 GOhm, 4x gain. configureForECG() handles the full vafe-only init
    // sequence per the ST PID driver.
    st1vafe3bx.configureForECG();
    st1vafe3bx.stopVAFE_SelfTest();
}

// Timing for ~128 Hz sample rate
unsigned long lastSampleTime = 0;
const unsigned long SAMPLE_INTERVAL_US = 7812;  // ~128 Hz

/**
 * @brief Send data packet in ProtoCentral OpenView format
 * @param ecg_sample ECG sample value (signed 32-bit)
 * @param bioz_sample BioZ/Respiration sample value (signed 32-bit)
 * @param bioz_skip Flag indicating if BioZ sample was skipped
 */
void sendDataPacket(int32_t ecg_sample, int32_t bioz_sample, bool bioz_skip)
{
    // Pack ECG data (4 bytes, LSB first)
    dataPacket[0] = ecg_sample;
    dataPacket[1] = ecg_sample >> 8;
    dataPacket[2] = ecg_sample >> 16;
    dataPacket[3] = ecg_sample >> 24;

    // Pack BioZ data (4 bytes, LSB first)
    dataPacket[4] = bioz_sample;
    dataPacket[5] = bioz_sample >> 8;
    dataPacket[6] = bioz_sample >> 16;
    dataPacket[7] = bioz_sample >> 24;

    // BioZ skip flag
    dataPacket[8] = bioz_skip ? 0xFF : 0x00;

    // Reserved bytes
    dataPacket[9] = 0x00;
    dataPacket[10] = 0x00;
    dataPacket[11] = 0x00;

    // Send packet header
    for (int i = 0; i < 5; i++)
    {
        Serial.write(dataPacketHeader[i]);
    }

    // Send data payload
    for (int i = 0; i < DATA_LEN; i++)
    {
        Serial.write(dataPacket[i]);
    }

    // Send packet footer
    for (int i = 0; i < 2; i++)
    {
        Serial.write(dataPacketFooter[i]);
    }
}

void loop()
{
    unsigned long now = micros();

    // Fixed sample rate timing (~128 Hz)
    if (now - lastSampleTime >= SAMPLE_INTERVAL_US)
    {
        lastSampleTime = now;

        int32_t ecgSample = (int32_t)st1vafe3bx.readVAFE_Raw();

        // Send ECG on both channels for visualization
        sendDataPacket(ecgSample, ecgSample, false);
    }
}
