#include <NewPing.h>
#include <Servo.h>
#include <I2Cdev.h>
#include <MPU6050.h>

int right1 = 4; // in1 of right tire
int rightEN = 3; // enA of right tire
int right2 = 2; // in2 of right tire
int left1 = 5; // in1 of left tire
int leftEN = 6; // enA of left tire
int left2 = 7; // in2 of right tire

int rightLED = 8;
int leftLED = 9;

int trig = 12;  // trigger PIN
int echo = 11;  // echo PIN
int max_dist = 300; // max distance of ultrasonic
int usReading; // reading of ultrasonic
int usReadingThreshold = 25; // max distance between car and walls
NewPing US (trig,echo,max_dist); // NewPing initialization
Servo servoMotor; // Servo initialization

int MAXSPDR = 125; // maximum speed of right tire
int MAXSPDL = 140; // maximum speed of left tire
int leftDistance;
int rightDistance;

int16_t gz;
MPU6050 accelgyro; // SCL -> A5 ------ SDA -> A4
float theta;
float oldTheta;
unsigned int oldtime = 0;
unsigned int now;

void setup() {

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    Wire.begin();
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    Fastwire::setup(400, true);
  #endif

  Serial.println("Initializing I2C devices...");
  accelgyro.initialize();

  int offset = 0;
  bool calibrated = false;

  while (calibrated == false) {
    accelgyro.setZGyroOffset(offset);
    for (int i = 0; i < 100; i++) {
      gz = accelgyro.getRotationZ() + gz;
    }
    gz = gz / 100;

    if ((gz > -1) && (gz < 1)) {
      calibrated = true;
      Serial.print("\nGyro offset: ");
      Serial.println(offset);
      delay(2000);
      gz = 0;
    }
    if (gz <= -2) {
      offset++;
    }
    if (gz >= 2) {
      offset--;
    }
    gz = 0;

  }

  pinMode(rightLED,OUTPUT);
  pinMode(leftLED,OUTPUT);
  pinMode(right1,OUTPUT);
  pinMode(right2,OUTPUT);
  pinMode(rightEN,OUTPUT);
  pinMode(left1,OUTPUT);
  pinMode(left2,OUTPUT);
  pinMode(leftEN,OUTPUT);
  Serial.begin(115200);
  servoMotor.attach(10); // Servo PIN
  servoMotor.write(86); // sets initial servo angle
  delay(200);

  moveForward(); // moves the car forward
}

void loop() {
  delay(10);
  usReading = US.ping_cm();
  printReading();
  if(usReading < usReadingThreshold && usReading !=0){ // checks if the reading is below the threshold
    moveStop();
    char vacantDirection = checkDirections(); // checks vacant directions left & right
    carTurn(vacantDirection); // turns the car in the vacant direction
  }
}

void printReading(){
  Serial.print("Distance is currently ");
  Serial.println(usReading);
}

void moveRightTire(int pwm){
  digitalWrite(right1, HIGH);
  digitalWrite(right2, LOW);
  analogWrite(rightEN, pwm); // moves the right tire forward at pwm speed
}

void reverseRightTire(int pwm){
  digitalWrite(right1, LOW);
  digitalWrite(right2, HIGH);
  analogWrite(rightEN, pwm); // moves the right tire forward at pwm speed in opposite direction
}

void moveLeftTire(int pwm){
  digitalWrite(left1, HIGH);
  digitalWrite(left2, LOW);
  analogWrite(leftEN, pwm); // moves the left tire forward at pwm speed
}

void reverseLeftTire(int pwm){
  digitalWrite(left1, LOW);
  digitalWrite(left2, HIGH);
  analogWrite(leftEN, pwm); // moves the left tire forward at pwm speed in opposite direction
}

void moveForward(){
  moveRightTire(MAXSPDR); // moves right tire at max speed
  moveLeftTire(MAXSPDL); // moves left tire at max speed
}

void moveStop(){
  moveRightTire(0); // stops right tire
  moveLeftTire(0); // stops left tire
}

char checkDirections(){
  servoMotor.write(180); //looks left
  delay(200);
  leftDistance = US.ping_cm(); //sets the left distance to the current reading
  servoMotor.write(0); //looks right
  delay(350);
  rightDistance = US.ping_cm(); //sets the right distance to the current reading
  servoMotor.write(86);
  delay(200);
  if(rightDistance>leftDistance){
    return 'R';
  }else{
    return 'L';
  }
}

void carTurn(char directn){
  theta = 0;
  if(directn == 'R'){
    digitalWrite(rightLED,HIGH);
    reverseRightTire(MAXSPDR);
    moveLeftTire(MAXSPDL);

    while (theta > -87) {
      getTheta();
    }

    moveForward();

    digitalWrite(rightLED,LOW);

  }else if(directn == 'L'){
    digitalWrite(leftLED,HIGH);
    moveRightTire(MAXSPDR);
    reverseLeftTire(MAXSPDL);

    while (theta < 87) {
      getTheta();
    }
    moveForward();

    digitalWrite(leftLED,LOW);
  }
}

void getTheta() {
  gz = accelgyro.getRotationZ();

  // display tab-separated accel/gyro x/y/z values
  now = millis();

  if (oldtime > now) {
    oldtime = now;
    Serial.print("Angle: \t");
    Serial.print(theta);
    Serial.print("\t");
  }
  else {
    theta = ((((float)gz / 131)) * (((float)now - oldtime) / 1000)) + theta;
    oldtime = now;
    if (abs(theta - oldTheta) <= 0.01) {
      theta = oldTheta;
    }
    oldTheta = theta;
    Serial.print("Angle: \t");
    Serial.print(theta);
    Serial.print("\t");
  }
}