////Code by Legend Engberg
///So for this code the Arduino is suppose to combine the LEDs with it's motor skills, helping to display is emotions. make sure that green is connected to M2B, red with 1014, blue with 1013, and white with GND (GND with GND). It should display four emotinal states including:
//1. Happy: Spins for 5 seconds with green LEDs, then pauses for 5 seconds with red LEDs. (resembles: happy twirl)
//2. Angry: Jerks forward and backward in a circular motion for 5 seconds with red and yellow LEDs flashing, then pauses for 5 seconds with red LEDs. (resembles: The mad dog back off warning)
//3. Somber: Moves forward slowly for 5 seconds with blue LEDs on, then pauses for 5 seconds with red LEDs. (resembles:walk of shame) 
//4. Panic: Moves backward quickly and spins with blinking LEDs for 10 seconds, then stops indefinitely with red LEDs. (resembles: frantically worried movments, pacing!)
//The LED will turn red when the bot is resting and is staying still, just pure red, nothing should be flashing. Each emotion is linked to different demostrations of LEDs, Happy is a sigular green, angry is flashing yellow and red, Somber is a sigular blue, and panic is red, blue and green constantly flashing, those should be in this order with the red light turning on for five seconds inbetween each motion









//Wire Library access to Arduino Core Libraries for I2C protocol (microcontroller connections)
#include <Wire.h>

// Motor control pins
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18

// LED control pins
#define LED_RED_PIN 25
#define LED_GREEN_PIN 26
#define LED_BLUE_PIN 27
#define BACK_LED_RED_PIN 14
#define BACK_LED_GREEN_PIN 12
#define BACK_LED_BLUE_PIN 13

// Speed definitions (speed options)
// *Do not adjust SLOW_SPEED below 190 or Bot will not move*
#define STOP_SPEED 128
#define FAST_SPEED 255
#define SLOW_SPEED 190

//Set up Motor Values
void motor(uint8_t left, uint8_t right) {
  // Left Motor Function Values Set Up 
  if (left == STOP_SPEED) { // Set left to stop
    ledcWrite(3, 0); // Set M2B to LOW
    ledcWrite(2, 0); // Set M2A to LOW
  } else if (left > STOP_SPEED) { // Set left to forward
    left = map(left, STOP_SPEED, 255, SLOW_SPEED, 255);
    ledcWrite(3, 0); // Set M2B to LOW
    ledcWrite(2, left); // Set M2A to pwm
  } else { // Set left to reverse
    left = map(left, STOP_SPEED, 0, SLOW_SPEED, 255);
    ledcWrite(3, left); // Set M2B to pwm
    ledcWrite(2, 0); // Set M2A to LOW
  }

  // Right Motor Function Values Set Up 
  if (right == STOP_SPEED) { // Set right to stop
    ledcWrite(1, 0); // Set M2B to LOW
    ledcWrite(0, 0); // Set M2A to LOW
  } else if (right > STOP_SPEED) { // Set right to forward
    right = map(right, STOP_SPEED, 255, SLOW_SPEED, 255);
    ledcWrite(1, 0); // Set M2B to LOW
    ledcWrite(0, right); // Set M2A to pwm
  } else { // Set right to reverse
    right = map(right, STOP_SPEED, 0, SLOW_SPEED, 255);
    ledcWrite(1, right); // Set M2B to pwm
    ledcWrite(0, 0); // Set M2A to LOW
  }
}

void setup() {
  // Set the 4 motor control lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  // Set the LED pins as outputs
  pinMode(LED_RED_PIN, OUTPUT);
  pinMode(LED_GREEN_PIN, OUTPUT);
  pinMode(LED_BLUE_PIN, OUTPUT);
  pinMode(BACK_LED_RED_PIN, OUTPUT);
  pinMode(BACK_LED_GREEN_PIN, OUTPUT);
  pinMode(BACK_LED_BLUE_PIN, OUTPUT);

  // Initialize the PWM channels for the motors
  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);

  // Initialize the PWM channels for the front LEDs
  ledcSetup(4, 5000, 8);
  ledcAttachPin(LED_RED_PIN, 4);
  ledcSetup(5, 5000, 8);
  ledcAttachPin(LED_GREEN_PIN, 5);
  ledcSetup(6, 5000, 8);
  ledcAttachPin(LED_BLUE_PIN, 6);

  // Initialize the PWM channels for the back LEDs
  ledcSetup(7, 5000, 8);
  ledcAttachPin(BACK_LED_RED_PIN, 7);
  ledcSetup(8, 5000, 8);
  ledcAttachPin(BACK_LED_GREEN_PIN, 8);
  ledcSetup(9, 5000, 8);
  ledcAttachPin(BACK_LED_BLUE_PIN, 9);
}

