#include <Arduino.h>
#include "LSM6DSL.h"
 
LSM6DSL imu;
 
// Current label we’re streaming with each sample
// 0 = idle, 1 = shake
volatile uint8_t currentLabel = 0;
 
// Timing
const uint32_t SAMPLE_PERIOD_MS = 10; // ~100 Hz
uint32_t lastSampleMs = 0;
 
void handleSerialLabel() {
    while (Serial.available()) {
        char c = Serial.read();
        if (c == 'i' || c == 'I') {
            currentLabel = 0;
            Serial.println("# label=IDLE(0)");
        } else if (c == 's' || c == 'S') {
            currentLabel = 1;
            Serial.println("# label=SHAKE(1)");
        }
    }
}
 
void setup() {
    Serial.begin(115200);
    delay(2000);
 
    Serial.println("# Hello shake!");
 
    if (!imu.begin()) {
        Serial.println("# ERROR: IMU init failed");
        while (true) { delay(1000); }
    }
 
    Serial.println("# Sparrow shake dataset logger");
    Serial.println("# Send 'i' for idle label 0, 's' for shake label 1");
    Serial.println("# CSV: millis,ax_g,ay_g,az_g,label");
}
 
void loop() {
    handleSerialLabel();
 
    uint32_t now = millis();
    if (now - lastSampleMs >= SAMPLE_PERIOD_MS) {
        lastSampleMs = now;
 
        float ax, ay, az;
        if (imu.readAccelG(ax, ay, az)) {
            Serial.print(now);
            Serial.print(',');
            Serial.print(ax, 6);
            Serial.print(',');
            Serial.print(ay, 6);
            Serial.print(',');
            Serial.print(az, 6);
            Serial.print(',');
            Serial.println(currentLabel);
        }
    }
}