//////////////////////////////////////////////////////////////////////////////////////////
//
//  ECG acquisition demo for the ST1VAFE3BX Biosensor Breakout Board
//
//  Copyright (c) 2025 ProtoCentral Electronics
//
//  Streams ECG (in mV) and a simple heart-rate estimate as CSV to the Serial
//  Monitor. The vAFE is configured via configureForECG() (differential,
//  1 GOhm, 4x gain, HP_800Hz mode with 45 Hz LPF).
//
//  Hardware connections:
//    AH_IN+ -> ECG electrode (Left Arm / LA)
//    AH_IN- -> ECG electrode (Right Arm / RA)
//    Simulator/reference ground -> Arduino GND (common-mode return path)
//
//  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.
//
/////////////////////////////////////////////////////////////////////////////////////////

#include <Wire.h>
#include "ProtoCentral_ST1VAFE3BX.h"

ST1VAFE3BX st1vafe3bx;

static float baseline = 0.0f;
static float heartRate = 0.0f;
static const float peakThreshold = 0.5f;  // mV, above adaptive baseline
static unsigned long lastPeakTime = 0;
static bool inPeak = false;

static unsigned long lastSampleTime = 0;
static const unsigned long SAMPLE_INTERVAL_MS = 4;  // 250 Hz

static void calibrateBaseline()
{
    baseline = 0.0f;
    const int samples = 100;
    for (int i = 0; i < samples; i++)
    {
        baseline += st1vafe3bx.readVAFE_mV();
        delay(10);
    }
    baseline /= samples;
}

static void detectHeartbeat(float ecg, unsigned long now)
{
    if (ecg > peakThreshold && !inPeak)
    {
        inPeak = true;
        if (lastPeakTime > 0)
        {
            unsigned long rr = now - lastPeakTime;
            if (rr > 300 && rr < 2000)
            {
                float instantHR = 60000.0f / rr;
                heartRate = heartRate * 0.8f + instantHR * 0.2f;
            }
        }
        lastPeakTime = now;
    }
    else if (ecg < peakThreshold * 0.5f)
    {
        inPeak = false;
    }
}

void setup()
{
    Serial.begin(115200);
    while (!Serial && millis() < 3000) delay(10);

    Wire.begin();

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

    if (!st1vafe3bx.configureForECG())
    {
        Serial.println("Failed to configure vAFE for ECG.");
        while (1) delay(1000);
    }

    calibrateBaseline();

    Serial.println("Time(ms),ECG(mV),HR(BPM)");
}

void loop()
{
    unsigned long now = millis();
    if (now - lastSampleTime < SAMPLE_INTERVAL_MS) return;
    lastSampleTime = now;

    float ecgRaw = st1vafe3bx.readVAFE_mV();
    float ecg = ecgRaw - baseline;
    baseline = baseline * 0.995f + ecgRaw * 0.005f;

    detectHeartbeat(ecg, now);

    Serial.print(now);
    Serial.print(',');
    Serial.print(ecg, 3);
    Serial.print(',');
    Serial.println(heartRate, 1);
}
