/*
6/7/2024
Hugh Torkelson & Felix Dawans

This is the code used to run our ROOMBA-BOT. It consists 
of a few functions for advanced motor control, and a loop function
that will run the motor control functions under
different circumstances.

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];

//Each motor (M1 and M2) has two pins connected.
//Setting one pin HIGH and the other LOW will make the motor turn one way
//Setting them in reverse will make it turn the opposite way
//Seting them both HIGH or both LOW stops the motor
#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);
}

#define SLOWEST_SPEED 190

//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() {
  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!"); //error detection for if the sensor is broken
    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() {
  int distance = sensor.readRangeSingleMillimeters(); //sets the integer distance equal to the reading from the sensor
  while (distance > 150) { //while the distance reading is over 150 millimeters
    distance = sensor.readRangeSingleMillimeters(); //sets the integer distance equal to the reading from the sensor
    Serial.println(distance); //prints distance to monitor for error checking
    motor(95,95); //has motor move robot forward at a set speed. (reduced speed to look like a roomba)
  }
  if (distance < 150) { //if the distance is under 150 millimeters
      motor(150,150); //set the motor to reverse
      delay(750); // for 0.75 seconds
      
      motor(0,150); //turns the robot clockwise
      delay(400); //for about 0.400 seconds, (around 90 degrees)
      
    }
  
}