/*
4/27/2024
Vani Jajam, Courtney Kuning
Cyrus the robot can perform 4 emotions - happy(green - initial, sad, angry, and scared
Cyrus is pretty confused, goes through emotions sensor based, pretty fast

GPIO22 SCL
GPIO21 SDA
GPIO19 M1A
GPIO18 M1B
GPIO17 M2A
GPIO16 M2B
GPIO4 WS2812
*/

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


#include <FastLED.h>
// To use FastLED library, we must define the number of LEDs 
// and a special array to hold the colors
#define NUM_LEDS 2
CRGB leds[NUM_LEDS];

#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18
// This is the pin on cyrus that is connected to the LEDs
#define WSLED 4


uint32_t lastTime = 0;
String currentEmotion = "";

// function to indicate an error using LEDs
void errorLed(int count = 1){
  for (int i = 0; i< count; i++){
    leds[0] = CRGB::Red; // turn LED 0 red
    leds[1] = CRGB::Red; // turn LED 1 red
    FastLED.show();      // show the LEDs
    delay(100);          // wait for 100 ms
    leds[0] = CRGB::Black; // turn LED 0 off
    leds[1] = CRGB::Black; // turn LED 1 off
    FastLED.show();        // show the LEDs
    delay(100);            // wait for 100 ms
  }
  delay(1000);  // wait for 1 second after blinking
}

// function to move cryus 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
}

// function to move cyrus forward
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
}

// function to move cyrus clockwise
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
}

// function to move cyrus counter clockwise
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
}

// function for cyrus' emotions
void setEmotion(String emotion){
  if (emotion != currentEmotion){
    currentEmotion = emotion; // update current emotion if different
  }
  
  if (emotion == "happy"){
    leds[0] = CRGB::Yellow; // set LEDs to yellow
    leds[1] = CRGB::Yellow; // set LEDs to yellow 
    FastLED.show();         // show the LEDs
    forward(220);           // move forward at speed 220
    delay(5000);            // wait for 5 seconds
  
    if (millis() - lastTime >= 10000){
      cw(255);              // turn clockwise at full speed
      delay(500);           // wait for 0.5 seconds
      lastTime = millis();  // update last time
    }
  } else if (emotion == "angry"){
    leds[0] = CRGB::Red;    // set LEDs to red
    leds[1] = CRGB::Red;    // set LEDs to red
    FastLED.show();         // show the LEDs
    forward(255);           // charges at people full speed
    delay(1000);            // wait for 1 second
    reverse(255);           // move backwards at full speed
    delay(500);             // wait for 0.5 seconds
  } else if (emotion == "sad"){
    leds[0] = CRGB::Blue;  // set LEDs to blue
    leds[1] = CRGB::Blue;  // set LEDs to blue
    FastLED.show();        // show the LEDs
    forward(200);          // move forward at speed 200

    if (millis() - lastTime >= 10000){
      cw(200);             // turn clockwise at speed 200
      delay(1000);         // wait for 1 second
      lastTime = millis(); // update last time
    }
  } else if (emotion == "afraid"){
    leds[0] = CRGB::Green; // set LEDs to green
    leds[1] = CRGB::Green; // set LEDS to green
    FastLED.show();        // show the LEDs
    reverse(255);          // move backward at full speed
    delay(500);            // wait for 0.5 seconds
  }
 }

void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);  // initiliaze the LEDs

  // debugging, check initial LED status
  leds[0] = CRGB::Black;
  leds[1] = CRGB::Black;
  FastLED.show();
  delay(500);
  
  // This section creates the I2C (Wire) connection and quieries for a VL53LX
  Wire.begin();
  sensor.setTimeout(500);
  Serial.println("Initializing VL53L0X sensor...");
  if (!sensor.init()){
    Serial.println("Failed to detect and initialize sensor!");
    while (1) {
      errorLed(3); // indicate error with LEDS
      Serial.println("Error: Sensor not detected");
  }
  } else {
    Serial.println("Sensor initialized successfully");
  }
  
  // Set the 4 motor controll 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);
  
  setEmotion("happy"); // set initial emotion
}

void loop() {
  int distance = sensor.readRangeSingleMillimeters(); // read distance from sensor
  if (sensor.timeoutOccurred()) {
    Serial.println("Sensor timeout!");
    errorLed(3); // indicate error with LEDs
  } else {
    Serial.print("Distance: ");
    Serial.println(distance);

// set emotion based on distance
  if (distance < 50){ //Real close! Avoid
    setEmotion("afraid");
  } else if (distance < 100){ // getting closer, not good
    setEmotion("angry"); 
  } else if (distance < 200){ // Getting close, slow down
    setEmotion("sad");
  } else {
    setEmotion("happy");
  }
  }
  delay(1000); // to make output readable, wait for 1 second
}