/* ENGR 103 Final Project
 *  Alex Loomis
 *  Angel Rodriguez
 *  Emotions Challenge
 */

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

// Define LED parameters
#define NUM_LEDS 2
CRGB leds[NUM_LEDS];
#define WSLED 4

// Define motor pins
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18

// Define motor speed constants
#define SLOWEST_SPEED 190
#define STOP_SPEED 128
#define MAX_SPEED 255

void errorLed(int count = 1){
  // Displays erros using LED lights, credit to Don Heer
  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);
  
  // Set up sensor
  Wire.begin();
  sensor.setTimeout(500);
  if (!sensor.init())
  {
    Serial.println("Failed to detect and initialize sensor!"); // Credit to Don Heer's "Bumpbot.ino" code
    while (1) {errorLed();}
  }  

  // Set up LEDs
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);

  // Set up motors
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);
  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(); // Stores sensor readings in 'distance', in units of mm
  Serial.println(distance);

  if(distance < 200) {
    // Turn around slowly if close to an obstacle
    for (int j = 0; j < NUM_LEDS; j++) leds[j] = CRGB::Black;
    FastLED.show();
    motor(STOP_SPEED, STOP_SPEED);
    delay(2000);
    motor(0, SLOWEST_SPEED); // Changed SLOWEST_SPEED to 0 to make it rotate in reverse rather than forward
    delay(650);
    motor(SLOWEST_SPEED, SLOWEST_SPEED);
    delay(1000);
    motor(STOP_SPEED, STOP_SPEED);
    delay(500);
  } else {
    // Perform emotion actions, but check distance between each action to ensure stopping mid-action if necessary
    EmotionCheck(&angry, 4000);
    EmotionCheck(&sad, 4000);
    EmotionCheck(&scared, 4000);
    EmotionCheck(&happy, 4000);
  }
  
  delay(1000); 
}

void EmotionCheck(void (*emotionAction)(), int actionDuration) { // Function to stop an emotion action prematurely if sensor reading gets low
  int startTime = millis();
  while (millis() - startTime < actionDuration) {
    int distance = sensor.readRangeSingleMillimeters();
    Serial.println(distance);
    if (distance < 200) {
      // Turn around slowly if close to an obstacle
      for (int j = 0; j < NUM_LEDS; j++) leds[j] = CRGB::Black;
      FastLED.show();
      motor(STOP_SPEED, STOP_SPEED);
      delay(2000);
      motor(0, SLOWEST_SPEED); // Changed SLOWEST_SPEED to 0 to make it reverse //
      delay(650);
      motor(SLOWEST_SPEED, SLOWEST_SPEED);
      delay(1000);
      motor(STOP_SPEED, STOP_SPEED);
      delay(500);
      return; // Exit the action early
    }
    emotionAction(); // Perform the current emotion action
    delay(100); // Small delay to allow for sensor reading updates
  }
}

void motor(uint8_t left, uint8_t right) { // motor function, which takes in unsigned integers for the wheels//

   if (left == STOP_SPEED) {
    ledcWrite(3, 0);
    ledcWrite(2, 0);
  } else if (left > STOP_SPEED) { // Left wheel move forward (direction of sensor)//
    left = map(left, STOP_SPEED, MAX_SPEED, SLOWEST_SPEED, MAX_SPEED);
    ledcWrite(3, left);
    ledcWrite(2, 0);
  } else { // Left wheel move backward (direction of wires)//
    left = map(left, STOP_SPEED, 0, SLOWEST_SPEED, MAX_SPEED);
    ledcWrite(3, 0);
    ledcWrite(2, left);
  }
  
  if (right == STOP_SPEED) {
    ledcWrite(1, 0);
    ledcWrite(0, 0);
  } else if (right > STOP_SPEED) { // Right wheel move forward (direction of sensor)//
    right = map(right, STOP_SPEED, MAX_SPEED, SLOWEST_SPEED, MAX_SPEED);
    ledcWrite(1, right);
    ledcWrite(0, 0);
  } else { // Right wheel move backward (direction of wires)//
    right = map(right, STOP_SPEED, 0, SLOWEST_SPEED, MAX_SPEED);
    ledcWrite(1, 0);
    ledcWrite(0, right);
  }
}

void angry() {
  // Red LED lights
  for (int i = 0; i < NUM_LEDS; i++) leds[i] = CRGB::Red;
  FastLED.show();

  // Move side to side
  for (int i = 0; i < 3; i++) {
    motor(MAX_SPEED, STOP_SPEED);
    delay(500);
    motor(STOP_SPEED, MAX_SPEED);
    delay(500);
  }

  // Move forward really fast
  motor(MAX_SPEED, MAX_SPEED);
  delay(1200);
  motor(STOP_SPEED, STOP_SPEED);
  delay(1000);
}

void sad() {
  // Blue LED lights
  for (int i = 0; i < NUM_LEDS; i++) leds[i] = CRGB::Blue;
  FastLED.show();

  // Turn around and walk away slowly
  motor(SLOWEST_SPEED, STOP_SPEED);
  delay(1500);
  motor(SLOWEST_SPEED, SLOWEST_SPEED);
  delay(3200);
  motor(STOP_SPEED, STOP_SPEED);
  delay(1000);
}

void scared() {
  // Blinking lights
  for (int i = 0; i < 10; i++) {
    for (int j = 0; j < NUM_LEDS; j++) leds[j] = CRGB::White;
    FastLED.show();
    delay(100);
    for (int j = 0; j < NUM_LEDS; j++) leds[j] = CRGB::Black;
    FastLED.show();
    delay(100);
  }

  // Back away slowly
  motor(0, 0);
  delay(1200);

  // Turn around and run away in zigzag
  for (int i = 0; i < 3; i++) {
    motor(MAX_SPEED, 0);
    delay(500);
    motor(0, MAX_SPEED);
    delay(500);
  }
  motor(MAX_SPEED, MAX_SPEED);
  delay(1000);
  motor(STOP_SPEED, STOP_SPEED);
  delay(1000);
}

void happy() {
  // Yellow LED lights
  for (int i = 0; i < NUM_LEDS; i++) leds[i] = CRGB::Yellow;
  FastLED.show();

  // Spin and go in a circle
  motor(MAX_SPEED, 0);
  delay(2000);
  motor(STOP_SPEED, STOP_SPEED);

  // Dance back and forth
  for (int i = 0; i < 2; i++) {
    motor(SLOWEST_SPEED, SLOWEST_SPEED);
    delay(800);
    motor(64, 64);
    delay(800);
  }
  motor(STOP_SPEED, STOP_SPEED);
  delay(1000);
}