/*ALAbot

Edit history
2016-08-06-11:26 gihanchanaka :
  Added sonar return time methods
  Added sonar micro second to centemeter conversion method
  @Dushan, complete the above funtions

2016-08-06-12-20 dushan:
  completed sonar distance

2016-08-06-12:25 gihanchanaka:
  Added whichSideShouldTheRobotGo()

2016-08-06-15:04 gihanchanaka:
   completed robotTurnRight(),robotTurnLeft().robotGoForward() functions

2016-08-06-15:09 gihanchanaka:
  debugging

2016-08-08-11:56 gihanchanaka:
  removed the turnRight,turnLeft,goForward functions
  added LED direction indication functions and defined pin numbers
  changes the whichDirectionToGo untion to indicate using LEDs instead of the mechanical movement
*/

//The following constants are used to define the pins used for sonar
const int sonarRightPingPin=7;
const int sonarForwardPingPin=5;
const int sonarForwardTriggerPin=6;
const int sonarLeftTriggerPin=-1;
const int sonarRightTriggerPin=4;


//Direction indication pin numbers
const int groundForDirectionLEDs=12;
const int  rightDirectionLED=8;
const int forwardDirectionLED=-1;
const int leftDirectionLED=-1;


//This is used to maintain the gap and also decide when to turn
const int gapBetweenRobotAndWall=5;

//This data stores the time needed for the motors to turn the robot by 90 degrees
const int microSecondsForATurn=0;
const int percentageSpeedInTurning=0;

int sonarReturnTimeMicroSecondsForward(){

  // establish variables for duration of the ping,
  // and the distance result in inches and centimeters:
  long duration, forwardcm;
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
   pinMode(sonarForwardTriggerPin, OUTPUT);
   digitalWrite(sonarForwardTriggerPin, LOW);
   delayMicroseconds(2);
   digitalWrite(sonarForwardTriggerPin, HIGH);
   delayMicroseconds(5);
   digitalWrite(sonarForwardTriggerPin, LOW);

   pinMode(sonarCommonPingPin, INPUT);
   duration = pulseIn(sonarCommonPingPin, HIGH);
   //Serial.println(duration);
   // convert the time into a distance
   forwardcm = sonarMilliSecondsToCentiMeters(duration);
   return forwardcm ;

}

int sonarReturnTimeMicroSecondsLeft(){

  // establish variables for duration of the ping,
  // and the distance result in inches and centimeters:
  long duration, leftcm;
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
   pinMode(sonarLeftTriggerPin, OUTPUT);
   digitalWrite(sonarLeftTriggerPin, LOW);
   delayMicroseconds(2);
   digitalWrite(sonarLeftTriggerPin, HIGH);
   delayMicroseconds(5);
   digitalWrite(sonarLeftTriggerPin, LOW);

   pinMode(sonarCommonPingPin, INPUT);
   duration = pulseIn(sonarCommonPingPin, HIGH);
   //Serial.println(duration);
   // convert the time into a distance
   leftcm = sonarMilliSecondsToCentiMeters(duration);

   return leftcm ;

}

int sonarReturnTimeMicroSecondsRight(){

  // establish variables for duration of the ping,
  // and the distance result in inches and centimeters:
  long duration, rightcm;
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
   pinMode(sonarRightTriggerPin, OUTPUT);
   digitalWrite(sonarRightTriggerPin, LOW);
   delayMicroseconds(2);
   digitalWrite(sonarRightTriggerPin, HIGH);
   delayMicroseconds(5);
   digitalWrite(sonarRightTriggerPin, LOW);

   pinMode(sonarCommonPingPin, INPUT);
   duration = pulseIn(sonarCommonPingPin, HIGH);
   //Serial.println(duration);
   // convert the time into a distance
   rightcm = sonarMilliSecondsToCentiMeters(duration);
   return rightcm ;




}

int sonarMilliSecondsToCentiMeters(int microSeconds){

   // The speed of sound is 340 m/s or 29 microseconds per centimeter.
   // The ping travels out and back, so to find the distance of the
   // object we take half of the distance travelled.
   return microSeconds / 29 / 2;

}


int whichSideShouldTheRobotGo(){
  int distanceRight=sonarReturnTimeMicroSecondsRight();
  int distanceForward=sonarReturnTimeMicroSecondsForward();
  int distanceLeft=sonarReturnTimeMicroSecondsLeft();
  Serial.println(distanceRight);
  Serial.println(distanceForward);
  Serial.println(distanceLeft);

  if(distanceRight>gapBetweenRobotAndWall){
    turnRightLED();
  }
  else{
    if(distanceForward>gapBetweenRobotAndWall){
      goForwardLED();
    }
    else{
      turnLeftLED();
    }
  }

}



//function to set the pins for direction LED indicators
void setDirectionIndicationLED(){
  pinMode(groundForDirectionLEDs,OUTPUT);
  pinMode(rightDirectionLED,OUTPUT);
  pinMode(forwardDirectionLED,OUTPUT);
  pinMode(leftDirectionLED,OUTPUT);
  allDirectionLEDPinsToLow();
}
void allDirectionLEDPinsToLow(){
  //digitalWrite(groundForDirectionLED,LOW);
  digitalWrite(rightDirectionLED,LOW);
  digitalWrite(forwardDirectionLED,LOW);
  digitalWrite(leftDirectionLED,LOW);
}
//functions to switch o LEDs to indicate directions
void turnRightLED(){
  allDirectionLEDPinsToLow();
  digitalWrite(rightDirectionLED,HIGH);
}
void goForwardLED(){
  allDirectionLEDPinsToLow();
  digitalWrite(forwardDirectionLED,HIGH);
}
void turnLeftLED(){
  allDirectionLEDPinsToLow();
  digitalWrite(leftDirectionLED,HIGH);
}


void loop(){
  whichSideShouldTheRobotGo();
  delay(1);
}

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