/*
Jack Davison and Myles Golston
ENGR 103
6/5/2024
ROBOT EMOTIONS FINAL CODE

Our Robot displays 4 different emotions - fear, happiness, anger, and sadness.
There is a function for each of these emotions, and they are simply looped through in the loop() section of the code to display each one
The robot is good at avoiding walls, and manages to keep itself safe 
To write the code, we used some of the example code provided in studio/announcements and then used our own ideas to make each function and emotion
*/

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

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

#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18
#define WSLED 4

uint32_t lastTime = 0;

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 off(){ // Turns everything off
  ledcWrite(3, 0);
  ledcWrite(2, 0);
  ledcWrite(1, 0);
  ledcWrite(0, 0);
}

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
}

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
}

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
}

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
}

void anger() {
    leds[0] = CRGB::Red;
    leds[1] = CRGB::Red; // Turn eyes Red
    FastLED.show();
    
    int randomSpeed = random(180, 255);  // Random speed
    int randomDirection = random(4);  // Random direction

    switch (randomDirection) { // Randomly does random action in random speed (where it still moves)
      case 0:
        forward(randomSpeed);
        break;
      case 1:
        reverse(randomSpeed);
        break;
      case 2:
        cw(randomSpeed);
        break;
      case 3:
        ccw(randomSpeed);
        break;
    }

      delay(random(100, 500));  // Random delay to make movement frantic
}

void scared(uint8_t mySpeedSlow, uint8_t mySpeedFast) { // Take a slower and faster speed
  leds[0] = CRGB::Purple;
  leds[1] = CRGB:: Purple; // Turn eyes purple for fear
  FastLED.show();

  reverse(mySpeedSlow); // Back up in fear
  delay(1000);

  ccw(mySpeedSlow); // Turn around and then for loop to RUN AWAY
  delay(650);

  for(int i = 0; i < 6; i++){
    int distance = sensor.readRangeSingleMillimeters(); // Reread distance every loop

    if(distance > 100){ 
      forward(mySpeedFast);
      delay(500);
    }
    else if(distance < 100){ 
      off();
      delay(3000);
      i = 6;
    }
  }
}

void sad(uint8_t mySpeed) {
  leds[0] = CRGB::Blue;
  leds[1] = CRGB::Blue; // Turn eyes blue
  FastLED.show();

  ccw(mySpeed); // Turn around
  delay(650);

  off(); // Stop for a moment
  delay(500);

  for(int i = 0; i < 10; i++){ // Slowly sulk away... (Checks every half second if against wall)
    int distance = sensor.readRangeSingleMillimeters(); // Reread distance every loop

    if(distance > 100){ 
      forward(mySpeed);
      delay(500);
    }
    else if(distance < 100){ // Sulk against wall if close
      off();
      delay(3000);
      i = 10;
    }
  }
}


void happiness(uint8_t mySpeed) {
    leds[0] = CRGB::Yellow;
    leds[1] = CRGB::Yellow; // Turn eyes Yellow
    FastLED.show();

    ccw(mySpeed); // SPIN AROUND WITH GLEE
    delay(5000); // YIPPEEEEEE

    off(); // Stop just for a second to catch breath
    delay(1000);
}
void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);  // GRB ordering is assumed
  
  // This section creates the I2C (Wire) connection and queries 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 control lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  //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);
}

void loop() {
  int distance = sensor.readRangeSingleMillimeters();
  Serial.println(distance);
  delay(500);
  if (distance < 50) { // Real close! Avoid
    reverse(255);
    delay(200);
    cw(255);
    delay(150); 
  } 
  else if (distance < 200) { // Getting close, slow down
    forward(230 - (30 - ((distance / 200.0) * 30)));
    
  } else {
     scared(200, 255); // Loop through emotions
     sad(200);
     happiness(255);
  }

    for(int i = 0; i < 25; i++){ // Anger needs this loop since it only does one action at a time, so loop it 25 times
       anger(); 
  }
  Serial.println(distance);
}