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

VL53L0X sensorForward;

#define NUM_LEDS 2
CRGB leds[NUM_LEDS];

#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18
#define WSLED 4

void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);  // GRB ordering is assumed
  FastLED.setBrightness(64); // Set LED brightness to 25%

  // This section creates the I2C (Wire) connection and queries for a VL53LX
  Wire.begin();
  sensorForward.setTimeout(500);
  if (!sensorForward.init()) {
    Serial.println("Failed to detect and initialize sensor!");
  
  }

  // 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() {
  showSadEmotion();
}

void forward(uint8_t leftSpeed, uint8_t rightSpeed){
  ledcWrite(2, leftSpeed); // Set M2A to leftSpeed
  ledcWrite(3, 0); // Set M2B to LOW
  ledcWrite(0, rightSpeed); // Set M1A to rightSpeed
  ledcWrite(1, 0); // Set M1B to LOW
}

void reverse(uint8_t leftSpeed, uint8_t rightSpeed){
  ledcWrite(3, leftSpeed); // Set M2B to leftSpeed
  ledcWrite(2, 0); // Set M2A to LOW
  ledcWrite(1, rightSpeed); // Set M1B to rightSpeed
  ledcWrite(0, 0); // Set M1A to LOW
}

void stopMotors(){
  ledcWrite(0, 0);
  ledcWrite(1, 0);
  ledcWrite(2, 0);
  ledcWrite(3, 0);
}

void showSadEmotion() {
  // Drive in an S-curve while animating LEDs
  driveInSCurveWithLEDs();
}

void animateSadLEDs() {
  for (int i = 0; i < 5; i++) {
    leds[0] = CRGB::Blue;
    leds[1] = CRGB::Blue;
    FastLED.show();
    delay(500);
    leds[0] = CRGB::DarkBlue;
    leds[1] = CRGB::DarkBlue;
    FastLED.show();
    delay(500);
  }
}

void driveInSCurveWithLEDs() {
  unsigned long previousMillis = 0;
  const long interval = 1000; // Interval for S-curve movement
  bool turnRight = true;

  while (true) {
    int distanceForward = sensorForward.readRangeSingleMillimeters();

    if (distanceForward < 50) {
      // Bonk into the wall repeatedly
      bonkIntoWall();
      break;
    } else {
      unsigned long currentMillis = millis();

      if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;
        
        // Alternate turning direction for S-curve
        if (turnRight) {
          forward(150, 255); // Reduce left wheel speed, full speed right wheel
        } else {
          forward(255, 150); // Full speed left wheel, reduce right wheel speed
        }
        turnRight = !turnRight;
      } else {
        forward(200, 200); // Drive straight between turns
      }
      
      // Animate LEDs simultaneously
      animateSadLEDs();
    }
  }
}

void bonkIntoWall() {
  for (int i = 0; i < 5; i++) {
    forward(200, 200);
    delay(500); // Move forward for 0.5 seconds
    reverse(200, 200);
    delay(500); // Move backward for 0.5 seconds
  }
}