//the right back motor will be controlled by the motor A pins on the motor driver 1
//I'm tired of commenting I basically changed every pin in this section.
//This section was originally written for two motors by sparkfun and Jaeyoung made it work for four motors then I changed the pins. //john beck



//this section contains the motor driver facing away from the sensor.
const int PWMA_1 = 11;            //speed control pin on the motor driver for the right motor
const int AIN1_1 = 13;           //control pin 1 on the motor driver for the right motor //shift register pin
const int AIN2_1 = 12;            //control pin 2 on the motor driver for the right motor //shift register pin

//this section contains the motor driver facing away from the sensor.
//the left motor will be controlled by the motor B pins on the motor driver 1
const int PWMB_1 = 10;           //speed control pin on the motor driver for the left motor
const int BIN1_1 = 7;           //control pin 1 on the motor driver for the left motor //shift register pin
const int BIN2_1 = 8;           //control pin 2 on the motor driver for the left motor //shift register pin




const int trigPin = 6;           //connects to the trigger pin on the distance sensor
const int echoPin = 5;           //connects to the echo pin on the distance sensor

float sensor_distance = 0;               //stores the distance measured by the distance sensor

// Pin definitions:
// The 74HC595 uses a type of serial connection called SPI
// (Serial Peripheral Interface) that requires three pins:

int datapin = 8; //changed pin number //john beck
int clockpin = 13; //changed pin number //john beck
int latchpin = 12; //changed pin number //john beck

// We'll also declare a global variable for the data we're
// sending to the shift register:

byte data = 0;

const int driveTime = 20;      //this is the number of milliseconds that it takes the robot to drive 1 inch
                               //it is set so that if you tell the robot to drive forward 25 units, the robot drives about 25 inches

const int turnTime = 8;        //this is the number of milliseconds that it takes to turn the robot 1 degree
                               //it is set so that if you tell the robot to turn right 90 units, the robot turns about 90 degrees

                               //Note: these numbers will vary a little bit based on how you mount your motors, the friction of the
                               //surface that your driving on, and fluctuations in the power to the motors.
                               //You can change the driveTime and turnTime to make them more accurate

String botDirection;           //the direction that the robot will drive in (this change which direction the two motors spin in)
String distance_string;               //the distance to travel in each direction

float distance=600.0;

//These are the floats for calibrating each wheel speed. They are custom variables //John beck
float left_adjustment=1, right_adjustment=1;

//these encode forwards back left and right //john beck
char forwards=0, left=1, back=2, right=3;

//------------------FUNCTIONS-------------------------------



//These motor_specific functions were written for two motors by sparkfun. Jaeyoung rewrote it to work on four motors. I have tweaked them with constant speed adjustements for each wheel so that the thing goes
//straight. Jaeyoung originally did her own speed adjustments but since I had to re-glue the wheels it needed to eb calibrated again. I changed all of the digitalWrite instnaces to digitalWrite (John Beck).

