/* 
Noah Wessel
Jackson Bennett
6/7/2024
ENGR 103 FINAL
1:53 PM
*/

#include <Wire.h>
#include <VL53L0X.h>
VL53L0X sensor;

#include <FastLED.h>
#define NUM_LEDS 2
CRGB leds[NUM_LEDS];

/* This section of code has been implemented from the bumpbot code.
It assings pin numbers to names for easier use later
M1 and M2 correspsond to motor 1 and motor 2
A and B correspond to the forwards and backwards function of each motor.
WSLED is the pin that the errorLED function uses.*/
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18
#define WSLED 4

/*These next sections of code are for assigning the LED pins
There are two leds and 3 color so there is a total of 6 pins needed to be assigned. */
#define LEDRED 26
#define LEDGREEN 27
#define LEDBLUE 14

#define LEDRED2 33
#define LEDGREEN2 32
#define LEDBLUE2 25

/*This section of code is assigning pin # to names for easier use later.
The four pins that are being assigned are for the input buttons that toggle the emotions*/
#define INPUT1 0
#define INPUT2 2
#define INPUT3 15
#define INPUT4 13

uint32_t lastTime = 0; //from my understanding this is a relic from the bumpbot code that is now unused. If testing proves i dont need it i will delete it.


/* reverse() is a function that has been taken from the bumpbot code.
it sets the motors to the inputed speed in reverse */
void reverse(uint8_t mySpeed) {
  ledcWrite(3, mySpeed);  //Set M2B to mySpeed
  ledcWrite(2, 0);        //Set M2A to LOW
  ledcWrite(1, mySpeed);  //Set M1B to mySpeed
  ledcWrite(0, 0);        //Set M1A to LOW
}
/*forward has also been implemented from the bumpbot code
It sets the A (forward) motors to the inputed speed */
void forward(uint8_t mySpeed) {
  ledcWrite(2, mySpeed);  //Set M2A to mySpeed
  ledcWrite(3, 0);        //Set M2B to LOW
  ledcWrite(0, mySpeed);  //Set M1A to mySpeed
  ledcWrite(1, 0);        //Set M1B to LOW
}
//turns the robot clockwise... Implemented from the bumpbot code.
void cw(uint8_t mySpeed) {
  ledcWrite(3, mySpeed);  //Set M2B to mySpeed
  ledcWrite(2, 0);        //Set M2A to LOW
  ledcWrite(1, 0);        //Set M1B to LOW
  ledcWrite(0, mySpeed);  //Set M1A to mySpeed
}
//turns the robot counter clockwise... Implemented from the bumpbot code.
void ccw(uint8_t mySpeed) {
  ledcWrite(2, mySpeed);  //Set M2A to mySpeed
  ledcWrite(3, 0);        //Set M2B to LOW
  ledcWrite(0, 0);        //Set M1A to LOW
  ledcWrite(1, mySpeed);  //Set M1BA to mySpeed
}
//Sets the robots motors to zero speed.
void stop() {
  ledcWrite(2, 0); //Set M2A to LOW
  ledcWrite(3, 0); //Set M2B to LOW
  ledcWrite(0, 0); //Set M1A to LOW
  ledcWrite(1, 0); //Set M1B to LOW
}

void figureeight() { //The name is misleading. figureeight() does NOT actually complete figure eights. The robot instead does a jolly stroll.
  ledcWrite(2, 250); //notice how this motor is faster than the other, this is so it does a partial turn while still maintaining forward momentum.
  ledcWrite(3, 0);
  ledcWrite(0, 200); //slower
  ledcWrite(1, 0);
  delay(750); //delay for ~750 ms, so it can gradually turn.
  ledcWrite(2, 200); //same thing but now gradully turning the other direction.
  ledcWrite(3, 0);
  ledcWrite(0, 250); //faster
  ledcWrite(1, 0);
}

//error led has been implemented from the bumpbot code.
void errorLed(int count = 1) {
  for (int i = 0; i < count; i++) {
    leds[0] = CRGB::Red;
    FastLED.show();
    delay(100);
    leds[0] = CRGB::Black;
    FastLED.show();
    delay(100);
  }
  delay(1000);
}

