/*
Oliver Grenon, Hunter Frohnert
06/06/2024
Dual motor esp32 robot controlled via PlayStation 4 controller
*/

#include <PS4Controller.h> // Header file can be found in src folder - https://github.com/pablomarquez76/PS4_Controller_Host/blob/main/src/PS4Controller.h
#include <Wire.h> //Used to communicate with vl530x sensor 
#include <VL53L0X.h> // Distance sensor - I^2C - Inter-Integrated Circuit - Serial communication bus 

// Motor pin definitions
#define M1A 17 // Left motor forward
#define M1B 16 // Left motor backward
#define M2A 19 // Right motor forward
#define M2B 18 // Right motor backward

// LED Pin definitions
#define LED_GREEN 12 //for status 
#define LED_RED 13 //for distance readings 
#define LED_BLUE 27 //for bluetooth status

// VL53L0X pin definitions
#define VL53L0X_SDA 21
#define VL53L0X_SCL 22

// Slowest speed - used Don's value for safety 
#define SLOWEST_SPEED 190

// Timestamp 
unsigned long lastTimeStamp = 0;

// creates object
VL53L0X sensor;

/* Function to set motor speeds:
If the speed is positive, then we'll write motorPinA to its speed and we'll write motorPinB to 0
Otherwise if the speed is negative, we'll write motorPinA to 0 and motorPinB to its speed 
If neither, (its gotta be zero or some weird number) we'll write both pins to 0 
*/
void setMotorSpeed(int motorPinA, int motorPinB, int speed) {
  if (speed > 0) {
    analogWrite(motorPinA, speed); 
    analogWrite(motorPinB, 0);
  } else if (speed < 0) {
    analogWrite(motorPinA, 0);
    analogWrite(motorPinB, -speed); 
  } else {
    analogWrite(motorPinA, 0); 
    analogWrite(motorPinB, 0); 
  }
}

/*
Function to control motors: 
Uses only the Y value coordinates from both the left and right stick. These will control their corresponding motors. 
The ints that are accepted by this function are elements of [-127,127]
Since this PS4 controller is quite old, we incorporated if..else statements to ignore any Y values from [-10,10]
if LStickY is greater than -10 AND less than 10, leftMotorSpeed = 0 
Otherwise, we use map(stick_input, lower_input_bound, upper_input_bound, pwm_lower_bound, pwm_upper_bound); to set leftMotorSpeed
Then - if leftMotorSpeed is positive (forward), we use constrain(x, a, b); to set leftMotorSpeed 
Otherwise, leftMotorSpeed must be negative so we constrain it to the negated pwm bounds
The same proccess runs for rStickY
The function ends by passing the motor pins as well as its respective speed to setMotorSpeed(int motorPinA, int motorPinB, int speed)
*/
void controlMotors(int lStickY, int rStickY) {
  int leftMotorSpeed;
  int rightMotorSpeed;

  // Check for dead zone for left stick
  if (lStickY > -10 && lStickY < 10) {
    leftMotorSpeed = 0;
  } else {
    leftMotorSpeed = map(lStickY, -127, 127, -255, 255);
    if (leftMotorSpeed > 0) {
      leftMotorSpeed = constrain(leftMotorSpeed, SLOWEST_SPEED, 255);
    } else {
      leftMotorSpeed = constrain(leftMotorSpeed, -255, -SLOWEST_SPEED);
    }
  }

  // Check for dead zone for right stick
  if (rStickY > -10 && rStickY < 10) {
    rightMotorSpeed = 0;
  } else {
    rightMotorSpeed = map(rStickY, -127, 127, -255, 255);
    if (rightMotorSpeed > 0) {
      rightMotorSpeed = constrain(rightMotorSpeed, SLOWEST_SPEED, 255);
    } else {
      rightMotorSpeed = constrain(rightMotorSpeed, -255, -SLOWEST_SPEED);
    }
  }

  setMotorSpeed(M2A, M2B, leftMotorSpeed);  // Left motor controlled by lStickY
  setMotorSpeed(M1A, M1B, rightMotorSpeed); // Right motor controlled by rStickY
}

/* Function to read the joystick values and control motors:
We set lStickY to it's value read from PS4Controller.h by calling the librarie's PS4.LStickY(); function
Repreat for rStickY then, 
if millis() (the first clock ever invented in this simulation) minus theLastTime stamp recorded, 
then we send the lStickY and rStickY values to the controlMotors() function and set lastTimeStamp to the current millis() value
*/
void notify() {
  int lStickY = PS4.LStickY();
  int rStickY = PS4.RStickY();

  if (millis() - lastTimeStamp > 50) {
    controlMotors(lStickY, rStickY);
    lastTimeStamp = millis();
  }
}

// After PS4 Controller has connected, writes LED_BLUE to high 
void onConnect() {
  Serial.println("Connected!.");
  digitalWrite(LED_BLUE, HIGH); // Turn on green LED when connected
}

void setup() {
  Serial.begin(115200);

  
  pinMode(M2A, OUTPUT); //Left motor +
  pinMode(M2B, OUTPUT); //Left motor -
  pinMode(M1A, OUTPUT); // Right motor + 
  pinMode(M1B, OUTPUT); // Right motor - 

 // Sets respective pins to output
  pinMode(LED_GREEN, OUTPUT); // Status
  pinMode(LED_RED, OUTPUT); // Distance sensor
  pinMode(LED_BLUE, OUTPUT); // Bluetooth status

  
  Wire.begin(VL53L0X_SDA, VL53L0X_SCL); // Serial communication
  sensor.init(); //Initialize sensor
  sensor.setTimeout(500); //Maximum wait time to wait for data
  sensor.startContinuous(); // Back to back reading

 
  PS4.attach(notify); // Calls PS4 headers attach() function and sends to notify()
  PS4.attachOnConnect(onConnect); // Calls PS4 headers attachOnConnect() function and sends to onConnect() 
  PS4.begin(); // Boolean return 


  // Program has initialized and is ready to connect to a controller
  Serial.println("Ready to connect.");
  digitalWrite(LED_GREEN, HIGH);
}

/* Function to handle distance sensor readings:
Very close -> Red light + setRumble(high, high) called from PS4 header 
Close -> Red light + setRumble(medium, medium) called from PS4 header 
Neither -> Red light off + setRumble(low, low) called from PS4 header 
*/

void handleDistanceAlerts(int distance) {
  if (distance < 80) { // Very close
    digitalWrite(LED_RED, HIGH);  
    PS4.setRumble(255, 255); // High vibration
    PS4.sendToController(); // Sends request
    Serial.println("Rumble: High (255, 255)");
  } 
  else if (distance < 200) { // Getting close
    digitalWrite(LED_RED, HIGH);  
    PS4.setRumble(128, 128); // Medium vibration
    PS4.sendToController(); // Sends request
  } 
  else { 
    digitalWrite(LED_RED, LOW);  // No red LED
    PS4.setRumble(0, 0); // No vibration
    PS4.sendToController(); // Sends request
  }
}

/* doesn't need to do a whole lot...
notify(); handles motion controls 
loop(); uses int distance from the vl503x library's readRangeContinuousMillimeters function
the if...else statement handles controller vibration and lighting control in the handleDistanceAlerts(); function. 
if the sencor is communicating, then we call handleDistanceAlerts(); to turn lights on or off as well as vibrate the controller
*refer to handleDistanceAlerts(); for info regarding its functionality*
*/
void loop() {
  int distance = sensor.readRangeContinuousMillimeters();

  if (!sensor.timeoutOccurred()) {
    handleDistanceAlerts(distance);
  } else {
    Serial.println("Sensor timeout!");
  }
}