#include "Servo.h"
#include "Wire.h"
#include "MPU6050.h"
#include <stdlib.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
 ******************************************************************************/

/******************************************************************************
 * Précautions : 
 * - Régler l'alimentation à 6V DC - 3 A maxi
 * - Brochage pupitre : joystick -> A0, bouton -> D2, voyant -> D3
 * - Brochage capteur : IMU en I2C
 * - Brochage actionneurs : axes (x,y) -> D5/D6 (x,y)
 * - Mettre les servomoteurs à 45° soit 1000 µs
 ******************************************************************************/

/****************************************************************************** 
* Servomoteurs
* Alimentation par servoteur : 6V DC à 1.5 A
******************************************************************************/

Servo servo_x; // Servomoteur axe x
Servo servo_y; // Servomoteur axe y
const int servo_x_pin =  5; // Servomoteur axe x sur broche D5
const int servo_y_pin =  6; // Servomoteur axe y sur broche D6
int angle_x_ms; // Angle en µs : 500 -> 0° ; 1500->90° et 2500-> 180°
int angle_y_ms; // Angle en µs : 500 -> 0° ; 1500->90° et 2500-> 180°
const float servo_pas_maxi = 20; // Pas angulaire (µs) maxi (pas de calcul en int)
const int servo_pas_auto = 5; // Pas angulaire (µs) pour le mode auto-horizontalité
int servo_pas = 0; // Pas angulaire (µs) du mouvement en cours (pas propotionel au joystick)
    
/****************************************************************************** 
* Pupitre : joystick + 1 bouton + 1 voyant
******************************************************************************/

const int bt_pin =  2; // Bouton du mode d'auto-horizontalité sur la broche D2
const int led_pin =  3; // Led du mode d'auto-horizontalité sur la  broche D3
const int jstk_x_pin =  A1; // Joystick axe x sur la broche A1
const int jstk_y_pin =  A0; // Joystick axe y sur la broche A0

/******************************************************************************
 * Capteur 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;

bool auto_horizontal = false; // Flag du mode de la mise à l'auto-horizontalité
bool auto_horizontal_mvt = false; // Flag du mouvement de l'auto-horizontalité
const int auto_horizontal_sensibilite =  1; // Angle maxi pour l'auto-horizontalité

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

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

    // Servomoteurs
    servo_x.attach(servo_x_pin); // Brochage
    servo_y.attach(servo_y_pin); // Brochage
    angle_x_ms = 1000; // 500 -> 0°, 1500 -> 90° et 2500-> 180°
    angle_y_ms = 1000; // 500 -> 0°, 1500 -> 90° et 2500-> 180°
    servo_x.writeMicroseconds(angle_x_ms); // 1000 -> 45° */
    servo_y.writeMicroseconds(angle_y_ms); // 1000 -> 45° */

    // Pupitre
    pinMode(bt_pin, INPUT);
    pinMode(jstk_x_pin, INPUT);
    pinMode(jstk_y_pin, INPUT);
    pinMode(led_pin, OUTPUT);
  
    // IMU
    Wire.begin();
    accelgyro.initialize();
    Serial.println();
}

/******************************************************************************
 * 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();

    /*****
     * Mouvement manuel : Axe x 
     *****/

    // en C : sur x de 237 à 784 (centre à 510) et sur y de 249 à 773 (centre à 511)
    // en Python de 0,25 à 0,75 avec une zone morte entre 0,48 et 0,52 (soit 491 et 532 pour une base sur 1024)

    if ((analogRead(jstk_x_pin) <491)  or (analogRead(jstk_x_pin) >532))
    {

	// Désactivation de l'horizontalité automatique
	if (auto_horizontal) 
	{
	    auto_horizontal = false;
	    digitalWrite(led_pin, LOW);
	}

	// Mouvement x -
	if (analogRead(jstk_x_pin) <491)
	{
	    servo_pas = (servo_pas_maxi/(511-249))*(511-analogRead(jstk_x_pin));
	    angle_x_ms=angle_x_ms-servo_pas;
	}

	// Mouvement x +
	if (analogRead(jstk_x_pin) >532)
	{
	    servo_pas = (servo_pas_maxi/(773-511))*(analogRead(jstk_x_pin)-511);
	    angle_x_ms=angle_x_ms+servo_pas;
	}

	// Fin de course logiciel
	if (angle_x_ms<600)
	    angle_x_ms=600;
	if (angle_x_ms>1600)
	    angle_x_ms=1600;

	// Moteur x
	servo_x.writeMicroseconds(angle_x_ms);
    }

    /*****
     * Mouvement manuel : Axe y
     *****/

    // en C : sur x de 249 à 773 (centre à 511) et sur y de 237 à 784 (centre à 510)
    // en Python de 0,25 à 0,75 avec une zone morte entre 0,48 et 0,52 (soit 491 et 532 pour une base sur 1024)

    if ((analogRead(jstk_y_pin) <491)  or (analogRead(jstk_y_pin) >532))
    {
	// Désactivation de l'horizontalité automatique
	if (auto_horizontal) 
	{
	    auto_horizontal = false;
	    digitalWrite(led_pin, LOW);
	}

	// Mouvement y -
	if (analogRead(jstk_y_pin) <491)
	{
	    servo_pas = (servo_pas_maxi/(511-237))*(511-analogRead(jstk_y_pin));
	    angle_y_ms=angle_y_ms-servo_pas;
	}

	// Mouvement y +
	if (analogRead(jstk_y_pin) >532)
	{
	    servo_pas = (servo_pas_maxi/(784-511))*(analogRead(jstk_y_pin)-511);
	    angle_y_ms=angle_y_ms+servo_pas;
	}

	// Fin de course logiciel
	if (angle_y_ms<600)
	    angle_y_ms=600;
	if (angle_y_ms>1600)
	    angle_y_ms=1600;

	// Moteur y
	servo_y.writeMicroseconds(angle_y_ms);
    }

    /*****
     * Mise à l'horizontale automatique
     *****/

    if (digitalRead(bt_pin) == HIGH)
	auto_horizontal = true;

    if (auto_horizontal)
    {
	digitalWrite(led_pin, HIGH); // Led
	auto_horizontal_mvt = false;
	
	// Mouvement
	if ((roll_deg < -1) or (roll_deg > 1) or (pitch_deg < -1) or (pitch_deg > 1))
	    auto_horizontal_mvt = true;
	if (roll_deg < -1)
	    angle_y_ms=angle_y_ms-servo_pas_auto;
	if (roll_deg > 1)
	    angle_y_ms=angle_y_ms+servo_pas_auto;
	if (pitch_deg < -1)
	    angle_x_ms=angle_x_ms+servo_pas_auto;
	if (pitch_deg > 1)
	    angle_x_ms=angle_x_ms-servo_pas_auto;

	// Fin de course logiciel
	if (angle_x_ms<600)
	    angle_x_ms=600;
	if (angle_x_ms>1600)
	    angle_x_ms=1600;
	if (angle_y_ms<600)
	    angle_y_ms=600;
	if (angle_y_ms>1600)
	    angle_y_ms=1600;

	// Moteurs x et y
	if (auto_horizontal_mvt)
	{
	    servo_x.writeMicroseconds(angle_x_ms);
	    servo_y.writeMicroseconds(angle_y_ms);
	}
    }
}