/*
 This program is to test up to three ultrasonic sensors
 It prints the value of the sensors each second.
*/
void readUltrasonicSensorsValue ();
void printUltrasonicSensorsValue ();
void measureIRSensors();
//Definition of the constant that are going to be used
    int IRsensorLeftPin = A0;
    int IRsensorFrontPin = A1;
    int IRsensorRightPin = A2;
    int valIRsensorLeft= 0;
    int valIRsensorFront= 0;
    int valIRsensorRight= 0;
    
    int ultraSonicSensorLeftPin = A3;   //this give the pin number used.
    int ultraSonicSensorFrontPin = A4; 
    int ultraSonicSensorRightPin = A5;

    int valUltraSonicLeftSensor= 0;
    int valUltraSonicFrontSensor= 0;
    int valUltraSonicRightSensor= 0;
    
    //Leds
    int LED1pinNumber = 3;
    int LED2pinNumber = 4;
    int LED3pinNumber = 5;
    int LED4pinNumber = 6;
    int LED5pinNumber = 7;
    int LED6pinNumber = 8;
    
    // motor configuration
    #include <Servo.h>

    Servo myservoM1;               //create the object "myservo". The name can change of course.
    int servoPinNumberM1 = 10;      //Note : on some arduino it is only pin 9 and 10 that can be used for mservomotors.
    Servo myservoM2;               
    int servoPinNumberM2 = 9;
    
    // working values
    int ultraSonicMax=0;
    int noize=60;
    int action=0;
    int detectionIR=115;
    

void setup()
{
	Serial.begin(9600);  //

	pinMode(ultraSonicSensorLeftPin, INPUT);  //In fact useless because the pins are in INPUT by default
        pinMode(ultraSonicSensorFrontPin, INPUT);
	pinMode(ultraSonicSensorRightPin, INPUT);
        pinMode(IRsensorLeftPin, INPUT);  //Pinmode for IR sensors
        pinMode(IRsensorFrontPin, INPUT);
        pinMode(IRsensorRightPin, INPUT);
        
        
          //Pinmode for LEDS very important. Check if every LEDS is on Output else they will be dull.
        pinMode(LED1pinNumber, OUTPUT);
        pinMode(LED2pinNumber, OUTPUT);
        pinMode(LED3pinNumber, OUTPUT);
        pinMode(LED4pinNumber, OUTPUT);
        pinMode(LED5pinNumber, OUTPUT);
        pinMode(LED6pinNumber, OUTPUT);
        
         myservoM1.attach(servoPinNumberM1);  //give the pin on wich the servo is located
         myservoM2.attach(servoPinNumberM2);
        
        stand();
        
}


void loop()   // MAIN PROGRAM HERE!
{
    readUltrasonicSensorsValue();        // Read the sensors values
    measureIRSensors();
    decision();
    mouvement();
    printUltrasonicSensorsValue();      // Print the sensors values
    delay(300);                // Wait for a second

}

void readUltrasonicSensorsValue ()
{
    valUltraSonicLeftSensor  = analogRead(ultraSonicSensorLeftPin);  //read value of the sensors
    valUltraSonicFrontSensor = analogRead(ultraSonicSensorFrontPin); 
    valUltraSonicRightSensor = analogRead(ultraSonicSensorRightPin)*0.8;
}

void measureIRSensors()
{
  digitalWrite(LED4pinNumber, LOW);
  digitalWrite(LED5pinNumber, LOW);
  digitalWrite(LED6pinNumber, LOW);
  
  valIRsensorLeft  = analogRead(IRsensorLeftPin);
  if (valIRsensorLeft>detectionIR)
  {
    digitalWrite(LED4pinNumber, HIGH);
  }
  valIRsensorFront = analogRead(IRsensorFrontPin);
   if (valIRsensorFront>detectionIR)
  {
   digitalWrite(LED5pinNumber, HIGH);
  }
  valIRsensorRight = analogRead(IRsensorRightPin);
   if (valIRsensorRight>detectionIR)
  {
    digitalWrite(LED6pinNumber, HIGH);
  }
}

