/*
Caden Humphreys
Kurtis Man
Final Project
6/6/2024
*/

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


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

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

uint32_t lastTime = 0;

//Sets up LED error code if there is a problem with the robot
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 happy(uint8_t mySpeed){
  //Run clockwise for 1 second
  ledcWrite(3, mySpeed); //Set M2B to mySpeed set in loop
  ledcWrite(2, 0); //Set M2A to LOW
  ledcWrite(1, 0); //Set M1B to LOW
  ledcWrite(0, mySpeed); //Set M1A to mySpeed
  delay(1000);
  //Run ccw for 1 second
  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
  delay(1000);
 
}

void scared(uint8_t mySpeed){
  //Repeatedly goes cw and ccw to appear a shaking motion, set at 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
  delay(150);
  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
  delay(150);
   ledcWrite(3, mySpeed);//Will repeat the shaking appearance 4 more times for each cw and ccw motion
  ledcWrite(2, 0); 
  ledcWrite(1, 0); 
  ledcWrite(0, mySpeed); 
  delay(150);
  ledcWrite(2, mySpeed);
  ledcWrite(3, 0); 
  ledcWrite(0, 0);
  ledcWrite(1, mySpeed);
  delay(150);
   ledcWrite(3, mySpeed); 
  ledcWrite(2, 0); 
  ledcWrite(1, 0); 
  ledcWrite(0, mySpeed);
  delay(150);
  ledcWrite(2, mySpeed);
  ledcWrite(3, 0);
  ledcWrite(0, 0); 
  ledcWrite(1, mySpeed);
  delay(150);
   ledcWrite(3, mySpeed); 
  ledcWrite(2, 0); 
  ledcWrite(1, 0); 
  ledcWrite(0, mySpeed); 
  delay(150);
  ledcWrite(2, mySpeed); 
  ledcWrite(3, 0); 
  ledcWrite(0, 0); 
  ledcWrite(1, mySpeed); 
  delay(150);
   ledcWrite(3, mySpeed); 
  ledcWrite(2, 0);
  ledcWrite(1, 0);
  ledcWrite(0, mySpeed);
  delay(150);
  ledcWrite(2, mySpeed);
  ledcWrite(3, 0);
  ledcWrite(0, 0);
  ledcWrite(1, mySpeed);
  delay(150);
}

void motor(uint8_t left, uint8_t right){ //Sets speed for left and right motors based on value "left" and "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, 190, 255);//128 is stoped, 255 is maximum speed, 190 is minimum
//    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, 190, 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, 190, 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, 190, 255);
    //right = (128 - right) + 200;
    ledcWrite(1, right); //Set M2B to pwm
    ledcWrite(0, 0); //Set M2A 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 setup() {
  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();}
  }
  
  // Set the 4 motor controll 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);
  
  forward(230);
}

void loop() {
  int distance = sensor.readRangeSingleMillimeters();//Assigns the distance from sensor in mm to "distance"
  if (distance > 600 && distance < 800){ //Happy between 600 and 800 mm
    happy(255);//Calls happy
    forward(255);//Calls forward after happy completes
    delay(700);//Allows robot to move into next distance range
  }
  else if(distance > 400 && distance < 600){//Angry between 400 and 600 mm. Robot appears to "load up" by slowly going backwards and suddenly joilting forwards
    for (int i = 50; i < 100; i++){//Starts off at a negative speed(backwords) and decreases speed until speed it is at 100
      motor(i,i);//Calls motor using i for left and right values
      delay(30);//30 millisendonds for every i value
    }
    forward(255);//Calls forward at max speed
    delay(1750);//Allows for robot to move into next distance range
 
  }else if(distance > 200 && distance < 400){//Scared between 200 and 400 mm
    scared(255);//Calls scared at max speed, appearing to shake
    forward(230);//Moves forward at a slower speed, taking precaution
    delay(1000);//Allows robot to move into next distance range
  }else if(distance < 200){//Sad under 200 mm. Robot stops, slowly turns away and slowly drives away
    int i = 128;//Sets speed to zero
    motor(i,i);//Robot is stopped for 800 ms
    delay(800);
    for (int i = 75; i < 128; i++){//Sets speed to 75(backwards) and slowly decreases speed
      motor(i,i); 
      delay(50);//50 ms for each i value for speed
    }
    cw(200);//Turns ccw at a slow speed, appearing sad
    delay(1200);//Allows robot to turn about 180 degrees
    forward(210);//Robot moves forward slowly, appearing sad
    delay(1500);
  }
  else {
    forward(255);//If no distance is sensed under 800 mm, it will move forward
  
  }
  Serial.println(distance);//Prints the distance in mm to the serial port
}