#include "Wire.h"
#include "Grove_Motor_Driver_TB6612FNG.h"
#include <stdlib.h>

/******************************************************************************
 * porcou-arduino.ino
 * @title: Programme pour la carte Arduino
 * @project: Blender-EduTech - Jumeau numérique - Portail coulissant
 * @lang: fr
 * @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
 * @copyright: Copyright (C) 2024 Philippe Roy 
 * @license: GNU GPL
 ******************************************************************************/

/******************************************************************************
 * Précautions : 
 * - Régler l'alimentation à 12V DC - 3 A maxi
 ******************************************************************************/

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

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

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

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

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

void loop() {

    /*****
     * Lecture des capteur/boutons
     *****/
    
    /* 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); */
    
    /*****
     * Capteur / boutons : Arduino -> UPBGE
     *****/

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

    /*****
     * Moteur / Led  : UPBGE -> Arduino
     *****/
    
    if (serial_msg_complet) {
	int xy= serial_msg.toInt(); // Message par chiffre : XY

	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;
	}
    }
}