#include <Wire.h>
#include <VL53L0X.h>// includes the library used specifically
VL53L0X sensor;// sesnor for the library so the robot can sense when it gets within range

#include <FastLED.h>// includes gthe library
#define NUM_LEDS 2
CRGB leds[NUM_LEDS];

#define M2A 17// defines where the motors are and assigns numbers to the motors
#define M2B 16
#define M1A 19
#define M1B 18
#define WSLED 4

uint32_t lastTime = 0;

void errorLed(int count = 1){ //if the Led encounters an error this code plays on the serial monitor
  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 M1B to mySpeed
}

void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);  // GRB ordering is assumed
  
  // This section creates the I2C (Wire) connection and queries for a VL53L0X
  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);//setting up which pins are being used to reference within the code
  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() {
  //Happy
// circles, dancing
  forward(230);//moves forward
  delay(1000);
  
  cw(255);// Makes a circle
  delay(2000);

  //Scared
 //reverse as in a form of fear, but then realization of its ok so moves forward
  reverse(230);//Backward movement
  delay(2000);//pause for 2 seconds
  
  forward(0);// moves forward
  delay(1000);// delay for 1 second

  //Angry
  // zigzag movement, or a less controlled movement than others to show anger
  for (int i = 0; i < 3; i++) {//causes the robot to move forward in a zig zag shape
                               //Due to the circular motion with short delay
    cw(230);// creates a movement within the zig zag,
    delay(500);//pause for half a second
    ccw(230);// this creates on of the movements within the zig zag
    delay(500);
    //robot starts making a circle but then the delay function cuts it off to start the next circle
  }
  //Sad
  //pacing
  forward(230);// forward
  delay(1000);
  
  
  reverse(230);//backward
  delay(1000);
 
  forward(0);// repeats the loop of the code after a delay of a second
  delay(1000);// 
}