#include "Wire.h"
#include "MPU6050.h"

/******************************************************************************
 * 4-labyrinthe-imu.ino
 * @title: Programme pour la carte Arduino de gestion de centrale inertielle (capteur IMU)
 * @project: Blender-EduTech - Tutoriel 4 : 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
 *
 ******************************************************************************/

/******************************************************************************
 * IMU - I2C
 ******************************************************************************/

MPU6050 accelgyro;

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;
String serial_msg_int_txt;

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

String serial_msg = ""; // Message

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

  // Liaison série
  Serial.begin(115200); 

  // IMU
  Wire.begin();
  accelgyro.initialize();
}

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

void loop() {

    /*****
     * Lecture des accélérations
     *****/
    
    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)); // position capteur (X vers la gauche, Y vers l'arriere, Z vers le haut)
    pitch_deg = pitch*57.3;
    pitch_txt = String(pitch_deg);
    
    /*****
     * IMU : Arduino -> UPBGE
     *****/

    Serial.print(roll_txt);
    Serial.print(",");
    Serial.print(pitch_txt);
    Serial.println();
   	
}