//-- MOTEUR A --
int ENA=5; //Connecté à Arduino pin 5(sortie pwm)
int IN1=2; //Connecté à Arduino pin 2
int IN2=3; //Connecté à Arduino pin 3

//-- MOTEUR B --
int ENB=6; //Connecté à Arduino pin 6(Sortie pwm)
int IN3=4; //Connecté à Arduino pin 4
int IN4=7; //Connecté à Arduino pin 7

//-- SERVO --
#include <Servo.h>
Servo myservo;
int pos=0;
int pinServo=9;

//-- Sensor InfraRed --
int sensorPin = 0;

//-- PiezoSpeaker --
int piezoPin = 10;
int length = 22; // the number of notes
char notes[] = "gC  DCDE  C ED CB DC  "; // a space represents a rest
float beats[] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
                
int tempo = 95;
void playTone(int ton1, int duration) {
  for (long i = 0; i < duration * 1000L; i += ton1) {
    tone(piezoPin, ton1);
    delayMicroseconds(ton1);
  }
  noTone(piezoPin);
}
void playNote(char note, int duration) {
//                        c    c#   d    d#   e    f    f#   g    g#   a    a#   b
  char names[] = { ' ',  '!', '2', '3', '4', '5', '6', '7', '8', '9', '0', '-', '=', 'c', 'd', 'e', 'f', 'g', 'a', 'b', 'C', 'D', 'E', 'F', 'G', 'A', 'B', 'i', 'N', 'R', 'u',  '1', 'L', 'k'}; // [i = b flat] [N = G flat] [R = D#] [u = g#] [1 = C oct. 5] [L = E flat]
  int tones[] =  {   0, 1046, 138, 146, 155, 164, 174, 184, 195, 207, 220, 233, 246, 261, 293, 329, 349, 391, 440, 493, 523, 587, 659, 698, 783, 880, 987, 466, 740, 622, 415, 1046, 622u, 227};
  
  // play the tone corresponding to the note name
  for (int i = 0; i < 34; i++) {
    if (names[i] == note) {
      playTone(tones[i], duration);
    }
  }
}


void setup() {
 pinMode(ENA,OUTPUT);//Configurer les broches comme sortie
 pinMode(ENB,OUTPUT);
 pinMode(IN1,OUTPUT);
 pinMode(IN2,OUTPUT);
 pinMode(IN3,OUTPUT);
 pinMode(IN4,OUTPUT);
 digitalWrite(ENA,LOW);// Moteur A - Ne pas tourner (désactivation moteur)
 digitalWrite(ENB,LOW);// Moteur B - Ne pas tourner (désactivation moteur)
  // Direction du Moteur A
 digitalWrite(IN1,LOW); 
 digitalWrite(IN2,LOW);

 // Direction du Moteur B
 digitalWrite(IN3,LOW);
 digitalWrite(IN4,LOW);
 
 myservo.attach(pinServo);
 myservo.write(90);
 
 pinMode(piezoPin,OUTPUT);
 for (int i = 0; i < length; i++) {
    if (notes[i] == ' ') {
      delay(beats[i] * tempo); // rest
    } else {
      playNote(notes[i], beats[i] * tempo);
    }
    
    // pause between notes
    delay(tempo / 2); 
  }

}
  
//--- Variables globales ---
  int var=1;
  float tensM;
  float tensL;
  float tensR;
  
void loop() {

  float grenze1 = 0.70 ;
  float grenze2 = 1.35 ;
  int stopTime=200;
  int backTime=2000;
  int turnTime=1300;
  
  switch(var){
    case 1://forward and check 90
      tensM = check(90);
      if (tensM<=grenze1){
        var=1;
        motA(1,255);
        motB(1,255);
        delay(stopTime);
      }
      else if (tensM>grenze1 && tensM<=grenze2){
        var=2;
        motA(1,200);
        motB(1,200);
      }
      else {
        motA(0,0);
        motB(0,0);
        tensL=check(30);
        tensR=check(150);
        var=3;
      }
    break;
  
    case 2://slow down and check sides
      tensL=check(30);
      tensR=check(150);
      if(tensL<=grenze1 && tensR<=grenze1){
        var=1;
      }
      else if(tensL<=grenze2 && tensR<=grenze2 && (tensL>grenze1 || tensR>grenze1)){
        if(tensL<tensR){
          motA(1,255);
          motB(0,0);
        }
        else {
          motA(0,0);
          motB(1,255);
        }
        delay(turnTime);
        var=1;
      }
      else if (tensL>grenze2 || tensR>grenze2){
        motA(0,0);
        motB(0,0);
        delay(stopTime);
        var=3;
      }
    break;
    
    case 3://stop and check sides
      if(tensL<=grenze2 || tensR<=grenze2){
        if(tensL<tensR){
          motA(0,0);
          motB(-1,250);
        }
        else {
          motA(-1,250);
          motB(0,0);
        }
      }
      else if (tensL>grenze2 && tensR>grenze2){
        motA(-1,250);
        motB(-1,250);
      }
      back(backTime);
      motA(0,0);
      motA(0,0);
      delay(stopTime);
      motA(1,200);
      motB(1,200);
      var=2;
    break;   
 
    default:
      motA(0,0);
      motB(0,0);
      tone(piezoPin,400,2000);
      
  }
}

void back(int backTime){
  for(int i=1; i<4; i++){
    tone(piezoPin,600,backTime/6);
    delay(backTime/3);
  }
}

float check(int angle)
{
  int currentPos=myservo.read();
  int sign=0;
  int pos=currentPos;
  int turnHeadSpeed = 4;
  int headWait = 50;
  if((angle-currentPos) < 0)
    {sign=-1;}
  else if((angle-currentPos) > 0)
    {sign=+1;}
    
  if(sign!=0){
    for(pos=currentPos; pos!=angle; pos+=sign){
      myservo.write(pos);
      delay(turnHeadSpeed);
    }
  }
  delay(headWait);
  return lectureTension();
}

void motA(int sens, int vitesse)
{
  if(sens==1)
  {
    digitalWrite(ENA,LOW);
    digitalWrite(IN1,HIGH);
    digitalWrite(IN2,LOW);
    analogWrite(ENA,vitesse);
  }
  else if (sens==-1)
  {
    digitalWrite(ENA,LOW);
    digitalWrite(IN1,LOW); 
    digitalWrite(IN2,HIGH);
    analogWrite(ENA,vitesse);
  }   
  else if (sens==0)
  {
    digitalWrite(ENA,LOW);
  }
}

void motB(int sens, int vitesse)
{
  if(sens==1)
  {
    digitalWrite(ENB,LOW);
    digitalWrite(IN3,LOW); 
    digitalWrite(IN4,HIGH);
    analogWrite(ENB,vitesse);
  }
  else if (sens==-1)
  {
    digitalWrite(ENB,LOW);
    digitalWrite(IN3,HIGH); 
    digitalWrite(IN4,LOW);
    analogWrite(ENB,vitesse);
  }   
  else if (sens==0)
  {
    digitalWrite(ENB,LOW);
  }
}

float lectureTension(){
 // Lecture de la valeur sur l'entrée analogique
 // Retourne une valeur entre 0->1024 pour 0->5v
 int valeur = analogRead(sensorPin);  

 // Converti la lecture en tension
 float tension = valeur * 5.0;
 tension /= 1024.0; 
 
 return tension;
}