/*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
*/

//The following constants are used to define the pins used for sonar
const int sonarCommonPingPin;
const int sonarForwardTriggerPin;
const int sonarLeftTriggerPin;
const int sonarRightTriggerPin;


//The following constants are used to define the interegers returned in the direction choosing method
const int goRight=1;
const int goForward=2;
const int goLeft=2;


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

//These data is used to configure the motor pins
//motorOne RIGHT side mortor
//motorTwo LEFT side mortor
const int motorOneDirection;
const int motorTwoDirection;
const int motorOneBrake;
const int motorTwoBrake;
const int motorOneSpeed;
const int motorTwoSpeed;

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

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);

   // 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);

   // 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);

   // 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=sonarMilliSecondsToCentiMeters(sonarReturnTimeMicroSecondsRight());
  int distanceForward=sonarMilliSecondsToCentiMeters(sonarReturnTimeMicroSecondsForward());
  int distanceLeft=sonarMilliSecondsToCentiMeters(sonarReturnTimeMicroSecondsLeft());

  if(distanceRight>gapBetweenRobotAndWall){
    robotTurnRight();
  }
  else{
    if(distanceForward>gapBetweenRobotAndWall){
      robotGoForward(100);
    }
    else{
      robotTurnLeft();
    }
  }

}

void setPinModeForMotorPins(){
  //This pinMode should go inside the setup;
  pinMode(motorOneDirection,OUTPUT);
  pinMode(motorOneBrake,OUTPUT);
  pinMode(motorOneSpeed,OUTPUT);
  pinMode(motorTwoDirection,OUTPUT);
  pinMode(motorTwoBrake,OUTPUT);
  pinMode(motorTwoSpeed,OUTPUT);
}

void robotGoForward(int speedPercentage){
  setPinModeForMotorPins();

  digitalWrite(motorOneDirecton,HIGH);
  digitalWrite(motorTwoDirecton,HIGH);
  digitalWrite(motorOneBrake,LOW);
  digitalWrite(motorTwoBrake,LOW);

  analogWrite(motorOneSpeed,(255*speedPercentage)/100);
  analogWrite(motorTwoSpeed,(255*speedPercentage)/100);

}

void robotTurnRight(){
  setPinModeForMotorPins();

  digitalWrite(motorOneDirecton,LOW);
  digitalWrite(motorTwoDirecton,HIGH);
  digitalWrite(motorOneBrake,LOW);
  digitalWrite(motorTwoBrake,HIGH);

  analogWrite(motorOneSpeed,(255*0)/100);
  analogWrite(motorTwoSpeed,(255*percentageSpeedInTurning)/100);
  delay(microSecondsForATurn);
}

void robotTurnLeft(){
  setPinModeForMotorPins();

  digitalWrite(motorOneDirecton,HIGH);
  digitalWrite(motorTwoDirecton,LOW);
  digitalWrite(motorOneBrake,HIGH);
  digitalWrite(motorTwoBrake,LOW);

  analogWrite(motorOneSpeed,(255*percentageSpeedInTurning)/100);
  analogWrite(motorTwoSpeed,(255*0)/100);
  delay(microSecondsForATurn);
}