#include "Wire.h"

/******************************************************************************
 * 4-labyrinthe-imu.ino
# @title: Programme pour la carte Arduino de gestion de centrale inertielle (capteur IMU)
 * @project: Blender-EduTech - Tutoriel 3 : Labyrinthe à bille - Interfacer avec une carte Arduino par la liaision série
 * @lang: fr
 * @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
 * @copyright: Copyright (C) 2023 Philippe Roy 
 * @license: GNU GPL
 *
 ******************************************************************************/

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

// fr : I2Cdev et MPU6050 doivent être installée comme bibilothèque ou sinon les fichiers .cpp et .h
// des deux classes doivent être inclus dans le chemin du projet
// en : 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"

// fr : L'adresse par défault de la classe I2C est 0x68
// en : 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;

int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;
float Axyz[3];
float roll;
float pitch;
float roll_deg;
float pitch_deg;
String roll_txt;
String pitch_txt;

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

  // Liaison série
  Serial.begin(115200); // 7 fps
  /* Serial.begin(38400); */ // 6 fps
  /* Serial.begin(9600); */ // trop lent 2fps

  // I2C
  Wire.begin();
  Serial.println("Initialisation des composants I2C.");
  accelgyro.initialize();
 }

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

void loop() {

    /*****
     * 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 -> UPBGE
     *****/

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

    /* delay(300); */
}