//////////////////////////////////////////////////////////////////////////////////////////
//
//  Heart Rate on Stillness — text log demo
//
//  Copyright (c) 2025 ProtoCentral Electronics
//
//  Minimal-output variant of the smart recorder. Runs MLC motion
//  classification in the background; when the subject is still for
//  STILL_REQUIRED_SEC seconds, briefly switches to ECG, measures heart
//  rate over MEASURE_DURATION_SEC, and reports:
//
//     [t=123456 ms] still → HR = 72 BPM (from 8 beats / 10.0 s)
//     [t=130456 ms] moving — activity class 4, HR not measured
//
//  Suitable for a one-line-per-reading product data log or a simple
//  LED-less demo of the chip's architecture. No OpenView, no waveform —
//  just the numeric output.
//
/////////////////////////////////////////////////////////////////////////////////////////

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

ST1VAFE3BX st1vafe3bx;

// --- Tuning ---
static const uint8_t  STILL_THRESHOLD       = 1;
static const uint8_t  STILL_REQUIRED_SEC    = 4;
static const uint16_t MEASURE_DURATION_SEC  = 10;
static const float    QRS_THRESHOLD_MV      = 0.5f;   // above baseline
static const uint16_t RR_MIN_MS             = 300;    // 200 BPM cap
static const uint16_t RR_MAX_MS             = 2000;   // 30 BPM floor

// --- UCF + MLC read (shared with 05.SmartRecorder) ---
static const uint8_t FUNC_CFG_ACCESS_ADDR = 0x3F;
static const uint8_t EMB_FUNC_ACCESS_BIT  = 0x80;
static const uint8_t MLC1_SRC_ADDR        = 0x34;

static bool loadMlcConfig()
{
    const ucf_line_ext_t* ucf = st1vafe3bx_motion_intensity;
    const size_t n = sizeof(st1vafe3bx_motion_intensity) / sizeof(ucf_line_ext_t);
    for (size_t i = 0; i < n; i++)
    {
        switch (ucf[i].op)
        {
            case MEMS_UCF_OP_WRITE:
                if (!st1vafe3bx.writeRegister(ucf[i].address, ucf[i].data)) return false;
                break;
            case MEMS_UCF_OP_DELAY:
                delay(ucf[i].data);
                break;
            default:
                break;
        }
    }
    return true;
}

static uint8_t readMlc1()
{
    uint8_t v = 0;
    st1vafe3bx.writeRegister(FUNC_CFG_ACCESS_ADDR, EMB_FUNC_ACCESS_BIT);
    st1vafe3bx.readRegister(MLC1_SRC_ADDR, &v);
    st1vafe3bx.writeRegister(FUNC_CFG_ACCESS_ADDR, 0x00);
    return v;
}

// --- HR measurement during a recording window ---
static float measureHR(float* out_avgBpm, uint8_t* out_beats, float* out_seconds)
{
    // Calibrate baseline for ~1 s
    float baseline = 0.0f;
    const int calN = 200;
    for (int i = 0; i < calN; i++)
    {
        baseline += st1vafe3bx.readVAFE_mV();
        delay(4);
    }
    baseline /= calN;

    // Measure
    unsigned long tStart = millis();
    unsigned long tEnd   = tStart + (unsigned long)MEASURE_DURATION_SEC * 1000UL;
    unsigned long lastPeakMs = 0;
    unsigned long rrSum = 0;
    uint8_t       rrCount = 0;
    uint8_t       beatCount = 0;
    bool          inPeak = false;

    while (millis() < tEnd)
    {
        float ecg = st1vafe3bx.readVAFE_mV() - baseline;
        unsigned long now = millis();

        if (ecg > QRS_THRESHOLD_MV && !inPeak)
        {
            inPeak = true;
            if (lastPeakMs > 0)
            {
                unsigned long rr = now - lastPeakMs;
                if (rr >= RR_MIN_MS && rr <= RR_MAX_MS)
                {
                    rrSum += rr;
                    rrCount++;
                }
            }
            lastPeakMs = now;
            beatCount++;
        }
        else if (ecg < QRS_THRESHOLD_MV * 0.5f)
        {
            inPeak = false;
        }
        delay(4);  // ~250 Hz sampling
    }

    *out_beats   = beatCount;
    *out_seconds = (millis() - tStart) / 1000.0f;
    *out_avgBpm  = (rrCount > 0) ? (60000.0f / (rrSum / rrCount)) : 0.0f;
    return *out_avgBpm;
}

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

    Serial.println("\nST1VAFE3BX — Heart Rate on Stillness");
    Serial.println("Waiting for subject to be still...");

    Wire.begin();
    Wire.setClock(400000);

    if (!st1vafe3bx.begin())
    {
        Serial.println("Sensor not detected!");
        while (1) delay(1000);
    }

    loadMlcConfig();
}

void loop()
{
    static unsigned long lastPollMs = 0;
    static uint8_t stillSec = 0;
    unsigned long now = millis();

    if (now - lastPollMs < 1000) return;
    lastPollMs = now;

    uint8_t cls = readMlc1();

    if (cls <= STILL_THRESHOLD)
    {
        stillSec++;
        if (stillSec >= STILL_REQUIRED_SEC)
        {
            // Arm ECG.
            st1vafe3bx.disableVAFE();  // (MLC leaves chip in a non-vafe state, this resets cleanly)
            st1vafe3bx.configureForECG();
            delay(10);
            for (uint8_t i = 0; i < 8; i++) (void)st1vafe3bx.readVAFE_Raw();

            float bpm = 0, secs = 0;
            uint8_t beats = 0;
            measureHR(&bpm, &beats, &secs);

            Serial.print("[t=");  Serial.print(millis());
            Serial.print(" ms] still → HR = ");
            if (bpm > 0) { Serial.print(bpm, 1); Serial.print(" BPM"); }
            else         { Serial.print("— (no clean beats)"); }
            Serial.print("  (");
            Serial.print(beats); Serial.print(" beats / ");
            Serial.print(secs, 1); Serial.println(" s)");

            // Go back to MLC monitoring.
            loadMlcConfig();
            stillSec = 0;
        }
        else
        {
            Serial.print("[t=");  Serial.print(now);
            Serial.print(" ms] still (");
            Serial.print(stillSec);
            Serial.print("/"); Serial.print(STILL_REQUIRED_SEC);
            Serial.println(" s)");
        }
    }
    else
    {
        stillSec = 0;
        Serial.print("[t="); Serial.print(now);
        Serial.print(" ms] moving — activity class ");
        Serial.print(cls);
        Serial.println(", HR not measured");
    }
}
