/*
Mohamed Mohamed and Fadel Barhum
ENGR 103
Final Assignment
Donald Heer
6/7/24
*/

#include <Wire.h>
#include <VL53L0X.h>
#include <FastLED.h>

#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18
#define LED_PIN 4
#define NUM_LEDS 2
// Above are the definitions for the motors
VL53L0X sensor;

CRGB leds[NUM_LEDS];

// Initialize the motor control pins as outputs, the distance sensor, and the LEDs
void setup() {
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  FastLED.addLeds<WS2812, LED_PIN, GRB>(leds, NUM_LEDS);

  Wire.begin();
  sensor.init();
  sensor.setTimeout(500);
  sensor.startContinuous();
//Above is the sensor for the bot which will be known
//as wall-e
  Serial.begin(115200);
  Serial.println("Setup complete");
}

// Turn off both motors
void off() {
  digitalWrite(M2B, LOW);
  digitalWrite(M2A, LOW);
  digitalWrite(M1B, LOW);
  digitalWrite(M1A, LOW);
}

// Move both motors forward
void forward(int speed) {
  digitalWrite(M2B, LOW);
  digitalWrite(M2A, speed);
  digitalWrite(M1B, LOW);
  digitalWrite(M1A, speed);
}

// Move both motors in reverse
void reverse(int speed) {
  digitalWrite(M2B, speed);
  digitalWrite(M2A, LOW);
  digitalWrite(M1B, speed);
  digitalWrite(M1A, LOW);
}

// Turn the robot clockwise
void cw() {
  digitalWrite(M2B, HIGH);
  digitalWrite(M2A, LOW);
  digitalWrite(M1B, LOW);
  digitalWrite(M1A, HIGH);
}

// Turn the robot counterclockwise
void ccw() {
  digitalWrite(M2B, LOW);
  digitalWrite(M2A, HIGH);
  digitalWrite(M1B, HIGH);
  digitalWrite(M1A, LOW);
}

// Set LED color
void setColor(CRGB color) {
  for (int i = 0; i < NUM_LEDS; i++) {
    leds[i] = color;
  }
  FastLED.show();
}

/* The angry emotion's movements are defined below by
for loops
While in its angry state it charges at you, then it
warns you before it starts charging*/
void angry() {
  setColor(CRGB::Red); // Red
  Serial.println("Angry");
  for (int i = 0; i < 5; i++) {
    forward(255);
    delay(200);
  }
  for (int i = 0; i < 5; i++) {
    forward(255);
    delay(200);
    reverse(255);
    delay(200);
  }
  for (int i = 0; i < 5; i++) {
    forward(255);
    delay(200);
  }
  off();
  delay(2000);
}
/* For the sad emotion, it turns clockwise 
then it keeps moving forward for around a second as to
like sulking away 
 */
void sad() {
  setColor(CRGB::Blue); // Blue
  Serial.println("Sad");
  for (int i = 0; i < 2; i++) {
    cw();
    delay(300);
  }
  for (int i = 0; i < 10; i++) {
    forward(15);
    delay(200);  
  }
  off();
  delay(2000);
}
/* When the scared emotion is turned on, the bot keeps
shaking clockwise and counterclockwise to display and 
show a state of fear and trembling
 */
void scared() {
  setColor(CRGB::Yellow); // Yellow
  Serial.println("Scared");
  for (int i = 0; i < 5; i++) {
    reverse(255);
    delay(100);
  }
  for (int i = 0; i < 40; i++) {
    cw();
    delay(50);
    ccw();
    delay(50);
}
  off();
  delay(2000);
}
/* The funtion below describes the happy emotion
which basically makes the bot go side to side which
displays a sign of happiness of content. This is 
described by the counterclockwise and clockwise 
movements of the bot and how they keep repeating 
 */
void happy() {
  setColor(CRGB::Green); // Green
  Serial.println("Happy");
  for (int i = 0; i < 3; i++) {
    forward(255);
    delay(500);
  }
  for (int i = 0; i < 5; i++){
    cw();
    delay(500);
  }
  for (int i = 0; i < 5; i++){
    ccw();
    delay(500);
  }
  for (int i = 0; i < 5; i++){
    cw();
    delay(600);
    ccw();
    delay(600);
  }
  off();
  delay(2000);
}
/*Main loop to run the emotions that were decribed and
defined above
 */
void loop() {
  angry();
  sad();
  scared();
  happy();
}