/*
 6/7/24
 Reece Campbell
 Final Projet Robot Code
 */

#include <Wire.h>
#include <VL53L0X.h>
#include <FastLED.h>

// Constants
#define NUM_LEDS 2
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18
#define WSLED 4
#define PWM_FREQ 30000
#define PWM_RES 8

// Variables
VL53L0X sensor;
CRGB leds[NUM_LEDS];
uint32_t lastTime = 0;

// Function Prototypes
void errorLed(int count = 1);
void stopMotors();
void reverse(uint8_t speed);
void forward(uint8_t speed);
void cw(uint8_t speed);
void ccw(uint8_t speed);
void waddle();
void motorControl(int M1A_speed, int M1B_speed, int M2A_speed, int M2B_speed);

// Starts the code
void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS); // GRB ordering is assumed
  
  // Starts I2C and VL53L0X sensor
  Wire.begin();
  sensor.setTimeout(500);
  if (!sensor.init()) {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) { errorLed(); }
  }
  
  // Set motor control pins as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  // Setup PWM channels for motor control
  ledcSetup(0, PWM_FREQ, PWM_RES);
  ledcAttachPin(M1A, 0);
  ledcSetup(1, PWM_FREQ, PWM_RES);
  ledcAttachPin(M1B, 1);
  ledcSetup(2, PWM_FREQ, PWM_RES);
  ledcAttachPin(M2A, 2);
  ledcSetup(3, PWM_FREQ, PWM_RES);
  ledcAttachPin(M2B, 3);
  
  forward(230);
}

void loop() {
  int distance = sensor.readRangeSingleMillimeters();
  if (distance < 50) { // too close to wall
    stopMotors();
    delay(5000); // Pause for 5 seconds
    reverse(255);
    delay(1000); // Reverse for 1 second
    cw(255);
    delay(200); // Turn right for 150 ms
    stopMotors();
  } else {
    waddle();
  }
  Serial.println(distance);
}

// Function Implementations
void errorLed(int count) {
  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);
}

// turns off the motors
void stopMotors() {
  motorControl(0, 0, 0, 0);
}

// reverses the robot
void reverse(uint8_t speed) {
  motorControl(0, speed, 0, speed);
}

//drives the robot forward
void forward(uint8_t speed) {
  motorControl(speed, 0, speed, 0);
}

// turns the robot clock wise
void cw(uint8_t speed) {
  motorControl(speed, 0, 0, speed);
}

// turns the robot counter clock wise
void ccw(uint8_t speed) {
  motorControl(0, speed, speed, 0);
}

void waddle() {
  // Left motor forward for 100 milliseconds
  motorControl(0, 0, 230, 0);
  delay(200);
  stopMotors();

  // Right motor forward for 100 milliseconds
  motorControl(230, 0, 0, 0);
  delay(200);
  stopMotors();
}

// controls the motors speed
void motorControl(int M1A_speed, int M1B_speed, int M2A_speed, int M2B_speed) {
  ledcWrite(0, M1A_speed);
  ledcWrite(1, M1B_speed);
  ledcWrite(2, M2A_speed);
  ledcWrite(3, M2B_speed);
}