/*
Steven Kuna
Robot Final Assignment
Emotion Challenge
*/

#include <Wire.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

//Happy
#define HAPPY 1
#define HFWRDSPEED 230
#define HRVRSSPEED 255
#define HTURNSPEED 255

//Sad
#define SAD 2
#define SFWRDSPEED 180
#define SRVRSSPEED 180
#define STURNSPEED 200

//Angry
#define ANGRY 3
#define AFWRDSPEED 200
#define ARAMSPEED 255
#define ARVRSSPEED 200
#define ATURNSPEED 255

//Scared/aFraid
#define SCARED 4
#define FFWRDSPEED 180
#define FRVRSSPEED 255
#define FTURNSPEED 255

//Blinking parameters
#define EYEOPEN 2000 //ms
#define BLINK 250 //ms

uint32_t previousMillis = 0;
int emotion = HAPPY;
int eyeState = 1; //0 = Off, 1 = On
int blinkCycleCounter = 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 reverse(uint8_t mySpeed){
  ledcWrite(M2B, mySpeed); //Set M2B to mySpeed
  ledcWrite(M2A, 0); //Set M2A to LOW
  ledcWrite(M1B, mySpeed);//Set M1B to mySpeed
  ledcWrite(M1A, 0); //Set M1A to LOW
}

void forward(uint8_t mySpeed){
  ledcWrite(M2A, mySpeed); //Set M2A to mySpeed
  ledcWrite(M2B, 0); //Set M2B to LOW
  ledcWrite(M1A, mySpeed);//Set M1A to mySpeed
  ledcWrite(M1B, 0); //Set M1B to LOW
}

void cw(uint8_t mySpeed){ //Right
  ledcWrite(M2B, mySpeed); //Set M2B to mySpeed
  ledcWrite(M2A, 0); //Set M2A to LOW
  ledcWrite(M1B, 0); //Set M1B to LOW
  ledcWrite(M1A, mySpeed); //Set M1A to mySpeed
}

void ccw(uint8_t mySpeed){ //Left
  ledcWrite(M2A, mySpeed); //Set M2A to mySpeed
  ledcWrite(M2B, 0); //Set M2B to LOW
  ledcWrite(M1A, 0); //Set M1A to LOW
  ledcWrite(M1B, mySpeed); //Set M1BA to mySpeed
}

void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);  // GRB ordering is assumed
  
  //Confirms distance sensor is connected, throws error if not
  Wire.begin();
  sensor.setTimeout(500);
  if (!sensor.init())
  {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) {errorLed();}
  }
  
  //Sets motor pins to output
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  //Connects motors
  ledcAttach(M1A, 30000, 8);
  ledcAttach(M1B, 30000, 8);
  ledcAttach(M2A, 30000, 8);
  ledcAttach(M2B, 30000, 8);
}

void loop() {
  int distance = sensor.readRangeSingleMillimeters();
  uint32_t currentMillis = millis();

  //Blinking
  if(eyeState){
    if(currentMillis - previousMillis >= EYEOPEN) {
      previousMillis = currentMillis;
      eyeState = 0;
    }
  } else {
    if(currentMillis - previousMillis >= BLINK) {
      previousMillis = currentMillis;
      eyeState = 1;
      blinkCycleCounter++;
    }
  }

  //Switches emotion after 5 blink cycles
  if(blinkCycleCounter >= 5){
    blinkCycleCounter = 0;
    emotion++;
    if(emotion > SCARED){
      emotion = HAPPY;
    }
  }

  switch (emotion) {
    case HAPPY:
      if (distance < 50) { //Real close! Avoid
        if(eyeState) { //1/4 bright eyes
          leds[0].setRGB(0,63,0);
          leds[1].setRGB(0,63,0);
          FastLED.show();
        }
        reverse(HRVRSSPEED);
        delay(200);
        cw(HTURNSPEED);
        delay(150);
      } else if (distance < 200) { // Getting close, slow down
        if(eyeState) { //1/2 bright eyes
          leds[0].setRGB(0,127,0);
          leds[1].setRGB(0,127,0);
          FastLED.show();
        }
        forward(HFWRDSPEED - (30 - ((distance / 200.0) * 30)));
      } else { 
        if(eyeState) { //Full bright eyes
          leds[0].setRGB(0,255,0);
          leds[1].setRGB(0,255,0);
          FastLED.show();
        }
        forward(HFWRDSPEED);
      }
      break;
    
    case SAD:
      if (distance < 50) { //Real close! Avoid
        if(eyeState) { //1/8 bright eyes
          leds[0].setRGB(0,0,31);
          leds[1].setRGB(0,0,31);
          FastLED.show();
        }
        reverse(SRVRSSPEED);
        delay(300);
        cw(STURNSPEED);
        delay(150);
      } else if (distance < 200) { // Getting close, slow down
        if(eyeState) { //1/4 bright eyes
          leds[0].setRGB(0,0,63);
          leds[1].setRGB(0,0,63);
          FastLED.show();
        }
        forward(SFWRDSPEED - (20 - ((distance / 200.0) * 20)));
      } else { 
        if(eyeState) { //1/2 bright eyes
          leds[0].setRGB(0,0,127);
          leds[1].setRGB(0,0,127);
          FastLED.show();
        }
        forward(SFWRDSPEED);
      }
      break;

    case ANGRY:
      if (distance < 50) { //Hit it, turn around
        if(eyeState) { //Full bright eyes
          leds[0].setRGB(255,0,0);
          leds[1].setRGB(255,0,0);
          FastLED.show();
        }
        reverse(ARVRSSPEED);
        delay(200);
        cw(ATURNSPEED);
        delay(150);
      } else if (distance < 250) { // Getting close, ram into it!
        if(eyeState) { //Full bright eyes
          leds[0].setRGB(255,0,0);
          leds[1].setRGB(255,0,0);
          FastLED.show();
        }
        forward(ARAMSPEED);
      } else { 
        if(eyeState) { //1/2 bright eyes
          leds[0].setRGB(127,0,0);
          leds[1].setRGB(127,0,0);
          FastLED.show();
        }
        forward(AFWRDSPEED);
      }
      break;

    case SCARED:
      if (distance < 50) { //Real close! Freak out and run away
        if(eyeState) { //Full bright eyes
          leds[0].setRGB(255,255,255);
          leds[1].setRGB(255,255,255);
          FastLED.show();
        }
        reverse(FRVRSSPEED);
        delay(200);
        cw(FTURNSPEED);
        delay(1350);
      } else { 
        if(eyeState) { //1/4 bright eyes
          leds[0].setRGB(63,63,63);
          leds[1].setRGB(63,63,63);
          FastLED.show();
        } //Zigzag/jitter/shake forward
        cw(FFWRDSPEED);
        delay(60);
        forward(FFWRDSPEED);
        delay(50);
        ccw(FFWRDSPEED);
        delay(50);
        forward(FFWRDSPEED);
        delay(50);
      }
      break;
    
  }

  //Blink
  if(!eyeState){
    leds[0].setRGB(0,0,0);
    leds[1].setRGB(0,0,0);
    FastLED.show();
  }

  Serial.print("emotion: ");
  Serial.print(emotion);
  Serial.print(", ");
  Serial.print("eyeState: ");
  Serial.print(eyeState);
  Serial.print(", ");
  Serial.print("blinkCycleCounter: ");
  Serial.print(blinkCycleCounter);
  Serial.print(", ");
  Serial.print("distance: ");
  Serial.println(distance);

}