/*
6/6/2024
Kyle King, and Miles Oyen
code for the esp32_rbt.0 board for final assignment to show emotions

Expected serial output is a list of distances based on the VLX530X module

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>
#define NUM_LEDS 2
CRGB leds[NUM_LEDS];

#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18
#define WSLED 4

uint32_t lastTime = 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(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
}

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
}

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
}

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
}

void off(){
  ledcWrite(2, 0); //Set M2A to LOW
  ledcWrite(3, 0); //Set M2B to LOW
  ledcWrite(0, 0); //Set M1A to LOW
  ledcWrite(1, 0); //Set M1BA to LOW
}
//Each motor (M1 and M2) has two pins connected.
//Setting one pin HIGH and the other LOW will make the motor turn one way
//Setting them in reverse will make it turn the opposite way
//Seting them both HIGH or both LOW stops the motor


// Based on the wight of the robot, some PWM speeds are too slow for
// the robot to be able to move. Decreasing SLOWEST_SPEED will
// let your robot go slower, but too low and it might not move on the
// lowest settings
#define SLOWEST_SPEED 190

void setup() {
  // Setup Red LED as an Output (Use pin 33)
  pinMode(33, OUTPUT);
  
  // Setup Green LED as an Output (Use pin 32)
  pinMode(32, OUTPUT);
  
  // Setup Blue LED as an Output (Use pin 25)
  pinMode(25, OUTPUT);
  

  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);  // GRB ordering is assumed
  
  // This section creates the I2C (Wire) connection and quieries for a VL53LX
  Wire.begin();
  sensor.setTimeout(500);
  if (!sensor.init())
  {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) {errorLed();}
  }
  
  //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);
  forward(200);
  
}
//
//
//
//
//
//
//
//
//
//
//int red = 33;
//int green = 32;
//int blue = 25;
void loop() {
  
  /* this is scared*/
  for (int i = 0; i <3; i++){
    for (int i = 255; i > 0; i--){ //Counts down to dimmer
      leds[0].setRGB( i, i, 0); //This command makes the leds[] array location get a new color
      leds[1].setRGB( i, i, 0); //This command makes the leds[] array location get a new color
      FastLED.show(); //This command makes the LEDs change to match values in the array leds[]
      delay(1);
    }
    delay(300);
    for (int i = 0; i < 256; i++){ // Counts up to brighter
      leds[0].setRGB( i, i, 0); //This command makes the leds[] array location get a new color
      leds[1].setRGB( i, i, 0); //This command makes the leds[] array location get a new color
      FastLED.show(); //This command makes the LEDs change to match values in the array leds[]
      delay(1);
    }
    for(int i = 0;i < 50; i++){
      ccw(500);
      delay(30);
      cw(500);
      delay(30);
      reverse(200);
      delay(10);
    }
    off();
    delay(1000);
  }
  
  /* this is happy*/
  for (int i = 0; i <2; i++){
    for (int i = 255; i > 0; i--){ //Counts down to dimmer
      leds[0].setRGB( 0, i, 0); //This command makes the leds[] array location get a new color
      leds[1].setRGB( 0, i, 0); //This command makes the leds[] array location get a new color
      FastLED.show(); //This command makes the LEDs change to match values in the array leds[]
      delay(1);
    }
    delay(300);
    for (int i = 0; i < 256; i++){ // Counts up to brighter
      leds[0].setRGB( 0, i, 0); //This command makes the leds[] array location get a new color
      leds[1].setRGB( 0, i, 0); //This command makes the leds[] array location get a new color
      FastLED.show(); //This command makes the LEDs change to match values in the array leds[]
      delay(1);
    }
    for(int i = 0;i < 2; i++){
      forward(230);
      delay(100);
      ccw(500);
      delay(1000);
      cw(500);
      delay(100);
      ccw(500);
      delay(100);
      cw(500);
      delay(100);
      reverse(230);
      delay(100);
      ccw(500);
      delay(1000);
      cw(500);
      delay(100);
      ccw(500);
      delay(100);
      cw(500);
      delay(100);
    }
    off();
    delay(1000);
  }
  
  /* this is angry*/
  for (int i = 0; i < 2; i++){
    for (int i = 255; i > 0; i--){ //Counts down to dimmer
      leds[0].setRGB( i, 0, 0); //This command makes the leds[] array location get a new color
      leds[1].setRGB( i, 0, 0); //This command makes the leds[] array location get a new color
      FastLED.show(); //This command makes the LEDs change to match values in the array leds[]
      delay(1);
    }
    delay(100);
    for (int i = 0; i < 256; i++){ // Counts up to brighter
      leds[0].setRGB( i, 0, 0); //This command makes the leds[] array location get a new color
      leds[1].setRGB( i, 0, 0); //This command makes the leds[] array location get a new color
      FastLED.show(); //This command makes the LEDs change to match values in the array leds[]
      delay(1);
    }
    for(int i = 0;i < 4; i++){
      ccw(500);
      delay(50);
      cw(500);
      delay(50);
      reverse(200);
      delay(200);
      forward(500);
      delay(400);
      off();
      delay(50);
    }
    delay(1000);
  }
  
  /* this is sad*/
  for (int i = 0; i <2; i++){
    for (int i = 190; i > 0; i--){ //Counts down to dimmer
      leds[0].setRGB( 0, 0, i); //This command makes the leds[] array location get a new color
      leds[1].setRGB( 0, 0, i); //This command makes the leds[] array location get a new color
      FastLED.show(); //This command makes the LEDs change to match values in the array leds[]
      delay(1);
    }
    delay(150);
    for (int i = 0; i < 190; i++){ // Counts up to brighter
      leds[0].setRGB( 0, 0, i); //This command makes the leds[] array location get a new color
      leds[1].setRGB( 0, 0, i); //This command makes the leds[] array location get a new color
      FastLED.show(); //This command makes the LEDs change to match values in the array leds[]
      delay(1);
    }
    for(int i = 0;i < 2; i++){
      forward(200);
      delay(1000);
      off();
      delay(100);
      cw(200);
      delay(500);
      off();
      delay(50);
      ccw(200);
      delay(600);
      off();
      delay(100);
      ccw(200);
      delay(550);
      off();
      delay(50);
      cw(200);
      delay(500);
      off();
      delay(50);
      reverse(200);
      delay(900);
      off();
      delay(1000);
    }
    delay(3000);
  }
}