/*
 Jack Kautzsch
 Robot Emotions Challenge
 06/06/2024 
 */

const int motor1f = 16;
const int motor1b = 17;
const int motor2f = 19;
const int motor2b = 18;
unsigned long previousmillis1 = 0;
unsigned long previousmillis2 = 0;
unsigned long previousmillis3 = 0;
unsigned long previousmillis4 = 0;
unsigned long emotionStartTime = 0;
unsigned long emotionTime = 5000;
unsigned long pulseTime = 5000;
const long interval1 = 200;
const long interval2 = 100;
const long interval3 = 10000;
const long interval4 = 10000;
int ledState = 0, motorState = 0;
bool isScared = true;
#include <FastLED.h>
# define NUM_LEDS 2
CRGB leds[NUM_LEDS];
#define SLOWEST_SPEED 190
#define WSLED 4

void motor(uint8_t left, uint8_t right){
  if (left == 128) {
    ledcWrite(0,0);
    ledcWrite(1,0);
  }
  else if(left > 128){
    left = map(left, 128, 255, SLOWEST_SPEED, 255);
    ledcWrite(1, 0);
    ledcWrite (0, left);
  }
  else{
   left = map(left, 128, 0, SLOWEST_SPEED, 255);
   ledcWrite(1, left);
   ledcWrite(0,0);
  }
  if (right == 128){
   ledcWrite(2, 0);
    ledcWrite(3, 0);
  }
  else if(right > 128){
    right = map(right, 128, 255, SLOWEST_SPEED, 255);
    ledcWrite(3,0);
    ledcWrite(2, right);
  }
  else {
    right = map(right, 128, 0, SLOWEST_SPEED, 255);
    ledcWrite(3, right);
    ledcWrite(2, 0);
  }
}

int swerve(){
  for(int i = 0; i < 3; i++){
    for(int j = 0; j < 4; j++){
      motor (210, 148);
      delay (700);
      motor (130, 210);
      delay (700);
    }
    motor (248, 168);
    delay (3600);
  }
}
int flashing(){
  for(int a = 0; a < 30; a++){
      leds[0].setRGB (0,255,0);
      leds[1].setRGB (0,255,0);
      FastLED.show();
      delay(50);
      leds[0].setRGB (0,0,0);
      leds[1].setRGB (0,0,0);
      FastLED.show();
      delay(50);
  }
}
void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);

  pinMode(18, OUTPUT);
  pinMode(19,OUTPUT);
  pinMode(16, OUTPUT);
  pinMode(17, OUTPUT); 

//set up for LEDS
  ledcSetup(0, 30000, 8);
  ledcAttachPin(motor1f, 0);
  ledcSetup(1, 30000, 8);
  ledcAttachPin(motor1b, 1);
  ledcSetup(2, 30000, 8);
  ledcAttachPin(motor2f, 2);
  ledcSetup(3, 30000, 8);
  ledcAttachPin(motor2b, 3);

  FastLED.addLeds<NEOPIXEL, WSLED> (leds, NUM_LEDS);
}

void loop() {
    unsigned long currentMillis = millis();
  //happy
  //sets the colors on the LEDs to represent yellow
  //each number represents how much of that color on a gradient
  leds[0].setRGB (252, 201, 61);
  leds[1].setRGB (252, 201, 61);
  FastLED.show();
  for(int i = 0; i < 3; i++){
    motor(165,200);
      delay(3000);
      motor(0,255);
      delay (1000);
  }
  motor(128,128);
  delay(2000);
  
  //sad
  leds[0].setRGB (0,0,255);
  leds[1].setRGB (0,0,255);
  FastLED.show();
  //function call for swerve
  swerve();
  //robot throws a fit at the end and shakes a little
  for(int i = 0; i < 8; i++){
  motor (0,255);
  delay(100);
  motor (255,0);
  delay (100);
  }
  motor(128,128);
  delay(2000);
    // want the robot to swerve in a direction generally straight
    // after swerving for some distance, want it to turn around and repeat

//   scared
  if(isScared){
  leds[0].setRGB (0,255,0);
  leds[1].setRGB (0,255,0);
  FastLED.show();
//  robot turns around
  motor(255, 0);
  delay(600);
   
  //the robot starts at max speed then decreases speed by 5 until it stops.
  for(int i = 255; i > 128; i-=5){
    motor(i,i);
    delay (250);
  }
  motor(128,128);
  delay(500);

  for( int i = 0; i < 8; i++){
    motor(255, 0);
    delay(100);
    motor(0,255);
    delay(100);
  }
  
  emotionStartTime = millis();
  isScared = false;
  }
  /*
  I tried using millis to run two functions at the same time.
  My goal was to have the LEDS to flash when the robot was shaking for 
  the scared emotion but when trying to piece all the emotions together 
  I could not get it to work. I tried trouble shooting it and in the end
  I had to scrap the idea because I could not get anymore help from the TAs
  or Don. I was really disappointed to have had to cut it out but I want
  to leave it in here because it was alot of work that I went into office 
  hours for I just was not able to show it in my video.  
// Once robot runs away, it will twitch to show it is still shaken up

  if(currentMillis - emotionStartTime <= emotionTime){
    while(emotionTime > currentMillis){
      currentMillis = millis();
      if(currentMillis - previousmillis1 >= interval1){
        previousmillis1 = currentMillis;
        if (ledState == 1){
          leds[0].setRGB (0,0,0);
          leds[1].setRGB (0,0,0);
          FastLED.show();
          ledState = 0;
        } else {
         leds[0].setRGB (0,255,0);
          leds[1].setRGB (0,255,0);
          FastLED.show();
          ledState = 1;
        }
      }
   
  

  if(currentMillis - previousmillis2 >= interval2){
    previousmillis2 = currentMillis;
    if (motorState == 1){
      motor(0, 255);
      motorState = 0;
    } else {
       motor (255,0);
       motorState = 1;
      }
    }
  }
}
  motor(128,128);
  ledState = 0;
  emotionTime = millis() + 5000;
//while(1);

/*
emotionTime = millis() + 5000;
motor(128,128);
delay(2000);
*/

 // if(currentMillis - previousmillis2 >= interval2){
 //   previousmillis2 = currentMillis;
//  for(int j = 0; j <3; j++){
 //    for(int i = 0; i < 8; i++){
 //      motor (0, 255);
 //      delay(100);
 //      motor (255, 0);
 //      delay (100);
 //   }
 // }
 // }
  motor(128,128);
  delay(2000);
  
  //angry

  leds[0].setRGB(255,0,0);
  leds[1].setRGB(255,0,0);
  FastLED.show();
  //robot will pace in a oval shape
  //while circling, it will have a burst of rage and "punch"
  // it will punch by going forward adn backward quickly before returning to pace.
  for (int i = 0; i < 4; i++){
    motor(168, 208);
    delay(3000);
    motor(255,0);
    delay(300);
    motor(0,255);
    delay(300);
    motor(255, 255);
    delay(750);
    motor(73,73);
    delay(1250);
  }
}