/*
driveTrain.c - Follows an object a certain distance away from it using 2 ultrasonic sensors

*/


//Pins used to control direction and speed of the motor. Speed pin should be a pwm pin.
#define MotorDirectionLeft 8
#define MotorSpeedLeft 9
#define MotorDirectionRight 12
#define MotorSpeedRight 11

#define leftTrigPin 4 // TODO: Double check these are correct ports // TRIG pin
#define leftEchoPin 5 // ECHO pin,
#define rightTrigPin 6
#define rightEchoPin 7

int SpeedVal = 0;

void setup() {
  //Declaration for the pins used, both should be outputs.
  Serial.begin(9600);
  pinMode(MotorDirectionLeft, OUTPUT);
  pinMode(MotorSpeedLeft, OUTPUT);
  pinMode(MotorDirectionRight, OUTPUT);
  pinMode(MotorSpeedRight, OUTPUT);
  pinMode(leftTrigPin, OUTPUT);
  pinMode(rightTrigPin, OUTPUT);
  pinMode(leftEchoPin,INPUT);
  pinMode(rightEchoPin,INPUT);
  analogWrite(MotorSpeedLeft, 0);
  analogWrite(MotorSpeedRight,0);

}

void loop() {
  long duration1, inches1, cm1;
  long duration2, inches2, cm2;
  // Get the ultrasonic sensors to gather distances
  // Left Sensor
  digitalWrite(leftTrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(leftTrigPin,HIGH);
  delayMicroseconds(5);
  digitalWrite(leftTrigPin,LOW);
  duration1 = pulseIn(leftEchoPin,HIGH); // gets the time value for snesor 1

  // convert sensor 1 time to inches/cm
  //inches1 = microsecondsToInches(duration1);
  cm1 = microsecondsToCentimeters(duration1);

  // Right Sensor
  digitalWrite(rightTrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(rightTrigPin,HIGH);
  delayMicroseconds(5);
  digitalWrite(rightTrigPin,LOW);
  duration2 = pulseIn(rightEchoPin,HIGH); // gets the time value for sensor 2

  // convert sensor 2 time to inches/cm
  //inches2 = microsecondsToInches(duration2);
  cm2 = microsecondsToCentimeters(duration2);

  // Serial Monitor

 //Serial.print(inches1);
 // Serial.print(" in, ");
 Serial.print("Left: ");
 Serial.print(cm1);

 Serial.println();


  //Serial.print(inches2);
  //Serial.print("in2, ");
  Serial.print("Right: ");
  Serial.print(cm2);

  Serial.println();


  delay(100);

  // Drive Train
  // Motor Right: High means move forward
  // Motor Left: Low Means move forward

 /*if(cm1 > 1000 || cm2 > 1000) // if further than 160 inches, brake
  {
    brake();
    Serial.print("Brake(misread)");
    Serial.println();
    return;

  }
  */

  if (cm1<80 && cm2 < 80){
    brake();

    Serial.print("BreakClose");
    Serial.println();
  }

  else  if((cm1<175 && cm2 < 175) && (cm1 > 80 && cm2 > 80)) // if less than 68 inches away, go forward (slowly)
  {
    goForward();
  }

  else if((cm1 > 175 or cm <20) &&  cm2 < 175) // if something in front of the RIGHT sensor, turn RIGHT
  {
    goRight();
  }

  else if(cm1 < 175 && (cm2 > 175 or cm < 20)) // if something in front of the LEFT sensor, turn left
  {
    goLeft();
  }

  else
  {
    brake();
    Serial.print("BrakeFar");
    Serial.println();
  }
}



long microsecondsToInches(long microseconds)// convert ms t inches
{
  return microseconds/74/2 ;
}

long microsecondsToCentimeters(long microseconds)
{
  return microseconds/ 29/2;
}

void brake()
{
  digitalWrite(MotorDirectionLeft, LOW);
  digitalWrite(MotorDirectionRight, HIGH);
  analogWrite(MotorSpeedLeft, 0);
  analogWrite(MotorSpeedRight, 0);
  delay(500);
}

void goLeft()
{
  digitalWrite(MotorDirectionLeft, HIGH);
  digitalWrite(MotorDirectionRight, HIGH);

  //analogWrite(MotorSpeedLeft, 150);
  analogWrite(MotorSpeedLeft, 60);
  analogWrite(MotorSpeedRight, 150);  // set right motor on to turn left

  Serial.print("Go Left");
  Serial.println();
  delay(500);
}

void goRight()
{
  digitalWrite(MotorDirectionLeft, LOW);
  digitalWrite(MotorDirectionRight, LOW);

  analogWrite(MotorSpeedLeft, 150);
  analogWrite(MotorSpeedRight, 60);
  //analogWrite(MotorSpeedRight, 150);  // set left motor on to turn right

  Serial.print("Go Right");
  Serial.println();
  delay(500);
}

void goForward()
{
  digitalWrite(MotorDirectionLeft, LOW);
  digitalWrite(MotorDirectionRight, HIGH);
  analogWrite(MotorSpeedLeft, 60);
  analogWrite(MotorSpeedRight, 60);

  Serial.print("Go Forward");
  Serial.println();
  delay(500);
}