//ENGR 103
//Final Project: Robot
//Aubry Mann and Ethan Russo

//definitions for motors
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18
#define WSLED 4

#define SLOWEST_SPEED 210

//FOR SENSOR

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


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

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
}



//FOR MOTOR

//This function accepts values for the left and right motor.
//Values of 128 will stop the motor.
//Increasing values over 128 will make the motor go faster and faster forward
//Decreasing values below 128 will make the motor go faster and faster in reverse

void motor(uint8_t left, uint8_t right){
  if (left == 128){ //Set left to stop
    ledcWrite(3, 0); //Set M2B to LOW
    ledcWrite(2, 0); //Set M2A to LOW
  } else if (left > 128){ //Set left to forward
    left = map(left, 128, 255, SLOWEST_SPEED, 255);
//    left = (left - 128) + 200;
    ledcWrite(3, 0); //Set M2B to LOW
    ledcWrite(2, left); //Set M2A to pwm
  } else { //else set left to reverse
    left = map(left, 128, 0, SLOWEST_SPEED, 255);
//    left = (128 - left) + 200;
    ledcWrite(3, left); //Set M2B to pwm
    ledcWrite(2, 0); //Set M2A to LOW
  }

  if (right == 128){ //Set right to stop
    ledcWrite(1, 0); //Set M2B to LOW
    ledcWrite(0, 0); //Set M2A to LOW
  } else if (right > 128){ //Set right to forward
    right = map(right, 128, 255, SLOWEST_SPEED, 255);
    //right = (right - 128) + 200;
    ledcWrite(1, 0); //Set M2B to LOW
    ledcWrite(0, right); //Set M2A to pwm
  } else { //else set right to reverse
    right = map(right, 128, 0, SLOWEST_SPEED, 255);
    //right = (128 - right) + 200;
    ledcWrite(1, right); //Set M2B to pwm
    ledcWrite(0, 0); //Set M2A to LOW
  }
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);  // GRB ordering is assumed

  //set up lights
  pinMode(32, OUTPUT);
  pinMode(33, OUTPUT);
  pinMode(25, OUTPUT);

  // Set the 4 motor controll lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

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

void lightsOut(){
  digitalWrite(33, LOW);
  digitalWrite(32, LOW);
  digitalWrite(25,LOW);
}

void beHappy(){
  //green light on
  digitalWrite(33, HIGH);
  digitalWrite(32, LOW);
  digitalWrite(25,LOW);
  
  // shimmies
  motor(128,200);
  delay(200);
  motor(200,128);
  delay(200);
  motor(128,200);
  delay(200);
  motor(200,128);
  delay(200);
  motor(128,200);
  delay(200);
  motor(200,128);
  delay(200);
  motor(128,200);
  delay(200);
  motor(200,128);
  delay(200);
  
  //spinning
  motor(128,230);
  delay(3000);
}

void beSad(){
  //blue light on
  digitalWrite(33, LOW);
  digitalWrite(32, HIGH);
  digitalWrite(25,LOW);

  motor(115, 128);
  delay(1000);
  motor(130, 130);
  delay(800);
  motor(128, 128);
  delay(150);
  motor(140, 140);
  delay(600);
  motor(128, 128);
  delay(150);
  motor(129, 129);
  delay(2000);
  
}

void beScared(){
  //blue and red light on (making purple)
  digitalWrite(33, LOW);
  digitalWrite(32, HIGH);
  digitalWrite(25,HIGH);

  motor(200, 200);
  delay(1300);
  
  for(int i = 200; i > 128; i--){
    motor(i, i);
    delay(15);
  }
  motor(128, 128);
  delay(300);

  motor(110, 110);
  delay(200);
  motor(128, 128);
  delay(800);

  for(int i = 128; i > 30; i--){
    motor(i, i);
    delay(10);
  }

  motor(30, 30);
  delay(2000);
  
}

void beAngry(){
  
  //red light on
  digitalWrite(33, LOW);
  digitalWrite(32, LOW);
  digitalWrite(25,HIGH);

  motor(50, 128);
  delay(300);
  motor(128, 50);
  delay(300);
  motor(50, 128);
  delay(200);
  motor(128, 128);
  delay(1000);

  
  motor(150,150);
  delay(400);
  motor(128, 128);
  delay(300);
  motor(250, 250);
  delay(1500);

  //punches
  motor(128, 128);
  delay(150);
  motor(180,180);
  delay(150);
  motor(128, 128);
  delay(150);
  motor(180,180);
  delay(150);
  motor(128, 128);
  delay(150);
  motor(180,180);
  delay(150);
  
}

void loop() {
  // put your main code here, to run repeatedly:

 beHappy();

 lightsOut();
 motor(128, 128);
 delay(3000);

 beSad();

 lightsOut();
 motor(128, 128);
 delay(3000);
 
 beScared();

 lightsOut();
 motor(128, 128);
 delay(3000);

 beAngry();

 lightsOut();
 motor(128, 128);
 delay(3000);
 
}