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


// serial variable string input

String ToneHValueString = "0";
String ToneBValueString = "0";
String VibroValueString = "0";

String  LedGValueString = "0";
String  LedJValueString = "0";


String SerialXString ="0";
String SerialYString ="0";
String SerialZString ="0";
String strArr[8]; //Set the size of the array to equal the number of values you will be receiveing





// serial variable output

int ToneHValue = 0;
int ToneBValue = 0;
int VibroValue = 0;

int LedGValue = 0;
int LedJValue = 0;


long SerialX =0;
long SerialY =0;
long SerialZ =0;



// output pin





const int ToneHPin = 5;    // output PRW Buzzer Passif 1 : utilise les pin analigique si besoin
const int ToneBPin = 6;    // output PRW Buzzer Pasif 2
const int VibroPin = 3;    // output PRW Vibro 

const int LedGPin = 9;    // output PRW Led digital joystock vert
const int LedJPin = 10;    // output PRW Led digital joystock jaune

// calculat variable 
long t=0;
long ti=0;
long X = 0;
long dX = 0; 
long Y = 0;
long dY = 0;
long Z = 0;
long dZ = 0;

int compt = 0 ; // rotary compteur

//input pin

// bouton 
const int TacPin = 2;    // input Attachinterupt digital Tactile bouton poussoir interupt
const int MidPin = 20;    // input Attachinterupt digital joystock Midle

//rotary bouton
const int PushPin = 4;   // input Attachinterupt digital push buton of Rotary

//rotary
const int ClkPin = 3;   //  input Attachinterupt digital rotary CLK
const int DtPin = 6;   // input Attachinterupt digital rotary DT


//stick buton
const int UpPin = 14;    // input Attachinterupt digital joystock Up
const int DwnPin = 3;    // input Attachinterupt digital joystock Down
const int LftPin = 16;    // input Attachinterupt digital joystock Left
const int RhtPin = 17;    // input Attachinterupt digital joystock Right



  int Valeur = 0 ;  // Swicht case valeur poussoir interupt

 String inputString = "";      // a String to hold incoming data
bool stringComplete = false;  // whether the string is complete
 
void setup() {

   pinMode(TacPin, INPUT_PULLUP); // Tactile bouton poussoir interupt
 
   pinMode(PushPin, INPUT_PULLUP); // push buton of Rotary     
   pinMode(ClkPin, INPUT_PULLUP); //  rotary CLK
   pinMode(DtPin, INPUT_PULLUP); // rotary DT

pinMode(UpPin, INPUT_PULLUP); // input Attachinterupt digital joystock Up
pinMode(DwnPin, INPUT_PULLUP); // input Attachinterupt digital joystock Down
pinMode(LftPin, INPUT_PULLUP); // input Attachinterupt digital joystock Left
pinMode(RhtPin, INPUT_PULLUP); // input Attachinterupt digital joystock Right
pinMode(MidPin, INPUT_PULLUP); //  input Attachinterupt digital joystock Midle




pinMode(ToneHPin, OUTPUT);   // output PRW Buzzer Passif 1 : utilise les pin analigique si besoin
pinMode(ToneBPin, OUTPUT);    // output PRW Buzzer Pasif 2
pinMode(VibroPin, OUTPUT);    // output PRW Vibro 

pinMode(LedGPin, OUTPUT);   // output PRW Led digital joystock vert
pinMode(LedJPin , OUTPUT);   // output PRW Led digital joystock jaune






 Serial.begin(9600);
    // reserve 200 bytes for the inputString:
  inputString.reserve(200);
       // start serial port at 9600 bps:
 
  while (!Serial) {
    ;  // wait for serial port to connect. Needed for native USB port only
  }
  
Serial.println("Appuis sur le bouton rouge");
 
  
      
 while (digitalRead(TacPin)== HIGH) {
   digitalRead(TacPin);
    Serial.print("Maintien Presser bouton Tactile :" );
     Serial.println(digitalRead(TacPin));  // send a capital A
     delay(300);
  }
  establishContact();  // send a byte to establish contact until receiver responds

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");     



attachInterrupt(digitalPinToInterrupt(TacPin), Bouton, CHANGE); // attache l'interruption externe Tactile bouton poussoir interupt

attachInterrupt(digitalPinToInterrupt(UpPin), Bouton, CHANGE); // attache l'interruption externe Bouton digital joystock Up
attachInterrupt(digitalPinToInterrupt(DwnPin), Bouton, CHANGE); // attache l'interruption externe Bouton digital joystock Down
attachInterrupt(digitalPinToInterrupt(LftPin), Bouton, CHANGE); // attache l'interruption externe Bouton digital joystock Left
attachInterrupt(digitalPinToInterrupt(RhtPin), Bouton, CHANGE); // attache l'interruption externe Bouton digital joystock Right
attachInterrupt(digitalPinToInterrupt(MidPin), Bouton, CHANGE); // attache l'interruption externe Bouton digital joystock Midle


attachInterrupt(digitalPinToInterrupt(PushPin), Bouton, CHANGE); // attache l'interruption externe à la fonction bouton de l'encodeur rotatif 
attachInterrupt(digitalPinToInterrupt(ClkPin), readEncoder, FALLING); // attache l'interruption externe à la fonction readRotary ont de l'encodeur rotatif


// inite tempo
 t=millis();
    ti=t-ti;

}

sensors_event_t event;
  
void loop() {

 PrintTag();
  
 


  
  
}


// ====================
// Callbacks.
// ====================

void readEncoder() {
  volatile int dtValue = digitalRead(DtPin);
  if (dtValue == HIGH) {

     compt +=1;
    
  }
  if (dtValue == LOW) {

      compt -=1;

}}
  
