//////////////////////////////////////////////////////////////////////////////////////////
//
//  MLC demo: motion intensity classification
//
//  Copyright (c) 2025 ProtoCentral Electronics
//
//  The ST1VAFE3BX embeds a 4-tree Machine Learning Core (MLC) that can run
//  decision trees on accelerometer / vAFE features entirely on-chip. The
//  MLC is configured by playing back a Unico Configuration File (.ucf)
//  — a sequence of register writes/delays generated by ST MEMS Studio or
//  Unico from labeled training data. The result of each tree is read from
//  MLC1_SRC..MLC4_SRC.
//
//  This sketch loads the official "motion intensity" example from ST's
//  MLC repository and prints the resulting intensity class (0..7) to the
//  Serial Monitor at 1 Hz.
//
//  UCF source (MIT/ST license):
//  https://github.com/STMicroelectronics/STMems_Machine_Learning_Core/
//    tree/master/application_examples/st1vafe3bx/Motion%20intensity
//
//  Classes:
//    0 = still / tiny motion
//    1..6 = increasing intensity (walking → running → shaking)
//    7 = maximum intensity
//
//  To plug in your own MLC model, replace st1vafe3bx_motion_intensity.h
//  with a .h exported from MEMS Studio for the ST1VAFE3BX.
//
//  This software is licensed under the MIT License (http://opensource.org/licenses/MIT)
//
/////////////////////////////////////////////////////////////////////////////////////////

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

ST1VAFE3BX st1vafe3bx;

// MLC1_SRC (0x34) lives on the EMBEDDED-FUNCTION memory bank, not the
// main page. We switch pages via FUNC_CFG_ACCESS (0x3F) bit 7, read the
// result, then switch back.
static const uint8_t MLC1_SRC_ADDR        = 0x34;
static const uint8_t FUNC_CFG_ACCESS_ADDR = 0x3F;
static const uint8_t EMB_FUNC_ACCESS_BIT  = 0x80;

static bool readMLC1(uint8_t* out)
{
    if (!st1vafe3bx.writeRegister(FUNC_CFG_ACCESS_ADDR, EMB_FUNC_ACCESS_BIT)) return false;
    bool ok = st1vafe3bx.readRegister(MLC1_SRC_ADDR, out);
    st1vafe3bx.writeRegister(FUNC_CFG_ACCESS_ADDR, 0x00);
    return ok;
}

static bool loadUcf(const ucf_line_ext_t* ucf, size_t n)
{
    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;
            case MEMS_UCF_OP_POLL_SET:
            {
                uint8_t v = 0;
                for (uint16_t t = 0; t < 1000; t++)
                {
                    if (!st1vafe3bx.readRegister(line.address, &v)) return false;
                    if ((v & line.data) == line.data) break;
                    delay(1);
                }
                break;
            }
            case MEMS_UCF_OP_POLL_RESET:
            {
                uint8_t v = 0;
                for (uint16_t t = 0; t < 1000; t++)
                {
                    if (!st1vafe3bx.readRegister(line.address, &v)) return false;
                    if ((v & line.data) == 0) break;
                    delay(1);
                }
                break;
            }
            case MEMS_UCF_OP_READ:
                // Not used in this UCF; ignore.
                break;
            default:
                return false;
        }
    }
    return true;
}

static const char* intensityLabel(uint8_t mlc)
{
    switch (mlc) {
        case 0: return "0 - still";
        case 1: return "1 - very light";
        case 2: return "2 - light";
        case 3: return "3 - low";
        case 4: return "4 - moderate";
        case 5: return "5 - high";
        case 6: return "6 - very high";
        case 7: return "7 - max";
        default: return "?";
    }
}

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

    Serial.println("ST1VAFE3BX MLC demo - motion intensity");
    Serial.println("======================================");

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

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

    Serial.print("Loading MLC config (");
    Serial.print((unsigned)(sizeof(st1vafe3bx_motion_intensity) / sizeof(ucf_line_ext_t)));
    Serial.println(" UCF lines) ...");

    if (!loadUcf(st1vafe3bx_motion_intensity,
                 sizeof(st1vafe3bx_motion_intensity) / sizeof(ucf_line_ext_t)))
    {
        Serial.println("UCF load FAILED");
        while (1) delay(1000);
    }

    Serial.println("MLC running. Move the board to change intensity class.\n");
    Serial.println("time_ms\tMLC1\tlabel");
}

void loop()
{
    static unsigned long lastPrint = 0;
    unsigned long now = millis();
    if (now - lastPrint < 1000) return;
    lastPrint = now;

    uint8_t mlc1 = 0;
    readMLC1(&mlc1);

    Serial.print(now);
    Serial.print('\t');
    Serial.print(mlc1);
    Serial.print('\t');
    Serial.println(intensityLabel(mlc1));
}
