#include <Servo.h>
Servo servo;
int angle;
int outputUS;

// Ultrasonic 
int trigPin = 3;    // Trigger
int echoPin = 4;    // Echo
long duration, cm, inches;

int RED = 11;
int GREEN = 13;
int BLUE = 12;

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

  // put your setup code here, to run once:
  //Servo motor 
  servo.attach(2);
  servo.write(angle);

  pinMode(RED, OUTPUT);
  pinMode(GREEN, OUTPUT);
  pinMode(BLUE, OUTPUT);

  // Ultra sonice Sensors
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop(void) {
servo.write(90);
int objDecValue  = UltraSonic(30);
  if(objDecValue == 0)
  {
    Serial.println("Object Found! Car Stop and checking side angle");
    redOn();
    if(chkLeftSide(60) == 0)
    {
      delay(5000);
      if(chkLeftSide(30) == 0)
      {
        delay(5000);
        if(chkLeftSide(0) == 0)
        {
          delay(5000);
          if(chkLeftSide(120) == 0)
          {
            delay(5000);
            if(chkLeftSide(120) == 0)
            {
              delay(5000);
              if(chkLeftSide(150) == 0)
              {
                delay(5000);
                chkLeftSide(180);            
              }
            }
          }
        }
      }
    }
  }
  else
  {
    Serial.println("Object Not found! Car Moving ");
    greenOn();
  }
}

int chkLeftSide(int ServoAngle)
{
    blueOn();
    int objectDectValue;
    servo.write(ServoAngle);    
    Serial.println("Function :chkLeftSide  Set Servo Motor angle to "+String(ServoAngle)+ " degrees ! Checking object");
    if(UltraSonic(30) == 1)
    {
      Serial.println("Function :chkLeftSide  Object Not found. Moving car to left side to " +String(ServoAngle)+ " and car moving");
      greenOn();
      objectDectValue=1;
    }
    else
    {
    Serial.println("Function :chkLeftSide  Object still found in angle "+String(ServoAngle)+" ! Set another angle");
    redOn();
    objectDectValue=0;
    }  

  return objectDectValue;
}

void MoveMotor()
{
  Serial.println("Moving Motor object using inputs");

}

int UltraSonic(int ObjectDistance)
{
    int USValue;
    // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
    // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(5);
    digitalWrite(trigPin, LOW);
  
    // Read the signal from the sensor: a HIGH pulse whose
    // duration is the time (in microseconds) from the sending
    // of the ping to the reception of its echo off of an object.
    pinMode(echoPin, INPUT);
    duration = pulseIn(echoPin, HIGH);
  
    // Convert the time into a distance
    cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
    inches = (duration/2) / 74;   // Divide by 74 or multiply by 0.0135
      if(cm <= ObjectDistance) 
      {
        // Serial.println("Object Dected");
        USValue=0;
      }
      else if (cm > ObjectDistance)
      {
        // Serial.println("Object Not Dected");
        USValue=1;
      }
    delay(100); 
    return USValue;
}

void redOn(void)
{
  digitalWrite(GREEN, LOW);
  digitalWrite(RED, HIGH);
  digitalWrite(BLUE, LOW);
}

void greenOn(void)
{
  digitalWrite(GREEN, HIGH);
  digitalWrite(RED, LOW);
  digitalWrite(BLUE, LOW);
}
void blueOn(void)
{
  digitalWrite(GREEN, LOW);
  digitalWrite(RED, LOW);
  digitalWrite(BLUE, HIGH);
}

void ledOff(void)
{
  digitalWrite(GREEN, LOW);
  digitalWrite(RED, LOW);
  digitalWrite(BLUE, LOW);
}