#include "Wire.h"

/******************************************************************************
 * imu-test.ino
 * @title: Test du capteur IMU (liaison série)
 * @project: Blender-EduTech - Tutoriel : Tutoriel 3 Labyrinthe à bille - Interfacer la scène 3D avec une carte Arduino
 * @lang: fr
 * @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
 * @copyright: Copyright (C) 2023 Philippe Roy 
 * @license: GNU GPL
 *
 ******************************************************************************/

/******************************************************************************
 * I2C
 ******************************************************************************/

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
I2Cdev   I2C_M;

uint8_t buffer_m[6];
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;

float heading;
float tiltheading;

float Axyz[3];
float roll;
float pitch;
float roll_deg;
float pitch_deg;
String roll_txt;
String pitch_txt;

/******************************************************************************
 * Pupitre
 ******************************************************************************/

// Adressage Led Arduino
const int led =  13; // Led de mouvement (onboard)
const int led_com =  10; // Led de communication modele 3d-> arduino

// Adressage Entrees Arduino
// const int bt_a_m =  A1; // Bouton A+ (verin droit)
// const int bt_a_d =  A0; // Bouton A- (verin droit)
// const int bt_b_m =  A3; // Bouton B+ (verin gauche arrière)
// const int bt_b_d =  A2; // Bouton B- (verin gauche arrière)
// const int bt_c_m =  3; // Bouton C+ (verin gauche avant)
// const int bt_c_d =  2; // Bouton C- (verin gauche avant)

// Entrees numeriques (modele 3D) 
// bool bt_a_m_num=false ; // Bouton A+ (verin droit)
// bool bt_a_d_num=false; // Bouton A- (verin droit)
// bool bt_b_m_num=false; // Bouton B+ (verin gauche arrière)
// bool bt_b_d_num=false; // Bouton B- (verin gauche arrière)
// bool bt_c_m_num=false; // Bouton C+ (verin gauche avant)
// bool bt_c_d_num=false; // Bouton C- (verin gauche avant) 
 
// // Adressage Sorties Arduino
// const int v_a_v = 5; // Mouvement A (verin droit) : Vitesse (grove fil blanc)
// const int v_a_s = 4; // Mouvement A (verin droit) : Sens (grove  fil jaune)
// const int v_b_v = 7; // Mouvement B (verin gauche arriere) : Vitesse (grove fil blanc)
// const int v_b_s = 6; // Mouvement B (verin gauche arriere) : Sens (grove fil jaune)
// const int v_c_v = 9; // Mouvement C (verin gauche avant) : Vitesse (grove fil blanc)
// const int v_c_s = 8; // Mouvement C (verin gauche avant) : Sens (grove fil jaune)

/******************************************************************************
 * Communication serie
 ******************************************************************************/

String serial_msg = ""; // Message 
bool serial_msg_complet = false;  // Flag de message complet

/******************************************************************************
 * Initialisation
 ******************************************************************************/
   
void setup() {

  // Configure les broches des entrees
  // pinMode(bt_a_m, INPUT);
  // pinMode(bt_a_d, INPUT);
  // pinMode(bt_b_m, INPUT);
  // pinMode(bt_b_d, INPUT);
  // pinMode(bt_c_m, INPUT);
  // pinMode(bt_c_d, INPUT);
  
  // // Configure les broches des sorties 
  // pinMode(v_a_v, OUTPUT);
  // pinMode(v_a_s, OUTPUT);
  // pinMode(v_b_v, OUTPUT);
  // pinMode(v_b_s, OUTPUT);
  // pinMode(v_c_v, OUTPUT);
  // pinMode(v_c_s, OUTPUT);

  pinMode(led, OUTPUT); // Led de mouvement
  pinMode(led_com, OUTPUT); // Led de communication modele 3d-> arduino
  digitalWrite(led, LOW);
  digitalWrite(led_com, LOW);

  // Moniteur serie
  Serial.begin(115200); // 7 fps
  /* Serial.begin(38400); */ // 6 fps
  /* Serial.begin(9600); */ // trop lent 2fps

  // I2C
  Wire.begin();
  Serial.println("Initializing I2C devices...");
  accelgyro.initialize();
 }

/******************************************************************************
 * Boucle principale
 ******************************************************************************/