void setLEDColor(uint8_t red, uint8_t green, uint8_t blue) { 
  // Set the color for LEDs
  ledcWrite(4, red);
  ledcWrite(5, green);
  ledcWrite(6, blue);
  ledcWrite(7, red);
  ledcWrite(8, green);
  ledcWrite(9, blue);
}

void blinkLEDs() {
  // Blink LEDs between red and blue
  setLEDColor(255, 0, 0); // Blink red
  delay(250); // 2.5 second delay
  setLEDColor(0, 0, 255); // Blink blue
  delay(250); // 2.5 second delay
}

/// EMOTION SIMULATION UNITS--------------------------------------------------------------------------------------------------------------------------

void loop() {
  // EMOTION SIMULATION 1: Happy
  // Spin around for 5 seconds and turn LEDs green 
  setLEDColor(0, 255, 0); // Set LED to green
  motor(FAST_SPEED, 0); // Spin left motor forward at max speed
  motor(0, FAST_SPEED); // Spin right motor backward at max speed
  delay(5000);
  
  // Stop and turn LEDs red
  motor(STOP_SPEED, STOP_SPEED);
  setLEDColor(255, 0, 0); // Set LED to red
  delay(5000);  ///Simulation transion with delay for 5 seconds 

  // EMOTION SIMULATION 2: Angery
  //(Imagine it's telling people to back off like a dog)
  // It should make the bot jerk back and fouth backwards in a circular motion with red and yellow LEDS flashing
  setLEDColor(255, 0, 0); // Set LED to red
  unsigned long start = millis();
  while (millis() - start < 5000) {
    motor(FAST_SPEED, FAST_SPEED); // Move forward quickly
    delay(250);
    motor(0, STOP_SPEED); // Move backward quickly
    delay(250); 
  }
  
  // Pause for 5 seconds with red LEDs on
  motor(STOP_SPEED, STOP_SPEED);
  setLEDColor(255, 0, 0); // Set LED to red
  delay(5000); ///Simulation transion with delay for 5 seconds 

  // EMOTION SIMULATION 3: Sad Walk
  // Move forward slowly for 5 seconds with blue LEDs
  setLEDColor(0, 0, 255); // Set LED to blue
  motor(SLOW_SPEED, SLOW_SPEED); // Move forward slowly
  delay(5000);
  
  // Pause for 5 seconds with red LEDs
  motor(STOP_SPEED, STOP_SPEED);
  setLEDColor(255, 0, 0); // Set LED to red
  delay(5000); ///Simulation transion with delay for 5 seconds 

  // EMOTION SIMULATION 4: Panic
  //(Its pacing around like it doesn't know what to do)
  // Move backward quickly and spin with blinking LEDs for 10 seconds
  unsigned long panickedStart = millis(); //Records current time in milliseconds from when bot started running
  while (millis() - panickedStart < 10000) {    //Run code, checking for if 10 seconds have passed since panickedStart
    motor(FAST_SPEED, FAST_SPEED); // Move backward quickly
    blinkLEDs(); // Blink LEDs
    motor(FAST_SPEED, 0); // Spin left motor forward at max speed
    motor(0, FAST_SPEED); // Spin right motor backward at max speed
    blinkLEDs(); // Blink LEDs
  }
  
  // Stop and turn LEDs red
  motor(STOP_SPEED, STOP_SPEED);
  setLEDColor(255, 0, 0); // Set LED to red
  while (true) {
    delay(1000); // Keep the bot stopped //end of program
  }
}