//////////////////////////////////////////////////////////////////////////////////////////
//
//  Smart ECG Recorder — context-aware biosensor node
//
//  Copyright (c) 2025 ProtoCentral Electronics
//
//  Demonstrates the intended wearable-node use of the ST1VAFE3BX:
//
//    MONITORING state
//      - Accelerometer + MLC run at low power (12.5 Hz).
//      - MLC classifies motion intensity 0..7 every ~80 ms.
//      - When the subject has been still (MLC class ≤ STILL_THRESHOLD)
//        for STILL_REQUIRED_SEC seconds, arm the ECG path.
//      - LED blinks blue at 1 Hz; nothing on the OpenView channel.
//
//    RECORDING state
//      - chip switches to vafe-only HP mode (800 Hz).
//      - Full-rate ECG streams over OpenView at 128 Hz (Example3 format).
//      - LED solid green.
//      - After RECORDING_DURATION_SEC, stop, reload MLC, go back to
//        MONITORING. This simulates the re-arm cycle real wearables do to
//        verify the subject is still before another recording window.
//
//  Hardware: Arduino Nano 33 BLE + ProtoCentral ST1VAFE3BX breakout,
//  electrodes on AH_IN+/AH_IN-, simulator/reference ground tied to GND.
//
//  UCF source: ST's motion-intensity example — see 04.MotionIntensityMLC.
//
/////////////////////////////////////////////////////////////////////////////////////////

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

ST1VAFE3BX st1vafe3bx;

// --- State-machine tuning ---
static const uint8_t  STILL_THRESHOLD       = 1;    // MLC class treated as "still"
static const uint8_t  STILL_REQUIRED_SEC    = 5;    // must be still this long to arm
static const uint16_t RECORDING_DURATION_SEC = 30;  // how long to record each pass

// --- State ---
enum class State : uint8_t { MONITORING, RECORDING };
static State state = State::MONITORING;
static unsigned long stateEnteredMs = 0;
static uint8_t       stillCounterSec = 0;
static unsigned long lastMlcPollMs  = 0;

// --- LED pins on Nano 33 BLE (active LOW) ---
#ifndef LEDG
  #define LEDG LED_BUILTIN
  #define LEDB LED_BUILTIN
  #define LED_ACTIVE_LOW 0
#else
  #define LED_ACTIVE_LOW 1
#endif

static void ledGreen(bool on)  { digitalWrite(LEDG, LED_ACTIVE_LOW ? !on : on); }
static void ledBlue(bool on)   { digitalWrite(LEDB, LED_ACTIVE_LOW ? !on : on); }
static void ledAllOff()        { ledGreen(false); ledBlue(false); }

// --- OpenView packet format (matches Example3) ---
#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

static char dataPacket[DATA_LEN];
static const char dataPacketFooter[2] = {ZERO, CES_CMDIF_PKT_STOP};
static const char dataPacketHeader[5] = {CES_CMDIF_PKT_START_1, CES_CMDIF_PKT_START_2,
                                         DATA_LEN, ZERO, CES_CMDIF_TYPE_DATA};

static void sendEcgPacket(int32_t ecg)
{
    dataPacket[0] = ecg;
    dataPacket[1] = ecg >> 8;
    dataPacket[2] = ecg >> 16;
    dataPacket[3] = ecg >> 24;
    for (int i = 4; i < DATA_LEN; i++) dataPacket[i] = 0;
    for (int i = 0; i < 5; i++)        Serial.write(dataPacketHeader[i]);
    for (int i = 0; i < DATA_LEN; i++) Serial.write(dataPacket[i]);
    for (int i = 0; i < 2; i++)        Serial.write(dataPacketFooter[i]);
}

// --- UCF playback + MLC read ---
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++)
    {
        const ucf_line_ext_t& line = ucf[i];
        switch (line.op)
        {
            case MEMS_UCF_OP_WRITE:
                if (!st1vafe3bx.writeRegister(line.address, line.data)) return false;
                break;
            case MEMS_UCF_OP_DELAY:
                delay(line.data);
                break;
            default:
                break;  // this UCF uses only WRITE + DELAY
        }
    }
    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;
}

// --- State transitions ---
static void enterMonitoring()
{
    st1vafe3bx.disableVAFE();
    delay(10);
    loadMlcConfig();
    state = State::MONITORING;
    stateEnteredMs = millis();
    stillCounterSec = 0;
    lastMlcPollMs = 0;
    ledAllOff();
}

static void enterRecording()
{
    st1vafe3bx.configureForECG();
    // burn a few settle samples so the first OpenView packet isn't transient
    delay(10);
    for (uint8_t i = 0; i < 8; i++) (void)st1vafe3bx.readVAFE_Raw();
    state = State::RECORDING;
    stateEnteredMs = millis();
    ledAllOff();
    ledGreen(true);
}

void setup()
{
    Serial.begin(57600);  // OpenView-compatible baud
    delay(500);

    pinMode(LEDG, OUTPUT);
    pinMode(LEDB, OUTPUT);
    ledAllOff();

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

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

    enterMonitoring();
}

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

    if (state == State::MONITORING)
    {
        // LED: blink blue at 1 Hz
        ledBlue((now / 500) & 1);

        // Poll MLC once per second
        if (now - lastMlcPollMs >= 1000)
        {
            lastMlcPollMs = now;
            uint8_t cls = readMlc1();
            if (cls <= STILL_THRESHOLD) {
                if (stillCounterSec < 0xFF) stillCounterSec++;
            } else {
                stillCounterSec = 0;
            }
            if (stillCounterSec >= STILL_REQUIRED_SEC) {
                enterRecording();
            }
        }
    }
    else  // RECORDING
    {
        static unsigned long lastEcgTime = 0;
        static const unsigned long ECG_INTERVAL_US = 7812;  // ~128 Hz
        unsigned long now_us = micros();
        if (now_us - lastEcgTime >= ECG_INTERVAL_US)
        {
            lastEcgTime = now_us;
            sendEcgPacket((int32_t)st1vafe3bx.readVAFE_Raw());
        }

        // Time-boxed recording window
        if (now - stateEnteredMs >= (unsigned long)RECORDING_DURATION_SEC * 1000UL)
        {
            enterMonitoring();
        }
    }
}
