//       _      ______ ____  
//      | |    |  ____/ __ \     
//      | |    | |__ | |  | |         __  ______  ____  ___  ____________
//      | |    |  __|| |  | |        / / / / __ \/ __ \/   |/_  __/ ____/
//      | |____| |___| |__| |       / / / / /_/ / / / / /| | / / / __/
//      |______|______\____/       / /_/ / ____/ /_/ / ___ |/ / / /___
//                                 \____/_/   /_____/_/  |_/_/ /_____/        24/04/2013
  

#include <Servo.h>
//____________________
//Servos
Servo servocenter; 
Servo servoright;
Servo servoleft;
Servo servohead;
Servo servogripper;
//____________________
//IR sensor
int sensorPin = 0;
int distance = 200;
int distance2= 500;
int distance3=600;
int distance4=700;
//____________________
//Movement variables
int pos1=130;//    ✓
int pos2=70;//    ✓
int pos3=70;//    ✓

int pos4=70;//    ✓
int pos5=70;//    ✓    
int pos6=110;//    ✓    

int pos7=70;//    ✓    
int pos8=110;//    ✓    
int pos9=70;//    ✓    


//____________________
//LEDS 
int led1 = 13;
int led2 = 11;
const int led3 = 12;
const int led4 = 2;
//____________________
//Blink
int ledState1 = LOW;
int ledState2 = HIGH;
long previousMillisBlink = 0;
long interval = 100;
//____________________
//Fade1
int value, value2 ;
long time=0;
int periode = 2000;
int displace = 500;
//____________________
//Fade2
unsigned long fadeMillis = 0;
boolean fadeUp = true;
int fadeValue = 0;
//____________________
//Search Timer
long previousMillisSearch = 0;        
long intervalSearch = 20000; 
//Wait Timer
long previousMillisWait = 0;        
long intervalWait = 120000;

//#########################################

void setup()
{
  //IR Sensor
  Serial.begin(9600);
  
  //Servos
  servocenter.attach(6);  
  servoright.attach(3);  
  servoleft.attach(5);
  servohead.attach(9);  
  servogripper.attach(10);
  
  //LEDS
  pinMode(led3, OUTPUT);
  pinMode(led4, OUTPUT);
}

//#########################################


//____________________
 

//____________________

//#########################################

void loop()
{
  servocenter.attach(6);  
  servoright.attach(3);  
  servoleft.attach(5);
  servohead.attach(9);  
  servogripper.attach(10);
  //__
  servohead.write(85);
  servogripper.write(90);
  
  //____________________
  Behaviourloop();
  //____________________
  
     unsigned long currentMillisSearch = millis();
    
    if(currentMillisSearch - previousMillisSearch > intervalSearch) {
      
    previousMillisSearch = currentMillisSearch, Search ();
  }
  //____________________
  
     unsigned long currentMillisWait = millis();
    
    if(currentMillisWait - previousMillisWait > intervalWait) {
      
    previousMillisWait = currentMillisWait, Wait ();
  }
  
  //____________________
  
     
}

//____________________
//____________________
//____________________
//#########################################
//_________________________________________
void Blink()
{
 unsigned long currentMillisBlink = millis();
 
  if(currentMillisBlink - previousMillisBlink > interval) {
    previousMillisBlink = currentMillisBlink;  

    if (ledState1 == LOW)
      ledState1 = HIGH;
    else
      ledState1 = LOW;

    digitalWrite(led3, ledState1);
    
      //______
      
       if (ledState2 == HIGH)
      ledState2 = LOW;
    else
      ledState2 = HIGH;

    digitalWrite(led4, ledState2);
    
  }
  
}

//_________________________________________

void fade1 ()

{
  time = millis();
  value = 128+127*cos(2*PI/periode*time);
  value2 = 128+127*cos(2*PI/periode*(displace-time));
  analogWrite(led1, value);           
  analogWrite(led2, value2);
  
    
}
//_________________________________________

void fade2()

{
  if (millis() - fadeMillis >= 5)
	  {
	    fadeMillis = millis();
	 
	    if (fadeUp == true)
	    {
	      if (fadeValue < 255)
	      {
	        fadeValue++;
	      }
	      else
	      {
	        fadeUp = false;
	        fadeValue--;
	      }
	    }
	    else
	    {
	      if (fadeValue > 0)
	      {
	        fadeValue--;
	      }
	      else
	      {
	        fadeUp = true;
	        fadeValue++;
	      }
	    }
	    analogWrite(led1, fadeValue);
	    analogWrite(led2, fadeValue);

	  }
 
	}
//_________________________________________

void Wait()

{
 
  servocenter.write(100);//    ✓
  servoright.write(90);//    ✓
  servoleft.write(90);//    ✓
  servohead.write(85);//    ✓
  servogripper.write(90);//    ✓
  
  delay(1000);
  servocenter.detach();
  servoright.detach();
  servoleft.detach();
 
  
  Middle();
  delay(500);
  Opened();
  delay(500);
  Middle();
  delay(500);
  Closed();
  
  servohead.detach();
  servogripper.detach();
  
 digitalWrite(led1, HIGH);
 digitalWrite(led2, HIGH);
 digitalWrite(led3, HIGH);
 digitalWrite(led4, HIGH);
 
 unsigned long currentMillisDelay = millis();
  
 while(millis() < currentMillisDelay + 70000 ){
 
  fade2();
  
}
}

