#include <Servo.h>
#include <Wire.h>
// Déclaration des servomoteurs
Servo servoBas;  // Servomoteur pilotant l'azimuth
Servo servoHaut; // Servomoteur pilotant l'elevation

/////////////////////////////////
///  DECLARATION DES BROCHES  ///
/////////////////////////////////

// PIN LDR (Sens: vue de face)
int LDR_HAUT = A3;
int LDR_BAS = A1;
int LDR_GAUCHE = A0;
int LDR_DROITE = A2;

//PIN LEDs
int LED_H = 7;
int LED_B = 4;
int LED_G = 6;
int LED_D = 5;

// PIN SERVOMOTEURS
int pin_servoBas = 9;
int pin_servoHaut = 10;


/////////////////////////////////
/// DECLARATION DES VARIABLES ///
/////////////////////////////////

// VARIABLES DE LECTURE DES LDRs
int LDR_H=0;
int LDR_B=0;
int LDR_G=0;
int LDR_D=0;

// Variables de calculs
int erreur = 30; // Erreur toleree entre les valeurs renvoyees par les photoresistances. Plus cette erreur est petite, plus le système sera nerveux (et risque d'être instable)

// Programme d'initialisation. S'execute une seule fois, en début de programme
void setup() {

  // Les servomoteurs sont déclarés et une premiere valeur leur est donnee, afin d'eviter une initialisation trop brutale
  servoBas.attach(pin_servoBas);  // Attache le servo du bas
  servoBas.write(90);
  servoHaut.attach(pin_servoHaut); // Attache le servo du haut
  servoHaut.write(160);
 
  Serial.begin(9600); // Lance la communication serie a 9600 bits/s
  Wire.begin(); // joindre le bus i2c (adresse est optionnelle pour un maître)
/// DECLARATION DES ENTREES ET SORTIES ///
  // Les LDR sont declarees comme des entrees:
  pinMode(LDR_HAUT, INPUT);
  pinMode(LDR_BAS, INPUT);
  pinMode(LDR_GAUCHE, INPUT);
  pinMode(LDR_DROITE, INPUT);

  // Les LEDs sont declarees comme des sorties:
  pinMode(LED_H, OUTPUT);
  pinMode(LED_B, OUTPUT);
  pinMode(LED_D, OUTPUT);
  pinMode(LED_G, OUTPUT);

}

/////////////////////////////////
///       SOUS-FONCTIONS      ///
/////////////////////////////////

// Fonction permettant de piloter un servo a partir d'une valeur donnee dans le programme
void w_servo(int servo, int val) {
  switch(servo) { // Selection du servomoteur
    case 0: 
      servoBas.write(limitS(0,val));                  
      break;
    case 1:
      servoHaut.write(limitS(1,val));
      break;
  }
}

// Fonction test des LDR (but: verifier le bon fonctionnement des LDR et fixer une erreur), allume les LEDs associees aux capteurs
void testLDR(){
  
  //Lecture des valeurs renvoyees par les LDR:
  LDR_H=analogRead(LDR_HAUT);
  LDR_B=analogRead(LDR_BAS);
  LDR_G=analogRead(LDR_GAUCHE);
  LDR_D=analogRead(LDR_DROITE);

 // Paire haut-bas
 if (abs(LDR_H-LDR_B) > erreur) {   // Si l'ecart entre les 2 LDR est significatif, on allume la LED correspondant a la LDR la plus eclairee
  if (LDR_H > LDR_B) {
    digitalWrite(LED_H, HIGH);
    digitalWrite(LED_B, LOW);
  }
  else{
    digitalWrite(LED_H, LOW);
    digitalWrite(LED_B, HIGH);
  }
 }
 else {   // Sinon, on éteint les LEDs
  digitalWrite(LED_H, LOW);
  digitalWrite(LED_B, LOW);
 }

 // Paire gauche-droite
 if (abs(LDR_G-LDR_D) > erreur) {  // Si l'ecart entre les 2 LDR est significatif, on allume la LED correspondant a la LDR la plus eclairee
  if (LDR_G > LDR_D) {
    digitalWrite(LED_G, HIGH);
    digitalWrite(LED_D, LOW);
  }
  else{ 
    digitalWrite(LED_G, LOW);
    digitalWrite(LED_D, HIGH);
  }
 }
 else {   // Sinon, on éteint les LEDs
  digitalWrite(LED_G, LOW);
  digitalWrite(LED_D, LOW);
 } 

  
}

