//Libraries Definition 
#include <MPU6050_light.h>
#include <Wire.h>
#include "mymodel.h"


//Constants Definition
#define NUM_SAMPLES 30
#define NUM_AXES 3
#define TRUNCATE 20
#define ACCEL_THRESHOLD 20
#define INTERVAL 30

//variables definition
float baseline[NUM_AXES];
float features[NUM_SAMPLES * NUM_AXES];
int flag=0;

MPU6050 imu(Wire);

void imu_setup() {
    // I2C pins για ESP32
    Wire.begin(26, 27);
    Wire.setClock(100000);
    
    Serial.println("MPU6050 test!");
    
    byte status = imu.begin();
    if (status != 0) {
        Serial.println("Failed to find MPU6050 chip");
        while (1) delay(10);
    }
    Serial.println("MPU6050 Found!");
    
    // Αυτόματη βαθμονόμηση
    Serial.println("Calibrating... Keep MPU still.");
    delay(1000);
    imu.calcOffsets(true, true);
    Serial.println("Calibration done!");
}

// Η σημαντική αλλαγή: μετατροπή G → m/s²
void imu_read(float *ax, float *ay, float *az) {
    imu.update();
    *ax = imu.getAccX() * 9.80665f;
    *ay = imu.getAccY() * 9.80665f;
    *az = imu.getAccZ() * 9.80665f;
}

void setup() {
    Serial.begin(115200);
    delay(20);
    imu_setup();
    calibrate();
}

void loop() {
    float ax, ay, az;

    if (flag == 0) {
        Serial.println("Ready, waiting for move");    
    }
    
    imu_read(&ax, &ay, &az);
    
    ax = constrain(ax - baseline[0], -TRUNCATE, TRUNCATE);
    ay = constrain(ay - baseline[1], -TRUNCATE, TRUNCATE);
    az = constrain(az - baseline[2], -TRUNCATE, TRUNCATE);

    if (!motionDetected(ax, ay, az)) {
        flag = 1;
        delay(10);
        return;
    }
    
    flag = 0;
    recordIMU();
    classify();
    delay(2000);
}

void calibrate() {
    Serial.println("Calibrating.....miso....");
    float ax, ay, az;
    for (int i = 0; i < 10; i++) {
        imu_read(&ax, &ay, &az);
        delay(100);
    }
    baseline[0] = ax;
    baseline[1] = ay;
    baseline[2] = az;
    Serial.println("ok....ksekina");
}

bool motionDetected(float ax, float ay, float az) {
    return (abs(ax) + abs(ay) + abs(az)) > ACCEL_THRESHOLD;
}

void recordIMU() {
    float ax, ay, az;

    for (int i = 0; i < NUM_SAMPLES; i++) {
        imu_read(&ax, &ay, &az);

        ax = constrain(ax - baseline[0], -TRUNCATE, TRUNCATE);
        ay = constrain(ay - baseline[1], -TRUNCATE, TRUNCATE);
        az = constrain(az - baseline[2], -TRUNCATE, TRUNCATE);

        features[i * NUM_AXES + 0] = ax;
        features[i * NUM_AXES + 1] = ay;
        features[i * NUM_AXES + 2] = az;

        delay(INTERVAL);
    }
}

void printFeatures() {
    const uint16_t numFeatures = sizeof(features) / sizeof(float);
    for (int i = 0; i < numFeatures; i++) {
        Serial.print(features[i]);
        Serial.print(i == numFeatures - 1 ? '\n' : ',');
    }
}

void classify() {
    Serial.print("Detected gesture: ");
    Serial.println(predictLabel(features));
}
