#include <math.h>
#include <I2Cdev.h>
#include <MPU6050.h>

#include <Adafruit_NeoPixel.h>

MPU6050 accelgyro;
int _AccelgyroScale = 2;

void SetAccGyroScale(int Scale) {
  Scale = abs(Scale);
  if ( Scale < 3 )
    { accelgyro.setFullScaleGyroRange(MPU6050_ACCEL_FS_2);  _AccelgyroScale = 2; }
  else if  ( Scale < 6 )
    { accelgyro.setFullScaleGyroRange(MPU6050_ACCEL_FS_4);  _AccelgyroScale = 4; }
  else if  ( Scale < 12 )
    { accelgyro.setFullScaleGyroRange(MPU6050_ACCEL_FS_8);  _AccelgyroScale = 8; }
  else
    { accelgyro.setFullScaleGyroRange(MPU6050_ACCEL_FS_16);  _AccelgyroScale = 16; }
}
void SetIntThreshold(int Seuil) {
  accelgyro.setDHPFMode(0);   //  Reset
  accelgyro.setIntMotionEnabled(true);
  accelgyro.setMotionDetectionThreshold(Seuil / 100);   //  Divider should be OK, but to be checked
  accelgyro.setMotionDetectionDuration(2);      //  2ms
  delay(2);         //  Little bit wait, need to be taken into account before new command
  accelgyro.setDHPFMode(0b111);   //  Hold
  accelgyro.setInterruptDrive(0); //  Pull Up
  accelgyro.setInterruptMode(1);          //  Active LOW
}
float AccelGyro_getAccelerationModule() {
  int16_t gx, gy, gz, ax, ay, az;
  float fx, fy, fz;
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  fx = ax / 3340.265 * _AccelgyroScale;    //  Get Ax as float absolute value
  fy = ay / 3340.265 * _AccelgyroScale;    //  Get Ay as float absolute value
  fz = az / 3340.265 * _AccelgyroScale;    //  Get Az as float absolute value
  return(sqrt(fx*fx + fy*fy + fz*fz));
}
float AccelGyro_getYaw() {
 //  To be done later  return(0.0);
}
float AccelGyro_getPitch() {
  int16_t gx, gy, gz, ax, ay, az;
  float fx, fz;
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  fx = ax / 3340.265 * _AccelgyroScale;    //  Get Ax as float absolute value
  fz = az / 3340.265 * _AccelgyroScale;    //  Get Az as float absolute value
  return(atan2(fx, fz) * RAD_TO_DEG);
}
float AccelGyro_getRoll() {
  int16_t gx, gy, gz, ax, ay, az;
  float fy, fz;
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  fy = ay / 3340.265 * _AccelgyroScale;    //  Get Ay as float absolute value
  fz = az / 3340.265 * _AccelgyroScale;    //  Get Az as float absolute value
  return(atan2(fy, fz) * RAD_TO_DEG);
}
Adafruit_NeoPixel pixels_DEL_RVB = Adafruit_NeoPixel(11, 12, NEO_GRB + NEO_KHZ800);

int acc_z;
int i;
long date_etat;
int etat;
float acc;
int acc_x;
int acc_y;
int MPU_OK;

// Décrire cette fonction
void calme() {
  acc_z = accelgyro.getAccelerationZ();
  if (acc_z > 15500) {
    for (i = 0; i <= 10; i=i+1) {
      pixels_DEL_RVB.setPixelColor(i, pixels_DEL_RVB.Color(100,0,0));
      pixels_DEL_RVB.show();
    }

  } else if (acc_z < -15500) {
    for (i = 0; i <= 10; i=i+1) {
      pixels_DEL_RVB.setPixelColor(i, pixels_DEL_RVB.Color(50,50,0));
      pixels_DEL_RVB.show();
    }
  } else {
    for (i = 0; i <= 10; i=i+1) {
      pixels_DEL_RVB.setPixelColor(i, pixels_DEL_RVB.Color(0,0,0));
      pixels_DEL_RVB.show();
    }

  }
  date_etat = millis();
  etat = 0;
}


// Décrire cette fonction
void un_peu_agit_C3_A9() {
  for (i = 0; i <= 10; i=i+1) {
    pixels_DEL_RVB.setPixelColor(i, pixels_DEL_RVB.Color(0,30,0));
    pixels_DEL_RVB.show();
  }
}


// Décrire cette fonction
void bouge_moyen() {
  for (i = 0; i <= 10; i=i+1) {
    pixels_DEL_RVB.setPixelColor(i, pixels_DEL_RVB.Color(0,0,100));
    pixels_DEL_RVB.show();
  }
}


// Décrire cette fonction
void bouge__C3_A9norm_C3_A9ment() {
  for (i = 0; i <= 10; i=i+1) {
    pixels_DEL_RVB.setPixelColor(i, pixels_DEL_RVB.Color(102,0,153));
    pixels_DEL_RVB.show();
  }
}



void setup() {
  Serial.begin(115200);
  pinMode(12, OUTPUT);
  pixels_DEL_RVB.begin();
  delay(1000);
    Wire.begin();
    accelgyro.reset();
    delay(50);
    accelgyro.initialize();
  if (!accelgyro.testConnection()) 
  {
    Serial.println("module mal câblé");
  }
 else
 {
  Serial.println("Module accéléromètre OK");
  MPU_OK = 0;
 }
  acc = (float)(0);
  etat = (int)(0);
  date_etat = (unsigned long)(0);

}

void loop() {
  if ( MPU_OK == 0 )
  {
    Wire.begin();
    accelgyro.reset();
    delay(50);
    accelgyro.initialize();
    MPU_OK = accelgyro.testConnection();
    if ( MPU_OK )
      Serial.println("Accéléromètre OK maintenant");
    else
    {
      Serial.println("Accéléromètre non OK");
      delay(100);
    }
    if ( (millis() /1000) & 1 )
    {
      for (i = 0; i <= 10; i=i+1) {
        pixels_DEL_RVB.setPixelColor(i, pixels_DEL_RVB.Color(88,88,88));
      }
      pixels_DEL_RVB.show();

    }
    else
    {
      for (i = 0; i <= 10; i=i+1) {
        pixels_DEL_RVB.setPixelColor(i, pixels_DEL_RVB.Color(0,0,0));
      }
      pixels_DEL_RVB.show();

    }
    return;
  }
    acc = AccelGyro_getAccelerationModule();
    acc = abs(acc - 10);
    Serial.print("acc=");
     Serial.println(acc);
    acc_x = accelgyro.getAccelerationX();
    acc_y = accelgyro.getAccelerationY();
    acc_z = accelgyro.getAccelerationZ();
    Serial.print("accx=");
     Serial.print(acc_x);
    Serial.print(" accy=");
     Serial.print(acc_y);
    Serial.print(" accz=");
     Serial.println(acc_z);
    delay(100);
    if (acc < 1) {
      calme();

    } else if (acc < 3) {
      un_peu_agit_C3_A9();
    } else if (acc < 6) {
      bouge_moyen();
    } else {
      bouge__C3_A9norm_C3_A9ment();

    }

}