void setup() {

  Serial.begin(115200); //Starts UART communication at 115200 baud.
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);  // GRB ordering is assumed

  // This section creates the I2C (Wire) connection and quieries for a VL53LX
  Wire.begin();
  sensor.setTimeout(500);
  if (!sensor.init()) {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) { errorLed(); }
  }

  // Set the 4 motor controll lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  //Sets the LED pins as outputs
  pinMode(LEDRED2, OUTPUT);
  pinMode(LEDGREEN2, OUTPUT);
  pinMode(LEDBLUE2, OUTPUT);
  pinMode(LEDRED, OUTPUT);
  pinMode(LEDGREEN, OUTPUT);
  pinMode(LEDBLUE, OUTPUT);

  //Sets the the buttons as inputs with resistors.
  pinMode(INPUT1, INPUT_PULLUP);
  pinMode(INPUT2, INPUT_PULLUP);
  pinMode(INPUT3, INPUT_PULLUP);
  pinMode(INPUT4, INPUT_PULLUP);

  //These lines connect 4 PWM channels to the 4 motor control Pins
  ledcSetup(0, 30000, 8);
  ledcAttachPin(M1A, 0);
  ledcSetup(1, 30000, 8);
  ledcAttachPin(M1B, 1);
  ledcSetup(2, 30000, 8);
  ledcAttachPin(M2A, 2);
  ledcSetup(3, 30000, 8);
  ledcAttachPin(M2B, 3);

  //This section has only been implemented to provide verification of the setup() function working.
  forward(230); //tells the bot to move forward at speed = 230
  delay(500); //for 500 ms
  stop(); //tells the bot to stop.
  errorLed(1); //led flashes once.
}
void loop() {

  int distance; //distance is used later and is used to store the distance sensor data.
  int inputval1 = digitalRead(INPUT1); //stores input from button 1.
  int inputval2 = digitalRead(INPUT2); //stores input from button 2.
  int inputval3 = digitalRead(INPUT3); //stores input from button 3.
  int inputval4 = digitalRead(INPUT4); //stores input from button 4.


  if (inputval1 != 1) { //Checks if button 1 has been pressed. It uses != because an INPUT_PULLUP returns a 1 whenever it is not being pressed. So when the button is being pressed it returns NOT 1.
    Serial.println("ANGRY"); //sends serial monitor message that the ANGRY loop is about to begin.
    digitalWrite(LEDRED2, HIGH); //Sets Red LED 2 to HIGH
    digitalWrite(LEDRED, HIGH); //Sets Red LED 1 to High
    delay(500); 

    inputval1 = 1; //Set input back to one so the loop can continue.
    while (inputval1 == 1) {  // ANGRY loop. This loop continues until the button is pressed again.

      inputval1 = digitalRead(INPUT1); //read button 1. If it is being pressed again the loop ends; leaving the emotion.
      distance = sensor.readRangeSingleMillimeters(); //sets distance = the distance sensor reading.

      if (distance < 50) { //Right next to wall.
        stop(); //stop motors
        delay(500); //wait half a second.
        reverse(230); //reverse at speed = 230
        delay(500); //wait half a second.
      }
      forward(230); //forward at speed = 230
      delay(100); // wait 100 ms.
    }
    digitalWrite(LEDRED2, LOW); //turn of red led 2.
    digitalWrite(LEDRED, LOW); //turn of red led 1.
    stop(); //stop the robot.
  } else if (inputval2 != 1) { //checks if button 2 has been pressed.

    Serial.println("SAD"); //alerts serial monitor that robot has entered sad mode.
    digitalWrite(LEDBLUE2, HIGH); //sets blue led to high.
    digitalWrite(LEDBLUE, HIGH); //sets other blue led to high.
    delay(500); //wait 500 ms
    inputval2 = 1; //stores input from button 2
    while (inputval2 == 1) { //breaks if button 2 is pressed again.

      inputval2 = digitalRead(INPUT2); //checks button 2.
      distance = sensor.readRangeSingleMillimeters(); //stoers distance
      if (distance < 50) { //If the robot is facing a wall, it will stand still for 5 seconds and then return to default mode.
        stop(); 
        delay(5000);
        break;
      } else { //if a wall isnt detected, continue forward.
        forward(230);
        delay(100);
      }
    }
    digitalWrite(LEDBLUE2, LOW); //turn off blue leds.
    digitalWrite(LEDBLUE, LOW);
    stop();
  } else if (inputval3 != 1) { //if button 3 is pressed.

    Serial.println("SCARED");
    digitalWrite(LEDRED2, HIGH); //set leds to purple. blue + red = purple.
    digitalWrite(LEDBLUE2, HIGH);
    digitalWrite(LEDRED, HIGH);
    digitalWrite(LEDBLUE, HIGH);
    delay(500);

    inputval3 = 1; //reset back to 1 for later inputs.
    while (inputval3 == 1) { // continue until button is pressed again. 

      inputval3 = digitalRead(INPUT3); //store input
      distance = sensor.readRangeSingleMillimeters(); //read distance
      if (distance < 50) {  //If wall is detected, reverse for 2.5 seconds.
        reverse(255);
        delay(2500);
      } else if (distance < 200) {  // Getting close, slow down
        forward(230 - (30 - ((distance / 200.0) * 30)));
      } else {
        forward(230); //keep going until wall is found.
      }
    }
    digitalWrite(LEDRED2, LOW); //turn off purple leds.
    digitalWrite(LEDBLUE2, LOW);
    digitalWrite(LEDRED, LOW);
    digitalWrite(LEDBLUE, LOW);
    stop();
    delay(500);
  } else if (inputval4 != 1) { //if button 4 is pressed

    Serial.println("HAPPY");
    digitalWrite(LEDGREEN2, HIGH); //set leds to green
    digitalWrite(LEDGREEN, HIGH);
    delay(500);
    inputval4 = 1;
    while (inputval4 == 1) { //continue until button 4 is pressed again.

      inputval4 = digitalRead(INPUT4);
      figureeight();
      delay(750); //delay so other direction can be used.
    }
    digitalWrite(LEDGREEN2, LOW); // turn off green led.
    digitalWrite(LEDGREEN, LOW);
    stop();
  }
  delay(100); //used for timings so button presses have a little window for error.
}