/*

*/
// C++ code handwritten entirely by me at like 1 in the morning-- it took 300 lines to do something so simple smh
//Libraries

// Variables for USS
const int echo = 9; // for echo pin on Ultrasonic Distance Sensor
const int trig = 6; // for trig pin on Ultrasonic Distance Sensor
//Variables for the wheels
const int leftWheel = 10; // 10 is a placeholder value for now. Change when you get the needed information!
const int leftWheel2 = 11; // 11 is a placeholder value for now. Change when you get the needed information!
const int leftWheelEnable = 12;

const int rightWheel = 5; // 5 is a placeholder value for now. Change when you get the needed information!
const int rightWheel2 = 6; // 6 is a placeholder value for now. Change when you get the needed information!
const int rightWheelEnable = 7; // This pins decides if the right wheel motor controllers get power lol. Change the value when the team gives out the needed info
//A buzzer, I guess
const int buzzer = 13; //Since you can't actually expect to use the serial monitor while this program is running on actual hardware, the buzzer will take it's place. I would do an LCD, but there's no room for it on the breadboard

//Non Pin Variables
float duration; // records the pulse sent via the echo pin after activating trig for a milisecond (see getData)
float distance; // The printed value after duration's data gets processed and converted into a distance in centimeters (see distanceCalculation)
float seconds; // Used to specify the duration of certain tasks
float speed = 0; // Used to specify how fast you want the motors running. Leave it at 0 if you want to solely use digitalWrite commands
bool rightWheelTrig;
bool leftWheelTrig;
// Variables that controls the buzzer's Sweet spot
float minimum; // The floor of the Sweet spot (can be a decimal). It's a remnate of my old code but I've kept it around in case I need it again... and I probably won't lol
float maximum = 70; // The ceiling of the Sweet spot (can be a decimal)

void setup() {
  Serial.begin(9600);

  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);

  pinMode(rightWheel, OUTPUT);
  pinMode(rightWheel2, OUTPUT);

  pinMode(leftWheel, OUTPUT);
  pinMode(leftWheel2, OUTPUT);

  pinMode(rightWheelEnable, OUTPUT);
  pinMode(leftWheelEnable, OUTPUT);

  pinMode(buzzer, OUTPUT);
}




void reset(float seconds) { //resets everything back to its default state before re-running the program. DOES NOT CLEAR LCD SCREEN!!!!!
  digitalWrite(trig, LOW);
  stopAll();
  Serial.println(String("The program has finished running! In order to give you some time to clear the Serial Monitor (for readibility's sake), I'll restart the program in ") + secondsCheck(seconds) + String("."));
  Serial.println("\nI Hope everything worked :)");
  delay(seconds * 1000); // 1 milisecond delay to give the program adapt time to finish all of its tasks
}

void clockwiseLeft(float speed) {
  if (speed <= 0) {
    Serial.println("Moving Left Wheel Clockwise in Digial mode...");
    digitalWrite(leftWheel, HIGH);
    digitalWrite(leftWheel2, LOW);
  } else {
    Serial.println(String("Moving Left Wheel Clockwise in Analog mode @ ") + speed + String("/255 ..."));
    analogWrite(leftWheel, speed);
    analogWrite(leftWheel2, 0);
  }

}
void counterClockwiseLeft(float speed) {
  if (speed <= 0) {
    Serial.println("Moving Left Wheel Counter-Clockwise in Digial mode...");
    digitalWrite(leftWheel, LOW);
    digitalWrite(leftWheel2, HIGH);
  } else {
    Serial.println(String("Moving Left Wheel Counter-Clockwise in Analog mode @ ") + speed + String("/255 ..."));
    analogWrite(leftWheel, 0);
    analogWrite(leftWheel2, speed);
  }

}
void stopLeft() {
  Serial.println("Stopping Left Wheel...");
  digitalWrite(leftWheel, LOW);
  digitalWrite(leftWheel2, LOW);

}
void clockwiseRight(float speed) {
  if (speed <= 0) {
    Serial.println("Moving Right Wheel Clockwise in Digial mode...");
    digitalWrite(rightWheel, HIGH);
    digitalWrite(rightWheel2, LOW);
  } else {
    Serial.println(String("Moving Right Wheel Clockwise in Analog mode @ ") + speed + String("/255 ..."));
    analogWrite(rightWheel, speed);
    analogWrite(rightWheel2, 0);
  }

}
void counterClockwiseRight(float speed) {
  if (speed <= 0) {
    Serial.println("Moving Right Wheel Counter-Clockwise in Digial mode...");
    digitalWrite(rightWheel, LOW);
    digitalWrite(rightWheel2, HIGH);
  } else {
    Serial.println(String("Moving Right Wheel Counter-Clockwise in Analog mode @ ") + speed + String("/255 ..."));
    analogWrite(rightWheel, 0);
    analogWrite(rightWheel2, speed);
  }

}
void stopRight() {
  Serial.println("Stopping Right Wheel...");
  digitalWrite(rightWheel, LOW);
  digitalWrite(rightWheel2, LOW);
}

void stopAll() {
  rightWheelTrig = false;
  leftWheelTrig = false;
  Serial.println("Beginning to Stop Everything...");
  stopRight();
  stopLeft();
  Serial.println("Success!");
  buzzerBeep(false, 3, 0, 0);
}
void forward(float speed, float seconds) {
  Serial.println(String("Beginning move forward script @ ") + speed + String("/255 power for ") + secondsCheck(seconds) + String("..."));
  buzzerBeep(true, 1, 0, 0);
  rightWheelTrig = true;
  leftWheelTrig = true;

  clockwiseRight(speed);
  clockwiseLeft(speed);
  countdown(seconds);
}

