/*
Emma Piccione
06/04/2024
Robot Showcase
Final Project
*/

#include <Wire.h>
#include <VL53L0X.h>
VL53L0X sensor; //sensor setup

#include <FastLED.h>
#define NUM_LEDS 2
CRGB leds[NUM_LEDS]; //LED setup

//define sensor pins and motor pins

#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);
}

//set dance moves

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 halt() {
  ledcWrite(3, 0); // set M2B to mySpeed
  ledcWrite(2, 0); // set M2A to LOW
  ledcWrite(1, 0); // set M1B to LOW
  ledcWrite(0, 0); // set M1A to mySpeed
}

void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);  // GRB ordering is assumed

  //ensures sensor works
  Wire.begin();
  sensor.setTimeout(500);
  if (!sensor.init())
  {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) {errorLed();}
  }
  
  // set the 4 motor control lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  // 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);
  
}

void loop() {

  halt();
  delay(10000);
  
  // intro: turn left and hold
  ccw(200);
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(300);
  halt();
  delay(14000);

  cw(200);
  leds[0] = CRGB::Blue;
  leds[1] = CRGB::Blue;
  FastLED.show();
  delay(300);
  halt();
  delay(1000);

  // verse 1

  //forward backward movement
  forward(200);
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(2000);
  halt();
  delay(8000);

  reverse(200);
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(1000);
  halt();
  delay(7000);

  //back and forth sway
  ccw(200);
  delay(300);
  halt();
  delay(500);
  cw(200);
  delay(300);
  halt();
  delay(500);
  cw(200);
  delay(300);
  halt();
  delay(500);
  ccw(200);
  delay(300);
  halt();
  delay(500);
  ccw(200);
  delay(300);
  halt();
  delay(500);
  cw(200);
  delay(300);
  halt();
  delay(500);
  cw(200);
  delay(300);
  halt();
  delay(500);
  ccw(200);
  delay(300);
  halt();
  delay(500);
  cw(200);
  delay(300);
  halt();
  delay(500);

  //faster back and forth
  ccw(200);
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(200);

  cw(200);
  leds[0] = CRGB::White;
  leds[1] = CRGB::White;
  FastLED.show();
  delay(200);

  cw(200);
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(200);

  ccw(200);
  leds[0] = CRGB::White;
  leds[1] = CRGB::White;
  FastLED.show();
  delay(200);

  ccw(200);
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(200);

  cw(200);
  leds[0] = CRGB::White;
  leds[1] = CRGB::White;
  FastLED.show();
  delay(300);

  //segue with forward and reverse
  forward(200);
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(2000);
  halt();
  delay(10000);

  reverse(200);
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(2000);
  halt();
  delay(5000);

  ccw(200);
  delay(500);
  halt();
  delay(4000);

  cw(200);
  delay(2000);

  // chorus
  
  forward(200);
  leds[0] = CRGB::Blue;
  leds[1] = CRGB::Blue;
  FastLED.show();
  delay(1000);
  halt();
  delay(5000);

  reverse(200);
  leds[0] = CRGB::Blue;
  leds[1] = CRGB::Blue;
  FastLED.show();
  delay(1000);
  halt();
  delay(5000);

  //small turn and back to center
  ccw(200);
  delay(300);
  halt();
  delay(3000);
  cw(200);
  delay(300);
  halt();
  delay(1000);

  forward(200);
  delay(500);
  halt();
  delay(3500);

  //side step, switch directions and repeat
  
  ccw(200);
  delay(500);
  halt();
  delay(1000);
  forward(200);
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(500);
  halt();
  delay(500);
  forward(200);
  delay(500);
  halt();
  delay(500);
  forward(200);
  delay(500);
  halt();
  delay(2000);
  
  cw(200);
  delay(500);
  halt();
  delay(1000);
  forward(200);
  leds[0] = CRGB::Blue;
  leds[1] = CRGB::Blue;
  FastLED.show();
  delay(500);
  halt();
  delay(500);
  forward(200);
  delay(500);
  halt();
  delay(500);
  forward(200);
  delay(500);
  halt();
  delay(500);

  //reset movement
  ccw(200);
  delay(500);
  halt();
  delay(1000);
  cw(200);
  delay(500);
  halt();
  delay(3000);

  
  // bridge
  
  forward(200);
  leds[0] = CRGB::Green;
  leds[1] = CRGB::Green;
  FastLED.show();
  delay(500);
  halt();
  delay(4500);

  reverse(200);
  delay(500);
  halt();
  delay(4500);

  //freestyling
  ccw(200);
  delay(500);
  halt();
  delay(500);
  cw(200);
  delay(500);
  halt();
  delay(500);
  cw(200);
  delay(500);
  halt();
  delay(500);
  ccw(200);
  delay(500);
  halt();
  delay(500);

  forward(200);
  leds[0] = CRGB::White;
  leds[1] = CRGB::White;
  FastLED.show();
  delay(500);

  reverse(200);
  delay(500);

  ccw(200);
  delay(2000);

  cw(200);
  delay(2000);

  forward(200);
  leds[0] = CRGB::White;
  leds[1] = CRGB::White;
  FastLED.show();
  delay(500);

  halt();
  delay(500);


  //chorus repeat

  forward(200);
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(1000);
  halt();
  delay(5000);

  reverse(200);
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(1000);
  halt();
  delay(5000);

  ccw(200);
  delay(300);
  halt();
  delay(3000);
  cw(200);
  delay(300);
  halt();
  delay(1000);

  forward(200);
  delay(500);
  halt();
  delay(3500);

  //side step, switch directions and repeat
  
  ccw(200);
  delay(500);
  halt();
  delay(1000);
  forward(200);
  leds[0] = CRGB::Blue;
  leds[1] = CRGB::Blue;
  FastLED.show();
  delay(500);
  halt();
  delay(500);
  forward(200);
  delay(500);
  halt();
  delay(500);
  forward(200);
  delay(500);
  halt();
  delay(2000);
  
  cw(200);
  delay(500);
  halt();
  delay(1000);
  forward(200);
  leds[0] = CRGB::Blue;
  leds[1] = CRGB::Blue;
  FastLED.show();
  delay(500);
  halt();
  delay(500);
  forward(200);
  delay(500);
  halt();
  delay(500);
  forward(200);
  delay(500);
  halt();
  delay(500);

  //reset movement
  ccw(200);
  delay(500);
  halt();
  delay(1000);
  cw(200);
  delay(500);
  halt();
  delay(3000);

  // final pose and hold
  cw(200);
  delay(400);
  halt();
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  delay(10000);
}