void loop() {

  //   /*****
  //    * Communication : modele 3d -> arduino
  //    *****/

  //   if (serial_msg_complet) {
	// if (serial_msg =="Bp Am R\n") bt_a_m_num=false;
	// if (serial_msg =="Bp Am\n") bt_a_m_num=true;
	// if (serial_msg =="Bp Ad R\n") bt_a_d_num=false;
	// if (serial_msg =="Bp Ad\n") bt_a_d_num=true;
	// if (serial_msg =="Bp Bm R\n") bt_b_m_num=false;
	// if (serial_msg =="Bp Bm\n") bt_b_m_num=true;
	// if (serial_msg =="Bp Bd R\n") bt_b_d_num=false;
	// if (serial_msg =="Bp Bd\n") bt_b_d_num=true;
	// if (serial_msg =="Bp Cm R\n") bt_c_m_num=false;
	// if (serial_msg =="Bp Cm\n") bt_c_m_num=true;
	// if (serial_msg =="Bp Cd R\n") bt_c_d_num=false;
	// if (serial_msg =="Bp Cd\n") bt_c_d_num=true;

	// /* Serial.println("Echo : "+serial_msg); */
  //   	serial_msg = "";
  //   	serial_msg_complet = false;
  //   }
     
  //   /*****
  //    * Verin A (verin droit)
  //    *****/

  //    // Bouton physique : LOW = actif et HIGH = pas actif
  //   // Bouton numérique (modele 3d) : true = actif et false = pas actif

  //   // A+ (sortie de tige)
  //   if ((digitalRead(bt_a_m) == LOW || bt_a_m_num) && digitalRead(bt_a_d) == HIGH && !bt_a_d_num) {
	// /* Serial.println("A+");  */
	// digitalWrite(v_a_v, HIGH); // Mouvement A : Vitesse (fil blanc)
	// digitalWrite(v_a_s, LOW); // Mouvement A : Sens trigo (fil jaune)
	// digitalWrite(led, HIGH);
  //   }

  //   // A- (rentrée de tige)
  //   if ((digitalRead(bt_a_d) == LOW || bt_a_d_num) && digitalRead(bt_a_m) == HIGH && !bt_a_m_num) {
	// /* Serial.println("A-"); */
	// digitalWrite(v_a_v, HIGH); // Mouvement A : Vitesse (fil blanc)
	// digitalWrite(v_a_s, HIGH); // Mouvement A : Sens horaire (fil jaune)
	// digitalWrite(led, HIGH);
  //   }

  //   // Stop A
  //   if (digitalRead(bt_a_m) == LOW && (digitalRead(bt_a_d) == LOW || bt_a_d_num)){ // Ordres contradictoires
	// /* Serial.println("Stop A"); */
	// digitalWrite(v_a_v, LOW); // Mouvement A : Vitesse nulle (fil blanc)
	// digitalWrite(led, LOW);
  //   }
  //   if (digitalRead(bt_a_d) == LOW && (digitalRead(bt_a_m) == LOW || bt_a_m_num)){ // Ordres contradictoires
	// /* Serial.println("Stop A"); */
	// digitalWrite(v_a_v, LOW); // Mouvement A : Vitesse nulle (fil blanc)
	// digitalWrite(led, LOW);
  //   }
  //   if (digitalRead(bt_a_m) == HIGH && digitalRead(bt_a_d) == HIGH && !bt_a_m_num && !bt_a_d_num){ // Aucun ordre
	// /* Serial.println("Stop A"); */
	// digitalWrite(v_a_v, LOW); // Mouvement A : Vitesse nulle (fil blanc)
	// digitalWrite(led, LOW);
  //   }

  //    /*****
  //    * Verin B (verin gauche arriere)
  //    *****/

  //   // B+ (sortie de tige)
  //   if ((digitalRead(bt_b_m) == LOW || bt_b_m_num) && digitalRead(bt_b_d) == HIGH && !bt_b_d_num) {
	// /* Serial.println("B+");  */
	// digitalWrite(v_b_v, HIGH); // Mouvement B : Vitesse (fil blanc)
	// digitalWrite(v_b_s, LOW); // Mouvement B : Sens trigo (fil jaune)
	// digitalWrite(led, HIGH);
  //   }

  //   // B- (rentrée de tige)
  //   if ((digitalRead(bt_b_d) == LOW || bt_b_d_num) && digitalRead(bt_b_m) == HIGH && !bt_b_m_num) {
	// /* Serial.println("B-"); */
	// digitalWrite(v_b_v, HIGH); // Mouvement B : Vitesse (fil blanc)
	// digitalWrite(v_b_s, HIGH); // Mouvement B : Sens horaire (fil jaune)
	// digitalWrite(led, HIGH);
  //   }

  //   // Stop B
  //   if (digitalRead(bt_b_m) == LOW && (digitalRead(bt_b_d) == LOW || bt_b_d_num)){ // Ordres contradictoires
	// /* Serial.println("Stop B"); */
	// digitalWrite(v_b_v, LOW); // Mouvement B : Vitesse nulle (fil blanc)
	// digitalWrite(led, LOW);
  //   }
  //   if (digitalRead(bt_b_d) == LOW && (digitalRead(bt_b_m) == LOW || bt_b_m_num)){ // Ordres contradictoires
	// /* Serial.println("Stop B"); */
	// digitalWrite(v_b_v, LOW); // Mouvement B : Vitesse nulle (fil blanc)
	// digitalWrite(led, LOW);
  //   }
  //   if (digitalRead(bt_b_m) == HIGH && digitalRead(bt_b_d) == HIGH && !bt_b_m_num && !bt_b_d_num){ // Aucun ordre
	// /* Serial.println("Stop B"); */
	// digitalWrite(v_b_v, LOW); // Mouvement B : Vitesse nulle (fil blanc)
	// digitalWrite(led, LOW);
  //   }

  //   /*****
  //    * Verin C (verin gauche avant)
  //    *****/

  //   // C+ (sortie de tige)
  //   if ((digitalRead(bt_c_m) == LOW || bt_c_m_num) && digitalRead(bt_c_d) == HIGH && !bt_c_d_num) {
	// /* Serial.println("C+");  */
	// digitalWrite(v_c_v, HIGH); // Mouvement C : Vitesse (fil blanc)
	// digitalWrite(v_c_s, LOW); // Mouvement C : Sens trigo (fil jaune)
	// digitalWrite(led, HIGH);
  //   }

  //   // C- (rentrée de tige)
  //   if ((digitalRead(bt_c_d) == LOW || bt_c_d_num) && digitalRead(bt_c_m) == HIGH && !bt_c_m_num) {
	// /* Serial.println("C-"); */
	// digitalWrite(v_c_v, HIGH); // Mouvement C : Vitesse (fil blanc)
	// digitalWrite(v_c_s, HIGH); // Mouvement C : Sens horaire (fil jaune)
	// digitalWrite(led, HIGH);
  //   }

  //   // Stop C
  //   if (digitalRead(bt_c_m) == LOW && (digitalRead(bt_c_d) == LOW || bt_c_d_num)){ // Ordres contradictoires
	// /* Serial.println("Stop C"); */
	// digitalWrite(v_c_v, LOW); // Mouvement C : Vitesse nulle (fil blanc)
	// digitalWrite(led, LOW);
  //   }
  //   if (digitalRead(bt_c_d) == LOW && (digitalRead(bt_c_m) == LOW || bt_c_m_num)){ // Ordres contradictoires
	// /* Serial.println("Stop C"); */
	// digitalWrite(v_c_v, LOW); // Mouvement C : Vitesse nulle (fil blanc)
	// digitalWrite(led, LOW);
  //   }
  //   if (digitalRead(bt_c_m) == HIGH && digitalRead(bt_c_d) == HIGH && !bt_c_m_num && !bt_c_d_num){ // Aucun ordre
	// /* Serial.println("Stop C"); */
	// digitalWrite(v_c_v, LOW); // Mouvement C : Vitesse nulle (fil blanc)
	// digitalWrite(led, LOW);
  //   }

    /*****
     * Lecture des accelerations
     *****/

    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
    Axyz[0] = (double) ax / 16384;
    Axyz[1] = (double) ay / 16384;
    Axyz[2] = (double) az / 16384; 
    roll = asin(-Axyz[0]);
    roll_deg = roll*57.3;
    roll_txt = String(roll_deg);
    /* pitch = asin(Axyz[1]/cos(roll)); */
    pitch = -asin(Axyz[1]/cos(roll)); // dépend du positionnement du capteur (X vers la droite, Y vers l'arriere, Z vers le haut)
    pitch_deg = pitch*57.3;
    pitch_txt = String(pitch_deg);
    
    /*****
     * Communication : arduino -> modele 3d
     *****/

    /* Serial.println("Roll (Rx): "+String(roll*57.3) + " Pitch (Ry): " + String(pitch*57.3) + */
    /* 		   " bt_a_m: " + String(digitalRead(bt_a_m)) + " bt_a_d: " + String(digitalRead(bt_a_d)) + */
    /* 		   " bt_b_m: " + String(digitalRead(bt_b_m)) + " bt_b_d: " + String(digitalRead(bt_b_d)) + */
    /* 		   " bt_c_m: " + String(digitalRead(bt_c_m)) + " bt_c_d: " + String(digitalRead(bt_c_d))); */

    // Serial.println("Roll (Rx): "+ roll_txt + " Pitch (Ry): " + pitch_txt +
    // 		   " bt_a_m: " + digitalRead(bt_a_m) + " bt_a_d: " + digitalRead(bt_a_d) +
    // 		   " bt_b_m: " + digitalRead(bt_b_m) + " bt_b_d: " + digitalRead(bt_b_d) +
    // 		   " bt_c_m: " + digitalRead(bt_c_m) + " bt_c_d: " + digitalRead(bt_c_d));

    Serial.println("Roll (Rx): "+ roll_txt + " Pitch (Ry): " + pitch_txt);

    /* delay(300); */
}

/******************************************************************************
 * Evenements provoques par la communication serie
 ******************************************************************************/

// void serialEvent() {
//   while (Serial.available()) {
//     char inChar = (char)Serial.read();
//     serial_msg += inChar;
//     if (inChar == '\n') {
// 	serial_msg_complet = true;
//     }
//   }
// }