/*
IoT-02_mp6050_dataForwarder.ino

Based on: https://eloquentarduino.com/gesture-classification/

Modified by Jordi Binefa (www.binefa.com or provisional.binefa.com)
20231212
*/

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

#define BT_I34 34
#define LED_W 19
#define LED_R 23
#define LED_Y 27
#define LED_G 32

#define FREQUENCY_HZ        60
#define INTERVAL_MS         (1000 / (FREQUENCY_HZ + 1))

// objeto da classe Adafruit_MPU6050
Adafruit_MPU6050 mpu;

static unsigned long last_interval_ms = 0;

bool bBotoPremut(int nQuin){
  delay(50);
  return !digitalRead(nQuin);
}

void setup() {
  Serial.begin(115200);
  Serial.println(__FILE__);

  pinMode(BT_I34,INPUT);
  pinMode(LED_W,OUTPUT);
  pinMode(LED_R,OUTPUT);
  pinMode(LED_Y,OUTPUT);
  pinMode(LED_G,OUTPUT);

  // Try to initialize!
  if (!mpu.begin()) { // if (!mpu.begin(0x68))
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
    case MPU6050_RANGE_2_G:
      Serial.println("+-2G");
      break;
    case MPU6050_RANGE_4_G:
      Serial.println("+-4G");
      break;
    case MPU6050_RANGE_8_G:
      Serial.println("+-8G");
      break;
    case MPU6050_RANGE_16_G:
      Serial.println("+-16G");
      break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
    case MPU6050_RANGE_250_DEG:
      Serial.println("+- 250 deg/s");
      break;
    case MPU6050_RANGE_500_DEG:
      Serial.println("+- 500 deg/s");
      break;
    case MPU6050_RANGE_1000_DEG:
      Serial.println("+- 1000 deg/s");
      break;
    case MPU6050_RANGE_2000_DEG:
      Serial.println("+- 2000 deg/s");
      break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
    case MPU6050_BAND_260_HZ:
      Serial.println("260 Hz");
      break;
    case MPU6050_BAND_184_HZ:
      Serial.println("184 Hz");
      break;
    case MPU6050_BAND_94_HZ:
      Serial.println("94 Hz");
      break;
    case MPU6050_BAND_44_HZ:
      Serial.println("44 Hz");
      break;
    case MPU6050_BAND_21_HZ:
      Serial.println("21 Hz");
      break;
    case MPU6050_BAND_10_HZ:
      Serial.println("10 Hz");
      break;
    case MPU6050_BAND_5_HZ:
      Serial.println("5 Hz");
      break;
  }

  Serial.println("");
  delay(100);
}

void loop() {
  // String sMoviment = ",0.0,quiet";
  // String sMoviment = ",1.0,accX";
  // String sMoviment = ",2.0,girY";
  long int liAra = millis();
  static long int sliPrevi = liAra;
  static bool bRepos = true, bLlegint = false, bEsperant3s = false;
  sensors_event_t a, g, temp;
  
  if(bBotoPremut(BT_I34) && bRepos){
    sliPrevi = liAra;
    bRepos = false;
    bEsperant3s = true;
  }
  if(bEsperant3s){ // Espera tres segons abans de llegir
    if(liAra - sliPrevi > 3000){
      sliPrevi = liAra;
      bEsperant3s = false;
      bLlegint = true;
    }
  }
  if(bLlegint){
    if (liAra > last_interval_ms + INTERVAL_MS) {
      last_interval_ms = liAra;

      mpu.getEvent(&a, &g, &temp);

      Serial.print("IMU: ");
      Serial.print(a.acceleration.x);
      Serial.print(",");
      Serial.print(a.acceleration.y);
      Serial.print(",");
      Serial.print(a.acceleration.z);
      Serial.print(",");
      Serial.print(g.gyro.x);
      Serial.print(",");
      Serial.print(g.gyro.y);
      Serial.print(",");
      Serial.print(g.gyro.z);
      // Serial.print(sMoviment);
      Serial.print("\n");
    }
    if(liAra - sliPrevi > 30000){ // 30 segons de lectura
      bLlegint = false;
      bRepos = true;
    }
  }
  digitalWrite(LED_R,bRepos);
  digitalWrite(LED_G,bLlegint);
  digitalWrite(LED_Y,bEsperant3s);
}