#include <SoftwareSerial.h>
#include <Servo.h>

SoftwareSerial SerialBT(10, 9); // Make sure you connect the bluetooth to these pins
Servo arm;
Servo ultra;
char d;
int armPos = 180;
unsigned long period;

const int in1Pin = 4; // Left motor Direction 1
const int in2Pin = 3; // Left motor Direction 2
const int in3Pin = 2; // Right motor Direction 1
const int in4Pin = 5; // Right motor Direction 2
const int armPin = 11; //signal pin for ultrasonic servo
const int triggerPin = 13;
const int echoPin = 12;
const int ultraServopin = 6;
void setup() {
  SerialBT.begin(9600); //Bluetooth device name
  Serial.begin(115200);
  Serial.println("ready");

  pinMode(in1Pin, OUTPUT); // what are the four motor pins (hint: look at lines 11-14)
  pinMode(in2Pin , OUTPUT);
  pinMode(in3Pin, OUTPUT);
  pinMode(in4Pin, OUTPUT);
  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);

  digitalWrite(triggerPin, LOW);
  arm.attach(armPin); // what is the pin for the arm
  arm.write(180);  // what angle do you want the arm to start off at?
  ultra.attach(ultraServopin);
  ultra.write(30);

}

void loop() {
  if (SerialBT.available() > 0) {
    d = SerialBT.read();

    // this part handles the controls
    if (d == 'F') {
      fwd();
    }
    else if (d == 'B') {
      bwd();
    }
    else if (d == 'L') {
      left();
    }
    else if (d == 'R') {
      right();
    }
    else if (d == 'Q') {
      // remember, the arm is a servo
      armPos += 5; // how many degrees do you want the arm to move right by
    }
    else if (d == 'A') {
      armPos -= 5; // how many degrees do you want the arm to move left by
    }
    else if (d == 'Z') {
      autonomous();
      armPos=180;
    }
  }
  else stop();
  constrain(armPos, 0, 180);
  arm.write(armPos);
  //SerialBT.("e"); // E
}

void fwd() {
  // fill in the blanks with HIGH or LOW so that the motors turn forward
  digitalWrite(in1Pin, HIGH); // do we want to turn on pin one? type in HIGH or LOW
  digitalWrite(in2Pin, LOW); // do we want to turn on pin two? type in HIGH or LOW
  digitalWrite(in3Pin, LOW); // do we want to turn on pin three? type in HIGH or LOW
  digitalWrite(in4Pin, HIGH); // do we want to turn on pin four? type in HIGH or LOW
  delay(100);
}

void bwd() {
  // here, make the car turn backwards
  // go back to fwd() if you need help
  digitalWrite(in1Pin, LOW); // fill in the two blanks. the first blank should be a pin variable. the second should be HIGH or LOW
  digitalWrite(in2Pin, HIGH); // fill in the two blanks. the first blank should be a pin variable. the second should be HIGH or LOW
  digitalWrite(in3Pin, HIGH); // fill in the two blanks. the first blank should be a pin variable. the second should be HIGH or LOW
  digitalWrite(in4Pin, LOW); // fill in the two blanks. the first blank should be a pin variable. the second should be HIGH or LOW
  delay(100);
}

void left() {
  // here, make the car turn left
  // go back to fwd() if you need help
  digitalWrite(in1Pin, LOW); // fill in the two blanks.
  digitalWrite(in2Pin, HIGH); // fill in the two blanks.
  digitalWrite(in3Pin, LOW); // fill in the two blanks.
  digitalWrite(in4Pin, HIGH); // fill in the two blanks.
  delay(75);
}

void right() {
  // here, make the car turn right
  // go back to fwd() if you need help
  digitalWrite(in1Pin, HIGH); // fill in the two blanks.
  digitalWrite(in2Pin, LOW); // fill in the two blanks.
  digitalWrite(in3Pin, HIGH); // fill in the two blanks.
  digitalWrite(in4Pin, LOW); // fill in the two blanks.
  delay(75);
}

void stop() {
  // do the same as fwd() or bwd()
  // hint, all pins should be turned off
  digitalWrite(in1Pin, LOW); // fill in the two blanks.
  digitalWrite(in2Pin, LOW); // fill in the two blanks.
  digitalWrite(in3Pin, LOW); // fill in the two blanks.
  digitalWrite(in4Pin, LOW); // fill in the two blanks.
}


//===========================================================
// Replace the autonomous code in the robot-car-basic with this code to get started
int rightDistance = 0;
int leftDistance = 0;
int distance = 0;
bool mazeComplete = false;
void autonomous() {
  Serial.println("Auto Engaged");
  while (!mazeComplete) {
    if (SerialBT.available()) {
      d = SerialBT.read();
    }
    if (d == 'X') {
      // we will stop the autonomous if we send the letter 'X'
      break;
    }
    // Write your autonomous code under this line
    // use the functions:
    // fwd(), bwd(), left(), right(), and stop()
    // you can also use readDistance() to get the distance from the ultrasonic sensor

    // Here is an example auto. It is bad, but you will make it better


    distance = readDistance();

    if (distance > 100) {
      fwd();
      delay(200);
      stop();
    } else {
      bwd();
      delay(50);
      stop();

      ultra.write(120);
      rightDistance = readDistance();
      delay(200);

      ultra.write(30);
      leftDistance = readDistance();
      delay(200);


      if (rightDistance > leftDistance) {
        right();
        delay(100);
        stop();
      } else {
        left();
        delay(100);
        stop();
      }

      ultra.write(30);
      delay(200);
    }
    Serial.println("Auto Finished");
  }
}
//===========================================================

// MAGIC CODE - DO NOT TOUCH

unsigned int readDistance()
{
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  period = pulseIn(echoPin, HIGH);
  return period * 343 / 2000;
}