void backward(float speed, float seconds) {
  Serial.println(String("Beginning move backward script @ ") + speed + String("/255 power for ") + secondsCheck(seconds) + String("..."));
  buzzerBeep(true, 2, 0, 0);
  rightWheelTrig = true;
  leftWheelTrig = true;

  counterClockwiseRight(speed);
  counterClockwiseLeft(speed);
  countdown(seconds);
}

void turnLeft(float speed, float seconds) {
  Serial.println(String("Beginning Turn Left script @ ") + speed + String("/255 power for ") + secondsCheck(seconds) + String("..."));
  buzzerBeep(true, 1, 2, 1);
  rightWheelTrig = true;
  leftWheelTrig = true;

  clockwiseLeft(speed);
  stopRight();
  countdown(seconds);
}
void turnRight(float speed, float seconds) {
  Serial.println(String("Beginning Turn Right script @ ") + speed + String("/255 power for ") + secondsCheck(seconds) + String("..."));
  buzzerBeep(false, 1, 1, 1);
  rightWheelTrig = true;
  leftWheelTrig = true;

  clockwiseLeft(speed);
  stopLeft();
  countdown(seconds);
}

void countdown(float seconds) {
  // if (rightWheelTrig == true) {
  //   digitalWrite(rightWheel, HIGH);
  // } else if (rightWheelTrig == false) {
  //   digitalWrite(rightWheel, LOW);
  // }

  // if (leftWheelTrig == true) {
  //   digitalWrite(leftWheel, HIGH);
  // } else if (leftWheelTrig == false) {
  // }
  Serial.println(String("Coundown for ") + secondsCheck(seconds) + String(" initiated!"));
  delay(seconds * 1000);
  Serial.print("Countdown Finished! ");
  stopAll();

}

void getData() {
  digitalWrite(trig, HIGH);  //activates trig pin
  delay(1); //delays for 1 milisecond to read data
  digitalWrite(trig, LOW);  //deactivates trig pin
}

void distanceCalculation() {
  getData(); //runs the function above this one
  duration = pulseIn(echo, HIGH);  //records pulse sent by echo (thanks to trig pin  during getData())
  distance = convert(duration); //Converts miliseconds to centimeters. Look at bottom function (only one without void as its starting one)
}

void wallCheck() { // checks if the car is a specific distance from a wall
  if (distance <= maximum) {
    Serial.println("AHHHHHH A WALL!!!"); // A bit of test code for now. If the distance the car ia away from a wall, it'll scream in the terninal!
  } else {
  }

}

float convert(float time) {
  // The calculation to convert the raw data into distance
  return  time / (29 * 2);
  // The formala is the time in miliseconds (time) / how long it takes sound to travle a centimeter (29 microseconds) * 2 (to account for the time it took for the data to come back to the mic)
}

void buzzerBeep(bool beepType, int times, int beepType2, int times2) {
  for (int i = 0; i > times; i++) {
    digitalWrite(buzzer, HIGH);
    if (beepType == true) {
      delay(250);
    } else {
      delay(3000);
    }
    digitalWrite(buzzer, LOW);
    delay(500);
  }

  if (beepType2 == 1 || beepType2 == 2) {
    for (int j = 0; j > times; j++) {
      digitalWrite(buzzer, HIGH);
      if (beepType2 == 2) {
        delay(3000);
      } else if (beepType2 == 1) {
        delay(250);
      }
      digitalWrite(buzzer, LOW);
      delay(500);
    }
  }
}
String secondsCheck(float seconds) {
  if (seconds == 1) {
    return String(" 1 second");
  } else {
    return String(" seconds");
  }
}

void start(float seconds) {
  Serial.println("Starting program... \nDO NOT DISCONNECT FROM BLUE CHORD UNTIL I SAY SO!!!!");
  digitalWrite(rightWheelEnable, HIGH);
  digitalWrite(leftWheelEnable, HIGH);
  Serial.println("Remember:");
  Serial.println("- 1 beep means it's going to go forward");
  Serial.println("- 3 beeps mean it's going to go backwards");
  Serial.println("- 1 long beep, then 1 short beep means it's turning left");
  Serial.println("- 1 short beep, then 1 long beep means it's turning right");
  Serial.println("- 2 long beeps mean that all componets have been stopped");
  Serial.println("- If testOfComponets() has been activated in the code, you'll hear 10 short beeps, then 3 long beeps before the robot does anything");
  Serial.println("Note that all beeps are spaced out by 1/2 a second, so listen until the end!");
  delay(1000);
  Serial.println(String("If you want to, you're clear to unplug the chord now. You got ") + secondsCheck(seconds) + String("!"));
  Serial.println("Please note that by keeping the chord plugged in, you'll start to get stuff written into the Serial monitor instead.\n\nGood Luck!");
}

//MAKE A NEW FILE STARTING HERE, PLEASE! This file is only for the functions. Thanks :)
void debugStuff(int fSeconds, int bSeconds, int lSeconds, int rSeconds) {
  Serial.println("Debug mode initiated!");
  buzzerBeep(true, 10, 2, 3);
  if (fSeconds == 0) {
    forward(0, 5);
    delay(1000);
    backward(0, 5);
    delay(1000);
    turnLeft(0, 3);
    delay(1000);
    turnRight(0, 3);
    delay(1000);
  } else {
    forward(0, fSeconds);
    delay(1000);
    backward(0, bSeconds);
    delay(1000);
    turnLeft(0, lSeconds);
    delay(1000);
    turnRight(0, rSeconds);
    delay(1000);
  }
  Serial.println("Finished Testing! Your clear to make any corrections in the code if needed.");
}
void loop() {
  start(5);
  digitalWrite(rightWheelEnable, HIGH);
  digitalWrite(leftWheelEnable, HIGH);
  debugStuff(0, 0, 0, 0);
  reset(5);
}