/*
6/6/2024
Isaac Hutchison
Robot project

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 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); //pin 19
  pinMode(M1B, OUTPUT); // pin 18
  pinMode(M2A, OUTPUT); //pin 17
  pinMode(M2B, OUTPUT); // pin 16

  pinMode(25, INPUT_PULLUP); //pin 25
  pinMode(32, INPUT_PULLUP); // pin 32
  pinMode(33, INPUT_PULLUP); // pin 33
  pinMode(34, INPUT_PULLUP); // pin 34

  //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 happy(){
  cw(255);
  delay(1000);
  ccw(255);
  delay(1000);
  forward(255);
  delay (1000);
  cw(255);
  delay(500);
  forward(255);
  delay(1000);
}

void scared(){
  cw(255);
  delay(50);
  ccw(255);
  delay(50);
  cw(255);
  delay(50);
  ccw(255);
  delay(50);
  cw(255);
  delay(50);
  ccw(255);
  delay(50);
  cw(255);
  delay(50);
  ccw(255);
  delay(50);
  cw(255);
  delay(50);
  ccw(255);
  delay(50);
  cw(255);
  delay(50);
  ccw(255);
  delay(50);
  cw(255);
  delay(50);
  ccw(255);
  delay(50);
  cw(255);
  delay(50);
  ccw(255);
  delay(50);
  cw(255);
  delay(50);
  ccw(255);
  delay(50);
  cw(255);
  delay(500);
  forward(255);
  delay (3000);
  
}

void bumpbot(){
  int distance = sensor.readRangeSingleMillimeters();
  if (distance < 200){ //Real close! Avoid
    reverse(255);
    delay(200);
    cw(255);
    delay(150);
  } else if (distance < 350){ // Getting close, slow down
    forward(230-(30-((distance/200.0)*30)));
  } else {
    forward(230);
  }
  Serial.println(distance);
  
}

void react(){
  int distance = sensor.readRangeSingleMillimeters();
  if (distance < 200){ //Real close! Avoid
    /*reverse(255);
    delay(200);
    cw(255);
    delay(150);
    */
    scared();
  } else if (distance < 350){ // Getting close, slow down
    forward(230-(30-((distance/200.0)*30)));
  } else {
    forward(230);
    
  }
  Serial.println(distance);
 }
void loop() {
  

  if(digitalRead(25) == LOW /*&& digitalRead(32) == HIGH && digitalRead(33) == HIGH && digitalRead(34) == HIGH*/){
    delay(3000);
    scared();
  }
  else if(/*digitalRead(25) == HIGH &&*/ digitalRead(32) == LOW /*&& digitalRead(33) == HIGH && digitalRead(34) == HIGH*/){
    delay(3000);
    happy();
  }
  else if(/*digitalRead(25) == HIGH && digitalRead(32) == HIGH &&*/ digitalRead(33) == LOW /*&& digitalRead(34) == HIGH*/){
    delay(3000);
    react();
  }
  else if(/*digitalRead(25) == HIGH && digitalRead(32) == HIGH && digitalRead(33) == HIGH &&*/ digitalRead(34) == LOW){
    delay(3000);
    bumpbot();
  } else {

  int distance = sensor.readRangeSingleMillimeters();
  if (distance < 200){ //Real close! Avoid
    reverse(255);
    delay(200);
    cw(255);
    delay(150);
  } else if (distance < 350){ // Getting close, slow down
    forward(230-(30-((distance/200.0)*30)));
  } else {
    forward(230);
  }
  Serial.println(distance);
  }
}