void decision()
{  action=0; // don't move
   digitalWrite(LED1pinNumber, LOW);
   digitalWrite(LED2pinNumber, LOW);
   digitalWrite(LED3pinNumber, LOW);

  if (valUltraSonicRightSensor > valUltraSonicFrontSensor+noize || valUltraSonicLeftSensor > valUltraSonicFrontSensor+noize)
     {action=2;  //go forward
     digitalWrite(LED2pinNumber, HIGH);}

  if (valUltraSonicLeftSensor > valUltraSonicRightSensor+4*noize)
     {action=1; // to the left
     digitalWrite(LED1pinNumber, HIGH);
     digitalWrite(LED2pinNumber, LOW);}
  
  if (valUltraSonicRightSensor > valUltraSonicLeftSensor+4*noize)
     {action=3; // to the right
     digitalWrite(LED3pinNumber, HIGH);
     digitalWrite(LED2pinNumber, LOW);}
  
}

void mouvement()  // after the decison, check if the motion is possible and if yes, execute the motion
{

  if (valIRsensorFront>detectionIR || valIRsensorRight>detectionIR || valIRsensorLeft>detectionIR)  // if obstacle, careful
      {stand();
       if (valIRsensorRight>detectionIR && action==1 && valIRsensorFront<detectionIR)
           {goLeft();}
       if (valIRsensorLeft>detectionIR && action==3 && valIRsensorFront<detectionIR)
          {goRight();}
      }
      
   if (valIRsensorFront<detectionIR && valIRsensorRight<detectionIR && valIRsensorLeft<detectionIR)  // if no obstacle go to the target
      {
        if(action==2)
        {goForward();}
        if(action==1)
        {goLeft();}
        if(action==3)
        {goRight();}
        if(action==0)
        {stand();}
      
      }

}

// MOTOR FUNCTION


void stand()    
{
  myservoM1.write (90);
  myservoM2.write (90);
  delay (300);
}


void goForward()    
{
  myservoM1.write (90+30);
  myservoM2.write (90-30);
  delay (100);
  myservoM1.write (90+10);
  myservoM2.write (90-10);
  delay (100);
  myservoM1.write (90+15);
  myservoM2.write (90-15);
  delay (60);
  myservoM1.write (90+20);
  myservoM2.write (90-20);
  delay (300);
}

void goLeft()    
{
  myservoM1.write (90+20-15);
  myservoM2.write (90-20-15);
  delay (300);
}

void goRight()    
{
  myservoM1.write (90+20+15);
  myservoM2.write (90-20+15);
  delay (300);
}



void printUltrasonicSensorsValue () //prints the value of the IR sensors on the serial monitor
{
   Serial.print("Left sound sensor ");
   Serial.println(valUltraSonicLeftSensor);
   Serial.print("Front sound sensor ");
   Serial.println(valUltraSonicFrontSensor);
   Serial.print("Right sound sensor ");
   Serial.println(valUltraSonicRightSensor);
   Serial.print("Left IR sensor ");
   Serial.println(valIRsensorLeft);
   Serial.print("Front IR sensor ");
   Serial.println(valIRsensorFront);
   Serial.print("Right IR sensor ");
   Serial.println(valIRsensorRight);
   Serial.print("calculation ");
   
   Serial.println(valUltraSonicFrontSensor+noize);
   Serial.println(valUltraSonicLeftSensor);
   Serial.println(valUltraSonicLeftSensor > valUltraSonicFrontSensor+noize);
   
   Serial.println(valUltraSonicFrontSensor+noize);
   Serial.println(valUltraSonicRightSensor);
   Serial.println(valUltraSonicRightSensor > valUltraSonicFrontSensor+noize);
   
   Serial.println(action);
   
   
   
   
   
   
}