/*
  www.aifes.ai
  https://github.com/Fraunhofer-IMS/AIfES_for_Arduino
  Copyright (C) 2020-2022  Fraunhofer Institute for Microelectronic Circuits and Systems.
  All rights reserved.

  AIfES is free software: you can redistribute it and/or modify
  it under the terms of the GNU Affero General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU Affero General Public License for more details.

  You should have received a copy of the GNU Affero General Public License
  along with this program.  If not, see <https://www.gnu.org/licenses/>.

  AIfES-Express gesture data acquisition
  --------------------

    Versions:
    1.0.0   Initial version

    
  You can find more AIfES tutorials here:
  https://create.arduino.cc/projecthub/aifes_team
  */

#include <aifes.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;

#define NUM_SAMPLES 10  //15 30
#define NUM_AXES 3
#define TRUNCATE 20
#define ACCEL_THRESHOLD 20 //5
#define INTERVAL 30

#define INPUTS          NUM_SAMPLES * NUM_AXES

// Baseline for calibration
float baseline[NUM_AXES];

// Features (inputs) of a single gesture
float features[INPUTS];

sensors_event_t a, g, temp;

void setup() {
  Serial.begin(115200);
  while (!Serial);
  Serial.println("Data acquisition:");
  Serial.print(INPUTS);
  Serial.println(" features");
  Serial.println("Record your 3 gestures and copy them into the CSV file");
  delay(10);
  
  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");
  Serial.println("");
  Serial.println("");
  
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  pinMode(4, INPUT_PULLUP);

}

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

  mpu.getEvent(&a, &g, &temp);
  ax = a.acceleration.x;
  ay = a.acceleration.y;
  az = a.acceleration.z;

  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)) {
  delay(10);
  return;
  }
  
  recordIMU();
  print_features();
  

  delay(500);

}

void print_features() {
    const uint16_t numFeatures = sizeof(features) / sizeof(float);
     
    for (int i = 0; i < numFeatures; i++) {
        Serial.print(features[i],5);
        
        if(i == numFeatures - 1){
          Serial.print(";");
          if(digitalRead(2))
          {
            Serial.print("0");
          }else
          {
            Serial.print("1");
          }
          
          Serial.print(";");

          if(digitalRead(3))
          {
            Serial.print("0");
          }else
          {
            Serial.print("1");
          }
          
          Serial.print(";");

          if(digitalRead(4))
          {
            Serial.print("0");
          }else
          {
            Serial.print("1");
          }
          
          Serial.print('\n');         
        }
        else{
          Serial.print(";");

        }
    }
}

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++) {
        mpu.getEvent(&a, &g, &temp);
        ax = a.acceleration.x;
        ay = a.acceleration.y;
        az = a.acceleration.z;

        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 calibrate() {
    float ax, ay, az;

    for (int i = 0; i < 10; i++) {
        mpu.getEvent(&a, &g, &temp);
        ax = a.acceleration.x;
        ay = a.acceleration.y;
        az = a.acceleration.z;
        delay(100);
    }

    baseline[0] = ax;
    baseline[1] = ay;
    baseline[2] = az;
}