// Auteur : Jérémie CACHOT
#include <Servo.h> // librairie pour le servomoteur
#include <NewPing.h> // librairie pour le capteur à ultrasons

// Servomoteur
const int ServoPin = 11; // Pin du servomoteur
const int ServoTime = 500; // Temps pour le mouvement du servomoteur
int Position[] = {0, 90, 180, 90}; // Toutes les positions du servomoteur possible
int NextPosition = 0;  // Indice utilisé pour la prochaine position du servomoteur
Servo MyServo; // Objet Servo 

// LEDs
const int LED1 = 5; // Pin de la première LED
const int LED2 = 6; // Pin de la deuxième LED

// Capteur à Ultrasons
const int EchoPin = 9;  // Pin Echo du capteur
const int TriggerPin = 10; // Pin Trigger du capteur
NewPing Sensor(TriggerPin, EchoPin); // Objet NewPing pour le capteur
int Distance; // Variable pour stocker la Distance mesurée

// Clignotement
bool d = true; // Variable pour le clignotement des LEDs

void setup() {
  Serial.begin(9600); // console série
  pinMode(LED1, OUTPUT); // Configuration de la broche de la première LED en sortie
  pinMode(LED2, OUTPUT); // Configuration de la broche de la deuxième LED en sortie
  analogWrite(LED1, 0); // Désactive la première LED au début
  analogWrite(LED2, 0); // Désactive la deuxième LED au début
  MyServo.attach(ServoPin); // Lie le servomoteur à la pin 
}

void LED_Blink() {
  digitalWrite(LED1, HIGH); // Allume la première LED
  digitalWrite(LED2, HIGH); // Allume la deuxième LED
  delay(500);  // Délai de 0.5s
  digitalWrite(LED1, LOW); // Éteint la première LED
  digitalWrite(LED2, LOW); // Éteint la deuxième LED
  delay(500); // Délai de 0.5s
}

void loop() {
  Distance = Sensor.ping_cm(); // Mesure la Distance à l'aide du capteur (cm)
  // Affiche la distance dans la console série
  Serial.print("Distance : "); 
  Serial.println(Distance);

  // La partie suivante est à décommenter pour voir le programme lorsque l'objet est à moins de 30cm
  // On force la distance à 20cm pour voir si les LEDs clignotent
  //Distance = 20;
  //if ((Distance == 20) && (d == true)) { 
  //Serial.print("************* Erreur *************** "); 
  //Serial.print("\nDistance : "); 
  //Serial.println(Distance);
  //Serial.print("************************************ "); 
  //}
  // Fin de la partie à décommenter pour voir quand l'objet est trop proche

  // Si l'objet est à moins de 30cm les LEDs clignotent 3 fois et on ne bouge plus le moteur
  if ((Distance < 30) && (d == true)) { 
    Serial.println("objet < 30");  // affiche ce message dans la console série
    // Clignotement 3 fois   
    for (int j = 0; j < 3; j+=1) { 
      LED_Blink();
      Serial.println(j);
      Serial.println(" blink");
      d = false;  // déclarer que le clinottement est réalisé
    }
  } 

  // Sinon le système continue
  else {
    Serial.println("objet > 30"); // affiche ce message dans la console série

    // Tant que la distance est inférieure à 30cm le système continue
    while (Distance > 30) {
      Distance = Sensor.ping_cm(); // Met à jour la la distance mesurée par le capteur

      // Permet de changer de positions
      int i = NextPosition;
      int position = Position[i];

      // Affiche dans la console série l'angle du moteur, i.e. la position
      //Serial.println(i);
      Serial.print("angle : ");
      Serial.println(position);

      analogWrite(LED1, map(position, 0, 180, 0, 255)); // Contrôle la luminosité de la première LED en fonction de la position
      analogWrite(LED2, map(position, 0, 180, 255, 0)); // Contrôle la luminosité de la deuxième LED en fonction de la position
      MyServo.write(position); // Déplace le servomoteur à la position spécifiée
      delay(ServoTime); // Attend la durée spécifiée pour le mouvement
      
      // Boucle pour faire le cycle des positions
      if (i < 3) {
        i++;
      } else {
        i = 0;
      }
      NextPosition = i;

      // Réinitialise la variable pour le prochain clignotement
      d = true;  
    }
  }    
}