void Bouton() // la fonction appelée par l'interruption externe
{
  // envoi l'info a chaque changement de statue des bouton
Valeur = 1;
   
}

void BoutonTag() // la fonction appelée par l'interruption externe
{
  // envoi l'info a chaque changement de statue des bouton
  Serial.print("Buton statue: ");
      Serial.print(digitalRead(TacPin));
  Serial.print(",");
  Serial.print(digitalRead(UpPin));
  Serial.print(",");
    Serial.print(digitalRead(DwnPin));
  Serial.print(",");
    Serial.print(digitalRead(LftPin));
  Serial.print(",");
    Serial.print(digitalRead(RhtPin));
  Serial.print(",");
    Serial.print(digitalRead(MidPin));
  Serial.print(",");
    Serial.print(compt);
    Serial.print(",");
    Serial.println(digitalRead(PushPin));
   
}



void establishContact() {
  while (Serial.available() <= 0) {
    Serial.println("Apuis sur le bouton Start ");  // send a capital A
    delay(300);
  }
}


void PrintTag() // la fonction appelée par l'interruption externe greenPin
{
 /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);


    t=millis();
    ti=t-ti;

 Serial.print("[");
  Serial.print(ti);
  Serial.println(" ms ]     ");

    

  /* Print out the values */
  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  dX =(   (ti*a.acceleration.x)  /  (cos(g.gyro.x))   );
  X= X+dX*cos(g.gyro.y)*cos(g.gyro.z);

  dY =(   (ti*a.acceleration.y)  /  (cos(g.gyro.y))   );
  Y= Y+dY*cos(g.gyro.z)*cos(g.gyro.x);

  dZ =(   (ti*a.acceleration.z)  /  (cos(g.gyro.z))   );
  Z= Z+dZ*cos(g.gyro.x)*cos(g.gyro.y);

   Serial.print("Derive X: ");
  Serial.print(   dX   );
  Serial.print(", Y: ");
  Serial.print(   dY  );
  Serial.print(", Z: ");
  Serial.print(   dZ  );
  Serial.println(" mm");

   Serial.print("Position X: ");
  Serial.print(   X   );
  Serial.print(", Y: ");
  Serial.print(   Y  );
  Serial.print(", Z: ");
  Serial.print(   Z  );
  Serial.println(" mm");

  Serial.print("Temperature: ");
  Serial.print(temp.temperature); // utile pour calculer la vistesse du son
  Serial.println(" °C");




  Serial.println("");
   delay(500); /// Delay a suprrimer utile juste pour les test
   ti=millis();
}


void ConfirmRecive()

{
  // envoi une confirmation de reception de data
Serial.print("Serial data: ");
     Serial.print(ToneHValue);
  Serial.print(",");
  Serial.print(ToneBValue);
  Serial.print(",");
    Serial.print(VibroValue);
  Serial.print(",");
    Serial.print(LedGValue);
  Serial.print(",");
    Serial.print(LedJValue);
  Serial.print(",");
    Serial.print(SerialX);
  Serial.print(",");
    Serial.print(SerialY);
    Serial.print(",");
    Serial.println(SerialZ);

    }

void DataCom()
{
    String rxString = "";
    /// input data format set  1,243,1,1,22,1,1,1,  separateur virgule
    if (Serial.available()) {
        ToneHValueString = "";
        //Keep looping until there is something in the buffer.
        while (Serial.available()) {
            //Delay to allow byte to arrive in input buffer.
            delay(2);
            //Read a single character from the buffer.
            char ch = Serial.read();
            //Append that single character to a string.
            rxString += ch;
        }           
        int stringStart = 0;
        int arrayIndex = 0;
        for (int i = 0; i  < rxString.length(); i++) {
            //Get character and check if it's our "special" character.
            if (rxString.charAt(i) == ',') {
                //Clear previous values from array.
                strArr[arrayIndex] = "";
                //Save substring into array.
                strArr[arrayIndex] = rxString.substring(stringStart, i);
                //Set new string starting point.
                stringStart = (i + 1);
                arrayIndex++;
            }
        }
        //Put values from the array into the variables.
       ///exemple cpuValueString = strArr[0];
        //memValueString = strArr[1];

         ToneHValueString = strArr[0];
         ToneBValueString = strArr[1];
         VibroValueString = strArr[2];

        LedGValueString = strArr[3];
        LedJValueString = strArr[4];


        SerialXString =strArr[5];
        SerialYString =strArr[6];
        SerialZString =strArr[7];
        //Convert string to int.

        //  exemple cpuValue = cpuValueString.toInt();

        ToneHValue = ToneHValueString.toInt();;
        ToneBValue = ToneBValueString.toInt();
        VibroValue = VibroValueString.toInt();

        LedGValue = LedGValueString.toInt();
        LedJValue = LedJValueString.toInt();

        /// trouver un moyent de convertir string en long
        SerialX =SerialXString.toInt();  
        SerialY =SerialYString.toInt();
        SerialZ =SerialYString.toInt();


    }

    
    analogWrite(ToneHPin, ToneHValue);   // output PRW Buzzer Passif 1 : utilise les pin analigique si besoin
analogWrite(ToneBPin, ToneBValue);    // output PRW Buzzer Pasif 2
analogWrite(VibroPin, VibroValue);    // output PRW Vibro 

analogWrite(LedGPin, LedGValue);   // output PRW Led digital joystock vert
analogWrite(LedJPin , LedJValue);   // output PRW Led digital joystock jaune

}
/*
  SerialEvent occurs whenever a new data comes in the hardware serial RX. This
  routine is run between each time loop() runs, so using delay inside loop can
  delay response. Multiple bytes of data may be available.
*/
void serialEvent() {

   DataCom();
   ConfirmRecive();
 
}