/********************************************************************************/
void rightMotor(int motorSpeed)                       //function for driving the right motor
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(AIN1_1, HIGH);                         //set pin 1 to high
    digitalWrite(AIN2_1, LOW);                          //set pin 2 to low
  }
  else if (motorSpeed < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(AIN1_1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2_1, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(AIN1_1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2_1, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMA_1, abs(motorSpeed * right_adjustment));                 //now that the motor direction is set, drive it at the entered speed
}

/********************************************************************************/
void leftMotor(int motorSpeed)                        //function for driving the left motor
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(BIN1_1, HIGH);                         //set pin 1 to high
    digitalWrite(BIN2_1, LOW);                          //set pin 2 to low
  }
  else if (motorSpeed < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(BIN1_1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2_1, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(BIN1_1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2_1, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMB_1, abs(motorSpeed * left_adjustment));                 //now that the motor direction is set, drive it at the entered speed
}




//RETURNS THE DISTANCE MEASURED BY THE HC-SR04 DISTANCE SENSOR
float getDistance()
{
  float echoTime;                   //variable to store the time it takes for a ping to bounce off an object
  float calculatedDistance;         //variable to store the distance calculated from the echo time

  //send out an ultrasonic pulse that's 10ms long
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  echoTime = pulseIn(echoPin, HIGH);      //use the pulsein command to see how long it takes for the
                                          //pulse to bounce back to the sensor

  calculatedDistance = echoTime / 148.0;  //calculate the distance of the object that reflected the pulse (half the bounce time multiplied by the speed of sound)

  return calculatedDistance;              //send back the distance that was calculated
}

//function by john beck
void all_stop(){
  //Each of these four lines was written by sparkfun.
     rightMotor(0);
     leftMotor(0);
}

//This function is stolen from the loop of the sparkfun. The original loop only included two motors and jaeyoung added four. I have put it into a movement function
//I have also improved the design by reducing the number of lines of code necessary.
void move(char botDirection, int movement_distance){

  if(botDirection == forwards)                          //if the entered direction is forward
      {
        rightMotor(200);                                //drive the right wheel forward
        leftMotor(200);                                 //drive the left wheel forward

        delay(driveTime * movement_distance);            //drive the motors long enough travel the entered distance
        rightMotor(0);                                  //turn the right motor off
        leftMotor(0);                                   //turn the left motor off

      }
      else if(botDirection == back)                     //if the entered direction is backward
      {
        rightMotor(-200);                               //drive the right wheel forward
        leftMotor(-200);                                //drive the left wheel forward

        delay(driveTime * movement_distance);            //drive the motors long enough travel the entered distance
        rightMotor(0);                                  //turn the right motor off
        leftMotor(0);                                   //turn the left motor off

      }
      else if(botDirection == left)                      //if the entered direction is right
      {
        rightMotor(-200);                               //drive the right wheel forward
        leftMotor(255);                                 //drive the left wheel forward

        delay(turnTime * movement_distance);             //drive the motors long enough turn the entered distance
        rightMotor(0);                                  //turn the right motor off
        leftMotor(0);                                   //turn the left motor off

      }
      else if(botDirection == right)                    //if the entered direction is left
      {
        rightMotor(255);                                //drive the right wheel forward
        leftMotor(-200);                                //drive the left wheel forward

        delay(turnTime * movement_distance);             //drive the motors long enough turn the entered distance
        rightMotor(0);                                  //turn the right motor off
        leftMotor(0);                                   //turn the left motor off

      }


  /*
      //botDirection = Serial.readStringUntil(' ');       //read the characters in the command until you reach the first space
     // distance = Serial.readStringUntil(' ');           //read the characters in the command until you reach the second space

      //print the command that was just received in the serial monitor
      //Serial.println(movement_direction);
     // Serial.println(movement_distance);

      if(movement_direction==forwards)                          //if the entered direction is forward
      {
        rightMotor(200);                                //drive the right wheel forward
        leftMotor(200);                                 //drive the left wheel forward

        delay(driveTime * movement_distance);             //drive the motors long enough turn the entered distance

      }
      else if(movement_direction==back)                     //if the entered direction is backward
      {
        rightMotor(-200);                               //drive the right wheel forward
        leftMotor(-200);                                //drive the left wheel forward

        delay(driveTime * movement_distance);             //drive the motors long enough turn the entered distance
      }
      else if(movement_direction==right)                      //if the entered direction is right
      {
        rightMotor(-200);                               //drive the right wheel forward
        leftMotor(255);                                 //drive the left wheel forward

        delay(turnTime * movement_distance);             //drive the motors long enough turn the entered distance
      }
      else if(movement_direction==left)                    //if the entered direction is left
      {
        rightMotor(255);                                //drive the right wheel forward
        leftMotor(-200);                                //drive the left wheel forward

        delay(turnTime * movement_distance);             //drive the motors long enough turn the entered distance
      }

      all_stop();
      */

  }


/********************************************************************************/
void setup()
{

  //I bairly ain't write nothin in the dang ol setup its mostly not my code yo. Don't be hatin.

  //set the motor control pins as outputs
  pinMode(PWMA_1, OUTPUT);
  pinMode(AIN1_1, OUTPUT);
  pinMode(AIN2_1, OUTPUT);

  pinMode(PWMB_1, OUTPUT);
  pinMode(BIN1_1, OUTPUT);
  pinMode(BIN2_1, OUTPUT);


  //sensor setup
  pinMode(trigPin, OUTPUT);   //the trigger pin will output pulses of electricity
  pinMode(echoPin, INPUT);    //the echo pin will measure the duration of pulses coming back from the distance sensor

  // Set the three SPI pins to be outputs:
  //shift register setup
  pinMode(datapin, OUTPUT);
  pinMode(clockpin, OUTPUT);
  pinMode(latchpin, OUTPUT);

  Serial.begin(9600);           //begin serial communication with the computer

  //prompt the user to enter a command
  /*Serial.println("Enter a direction followed by a distance.");
  Serial.println("f = forward, b = backward, r = turn right, l = turn left");
  Serial.println("Example command: f 50");*/

  //for now the code is inside of setup because it occurs once. Make sure to erase this when moving to loop
  //move(forwards, 150);
  //move(forwards, 200);
          //rightMotor(200);                                //drive the right wheel forward
        //leftMotor(-200);                                //drive the left wheel forward
                  //leftMotor(-200);                                //drive the right wheel forward


  // move(right,145);


}


int tries=0;
/********************************************************************************/
void loop()
{

  float distance2=getDistance();
  float difference=distance-distance2;
  if(difference < 0){
    difference=difference * -1;
  }
  if(difference < 20){
    tries++;
  }
  if(tries==100){
    move(back,30);
    tries=0;
  }
  distance=distance2;
  unsigned long le_time=millis();
  while(distance > 7.0 && millis() < (le_time + 8000)){
      leftMotor(200);
  rightMotor(200);
       distance=getDistance();
      Serial.println(distance);
  }
  Serial.println("out");
  all_stop();
  delay(200);
  move(back,30);
  move(right,270);

}