/*
/2026/06/20244
Ryan Allais 
Anijah Rivas
This code will make the robot move in a swaying motion like a shark by powering one wheel more then the other.

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;
bool swayingRight = true;

// Function to indicate error with the LED
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);
}

// Function to move the robot in reverse
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
}

// Function to move the robot forward
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
}

// Function to rotate the robot clockwise
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
}

// Function to rotate the robot counter-clockwise
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 M1B to mySpeed
}

// Function to sway the robot forward, simulating a hesitant, sad movement
void swayForward(uint8_t mySpeed, bool swayRight) {
  if (swayRight) {
    // Sway to the right
    ledcWrite(2, mySpeed); // Set M2A to mySpeed
    ledcWrite(3, 0); // Set M2B to LOW
    ledcWrite(0, mySpeed * 0.5); // Set M1A to 50% of mySpeed
    ledcWrite(1, 0); // Set M1B to LOW
  } else {
    // Sway to the left
    ledcWrite(2, mySpeed * 0.5); // Set M2A to 50% of mySpeed
    ledcWrite(3, 0); // Set M2B to LOW
    ledcWrite(0, mySpeed); // Set M1A to mySpeed
    ledcWrite(1, 0); // Set M1B to LOW
  }
}

void setup() {
  Serial.begin(115200); // Initialize serial communication at 115200 baud
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS); // Initialize the WS2812 LED

  // Initialize I2C communication and the VL53L0X sensor
  Wire.begin();
  sensor.setTimeout(500);
  if (!sensor.init()) {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) {
      errorLed(); // Indicate error if sensor initialization fails
    }
  }

  // Set the 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, 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);

  swayForward(230, swayingRight); // Start with swaying movement
}

void loop() {
  int distance = sensor.readRangeSingleMillimeters(); // Read distance from VL53L0X sensor
  if (distance < 50) { // If an object is very close, avoid it
    reverse(255); // Move backward
    delay(200); // Pause for a moment
    cw(255); // Rotate clockwise
    delay(150); // Pause for a moment
  } else if (distance < 200) { // If an object is getting closer, slow down
    forward(230 - (30 - ((distance / 200.0) * 30))); // Slow down as it approaches the object
  } else {
    // Sway forward to simulate sadness
    swayForward(150, swayingRight); // Slow swaying movement
    swayingRight = !swayingRight; // Alternate sway direction
  }

  Serial.println(distance); // Print the distance to the serial monitor
  delay(1000); // Longer delay to make movements appear slower and more hesitant
}