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

/******************************************************************************
 * 6-labyrinthe-arduino.ino
 * @title: Programme pour la carte Arduino (capteur imu + 2 servomoteurs)
 * @project: Blender-EduTech - Tutoriel 6 : Labyrinthe à bille - Développer le jumeau numérique du labyrinthe
 * @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;

/******************************************************************************
 * Led Matrix - I2C
 ******************************************************************************/

Matrix_8x8 matrix;

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

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

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

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

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

  // Led Matrix
  matrix.init();
  matrix.setBrightness(0);
  matrix.setBlinkRate(BLINK_OFF);
  matrix.clear();
  matrix.display();
 }

/******************************************************************************
 * 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();
   	
    /*****
     * Led Matrix : UPBGE -> Arduino
     *****/
    
    if (serial_msg_complet) {
	matrix.clear();
	int xy= serial_msg.toInt(); // Message par chiffre : XY

	// Position de la bille en x et y
	if (xy<90) {
	    int x= xy/10;
	    int y= xy-(x*10);
	    matrix.writePixel(x, y, true);
	}
	
	// Chute
	if (xy==91) {
	    matrix.writeOnePicture(0x81423c0000666600);
	    matrix.display();
	    delay(1000);
	    Serial.println("start"); // Relance le jeu
	}

	// Victoire
	if (xy==92)  matrix.writeOnePicture(0x003c428100666600);

	// Affichage
	matrix.display();
	serial_msg = "";
	serial_msg_complet = false;
    }
}

    
/******************************************************************************
 * Évènement provoqué par UPBGE (via la liaison série)
 ******************************************************************************/

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