// Asservissement en position des servomoteurs
void asserv() {
  
  
  int angleActuel0;
  int angleNouveau0=servoBas.read();
  int angleActuel1;
  int angleNouveau1=servoHaut.read();
  
  //Lecture des valeurs renvoyees par les LDR:
  LDR_H=analogRead(LDR_HAUT);
  LDR_B=analogRead(LDR_BAS);
  LDR_G=analogRead(LDR_GAUCHE);
  LDR_D=analogRead(LDR_DROITE);

if(LDR_H < 400 && LDR_B < 400 && LDR_G < 400 && LDR_D < 400){ // cette boucle permet d'effectuer rotation uniquement s'il fait jour
   w_servo(0,10); 
   w_servo(1,160); // s'il fait nuit, le robot se met en position pour attendre le soleil le lendemain matin
}
else{ 
  // Paire haut-bas
 if (abs(LDR_H-LDR_B) > erreur) {
  // Si c'est plus lumineux en haut, il faut relever le panneau
  if (LDR_H > LDR_B) {
   angleActuel1 =  servoHaut.read();
   angleNouveau1 = angleActuel1-1;    // Decrementer l'angle de commande = relever le panneau
  }
  else{
   angleActuel1 = servoHaut.read();
   angleNouveau1 = angleActuel1+1;    // Incrementer l'angle = baisser le panneau
  }
 }
 w_servo(1,angleNouveau1);  // Pilotage du servomoteur. 
 
  // Paire gauche-droite
  if (abs(LDR_G-LDR_D) > erreur) {
    // Si c'est plus lumineux a gauche, il faut tourner le panneau vers la "droite"
    if (LDR_G > LDR_D) {
     angleActuel0 = servoBas.read();
     angleNouveau0 = angleActuel0-1;
    }
    else{
     angleActuel0 = servoBas.read();
     angleNouveau0 = angleActuel0+1;
    }
   }
   w_servo(0,angleNouveau0);
delay(50);  
}
}

// Fonction de limite d'angle pour les servomoteurs
// Permet de fixer des butees angulaires et d'empêcher des collisions ou blocage/forçage mecanique
int limitS(int i, int angle) {  // i=0 pour le servo du bas, i=1 pour le servo du haut 
  
  int angle2 = angle; // Nouvel angle de sortie
  // Declaration de variables qui seront les limites angulaires du servomoteur (en degres)
  int limBasse;
  int limHaute;
  
  // Definition des limites angulaires selon le servomoteur (en degrés)
  switch(i) {
    case 0: // Servomoteur du bas 
      limBasse = 5;
      limHaute = 175;
      break;
    case 1: // Servomoteur du haut
      limBasse = 90;
      limHaute = 175;
      break;
    default: break;
  }

  // Si l'angle demande au servo depasse les limites, on fixe cet angle a la limite depassee
  if (angle > limHaute) {
    angle2 = limHaute;
  }
   if (angle < limBasse) {
    angle2 = limBasse;
  }

  return angle2;
  
}

// FONCTIONS DE DEBUG

// Lecture et ecriture des valeurs renvoyees par les LED haut et bas
void lectureHB() {
  LDR_H = analogRead(LDR_HAUT);
  LDR_B = analogRead(LDR_BAS);
   Serial.print("           , ");
  Serial.print(LDR_H);
  Serial.print("   , ");
  Serial.print(LDR_B);
  Serial.println();
  delay(100);
}

// Lecture et ecriture des valeurs renvoyees par les LDR gauche et droite
void lectureGD() {
  LDR_G = analogRead(LDR_GAUCHE);
  LDR_D = analogRead(LDR_DROITE);
  Serial.print(LDR_G);
  Serial.print(", ");
  Serial.print(LDR_D);
  Serial.println();
  
}

void mesureangle(){
 int angleActuel0=servoBas.read();
 int angleActuel1=servoHaut.read();
 
  Serial.print(angleActuel0);
  Serial.print("   , ");
  Serial.print(angleActuel1);
  Serial.println();
  delay(100);
 
}
void envoidonnees(){

  int x = servoBas.read();
  int y =  servoHaut.read();
  
  Wire.beginTransmission(4); // Commencer transmission vers l'esclave  #4
  Wire.write("x is ");       // Envoi de 5 octets (5 bytes)
  Wire.write(x);             // envoi d'un byte/octet (valeur numérique)
  Wire.write("                 y is ");       // Envoi de 5 octets (5 bytes)
  Wire.write(y);             // envoi d'un byte/octet (valeur numérique)  
  Wire.endTransmission();    // fin transmission
  delay(500); // delay de 500 ms
  
}
/////////////////////////////////
///   FONCTION PRINCIPALE     ///
/////////////////////////////////

void loop() {
  //asserv();
  //lectureGD();
  //testLDR();
  //lectureHB();
  //mesureangle();
  envoidonnees();
}