//_________________________________________

void Play()
{
  servocenter.write(100);//    ✓
  servoright.write(90);//    ✓
  servoleft.write(90);//    ✓
  servohead.write(85);//    ✓
  servogripper.write(90);//    ✓
  
  delay(1000);
  servocenter.detach();
  servoright.detach();
  servoleft.detach(); 
  
  unsigned long currentMillisPlay = millis();
  
  while(millis() < currentMillisPlay + 70000 ){
 
  Gripper();
  
  }
}




//_________________________________________

void Behaviourloop()
{
  
  
  Serial.println(analogRead(0));
  {

    if (analogRead(0) <= distance)
    {
      Forward();  
    }
   
   
    if (analogRead(0) > distance2)
    {
      Play();          
    }
    
    
    if (analogRead(0) > distance &&  analogRead(0)< distance2) 
    {
      Search();
    }
    
  }
}
//_________________________________________

void Search ()
{
   servohead.write(55);//    ✓
   delay(600);
   if (analogRead(0) > distance &&  analogRead(0)< distance2) 
    {
    Turn_left();
    Turn_left();
    Turn_left();
    }
   servohead.write(115);//    ✓
   if (analogRead(0) > distance &&  analogRead(0)< distance2) 
    {
    Turn_right();
    Turn_right();
    Turn_right();
    }
   delay(600);
   servohead.write(85);//    ✓
 }

//_________________________________________
  


  void Forward ()
 { 
  fade1 ();
  Blink();
  
{
  
  servocenter.write(60); 
  delay(1000);
  
  fade1 ();
  Blink();
  
  for(pos2 = 70; pos2 < 110; pos2 += 1)//    ✓                             
  {
  servoright.write(pos2); 
  delay(15);
  
  }
  fade1 ();
  Blink();
  for(pos3 = 70; pos3 < 110; pos3 += 1)//    ✓
  {
  servoleft.write(pos3);
  delay(15);
  }
  
  
  
  fade1 ();
  Blink();
  
  servocenter.write(140); 
  delay(1000);
  
  fade1 ();
  Blink();
  for(pos2 = 110; pos2 >= 70; pos2 -= 1)//    ✓
  {
  servoright.write(pos2); 
  delay(15);
  }
  fade1 ();
  Blink();
  for(pos3 = 110; pos3 >= 70; pos3 -= 1)//    ✓
  {
  servoleft.write(pos3);
  delay(15);
  }
  
  
}
}
//_________________________________________


 void Turn_right()
{
  fade1 ();
  Blink();
  
{
   
  servocenter.write(140); 
  delay(1000);

  fade1 ();
  Blink();
  
  for(pos5 = 70; pos5 < 110; pos5 += 1)//    ✓
  {
  servoright.write(pos5); 
  delay(15);
  }
  fade1 ();
  Blink();
  for(pos6 = 110; pos6 >= 70; pos6 -= 1)//    ✓
  {
  servoleft.write(pos6);
  delay(15);
  }
  
  
  
  
  fade1 ();
  Blink();
  
  servocenter.write(60); 
  delay(1000);
  
  fade1 ();
  Blink();
  for(pos5 = 110; pos5 >= 70; pos5 -= 1)//    ✓
  {
  servoright.write(pos5); 
  delay(15);
  }
  fade1 ();
  Blink();
  for(pos6 = 70; pos6 < 110; pos6 += 1)//    ✓
  {
  servoleft.write(pos6);
  delay(15);
  }
}

}
//_________________________________________

void Turn_left()
{
  fade1 ();
  Blink();
  
{
   
  servocenter.write(140); 
  delay(1000);
  
  fade1 ();
  Blink();
  
  for(pos8 = 110; pos8 >= 70; pos8 -= 1)//    ✓
  {
  servoright.write(pos8); 
  delay(15);
  }
  fade1 ();
  Blink();
  for(pos9= 70; pos9 < 110; pos9 += 1)//    ✓
  {
  servoleft.write(pos9);
  delay(15);
  }
  
  
  
  
  fade1 ();
  Blink();
  
  servocenter.write(60); 
  delay(1000);
  
  fade1 ();
  Blink();
  for(pos8 = 70; pos8 < 110; pos8 += 1)//    ✓
  {
  servoright.write(pos8); 
  delay(15);
  }
  fade1 ();
  Blink();
  for(pos9 = 110; pos9 >= 70; pos9 -= 1)//    ✓
  {
  servoleft.write(pos9);
  delay(15);
  }
}

}

//_________________________________________
//Gripper
void Opened ()
{
servogripper.write(110);//    ✓
}

void Closed()
{
  servogripper.write(70);//    ✓
}
void Middle()
{
  servogripper.write(90);//    ✓
}
//_________________________________________
void Gripper()


{
  
   Serial.println(analogRead(0));
  
  {

    if (analogRead(0) <= distance3)
    {
      Closed();  
    }
   
    if (analogRead(0) > distance4)
    {
      Closed();          
    }
    if (analogRead(0) > distance3 &&  analogRead(0)< distance4) 
    {
    Opened();
    }
  }
}