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

VL53L0X sensor;

#define NUM_LEDS 2
CRGB leds[NUM_LEDS];

#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18
#define WSLED 4
#define I025 25  // Changed from 35 to 25
#define I032 32
#define I033 33

// New pins for the purple light
#define PURPLE_RED_PIN 26
#define PURPLE_BLUE_PIN 27

unsigned long previousMillis1 = 0;
unsigned long previousMillis2 = 0;
unsigned long previousMillis3 = 0;
unsigned long previousMillis4 = 0;
const long interval1 = 200; // Interval at which to blink (milliseconds)
const long interval2 = 300; // Interval at which to blink (milliseconds)
const long interval3 = 400; // Interval at which to blink (milliseconds)
const long interval4 = 250; // Interval at which to blink purple light (milliseconds)
bool ledState1 = LOW;
bool ledState2 = LOW;
bool ledState3 = LOW;
bool ledState4 = LOW;

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);
}

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
}

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
}

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
}

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
}

void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);  // GRB ordering is assumed

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

  // Set the 4 motor control lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);
  pinMode(WSLED, OUTPUT);
  pinMode(I025, OUTPUT);  // Changed from 35 to 25
  pinMode(I032, OUTPUT);
  pinMode(I033, OUTPUT);

  // Set the new purple light pins as outputs
  pinMode(PURPLE_RED_PIN, OUTPUT);
  pinMode(PURPLE_BLUE_PIN, 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);

  // Attach PWM channels to the purple light pins
  ledcSetup(4, 5000, 8); // PWM for purple red pin
  ledcAttachPin(PURPLE_RED_PIN, 4);
  ledcSetup(5, 5000, 8); // PWM for purple blue pin
  ledcAttachPin(PURPLE_BLUE_PIN, 5);

  forward(230);
}

void loop() {
  int distance = sensor.readRangeSingleMillimeters();

  if (distance < 50) { // Real close! Avoid
    reverse(255);
    blinkWhileReversing(500);
    cw(255);
    blinkWhileReversing(5000);
  } else if (distance < 200) { // Getting close, slow down
    forward(230 - (30 - ((distance / 200.0) * 30)));
  } else {
    forward(230);
  }

  Serial.println(distance);
  digitalWrite(WSLED, HIGH);
}

// Function to handle blinking LEDs while reversing or turning
void blinkWhileReversing(unsigned long duration) {
  unsigned long startMillis = millis();
  unsigned long currentMillis;

  while (millis() - startMillis < duration) {
    currentMillis = millis();

    // Blinking logic for LED on I025
    if (currentMillis - previousMillis1 >= interval1) {
      previousMillis1 = currentMillis;
      ledState1 = !ledState1;
      digitalWrite(I025, ledState1);
    }

    // Blinking logic for LED on I032
    if (currentMillis - previousMillis2 >= interval2) {
      previousMillis2 = currentMillis;
      ledState2 = !ledState2;
      digitalWrite(I032, ledState2);
    }

    // Blinking logic for LED on I033
    if (currentMillis - previousMillis3 >= interval3) {
      previousMillis3 = currentMillis;
      ledState3 = !ledState3;
      digitalWrite(I033, ledState3);
    }

    // Blinking logic for the purple light
    if (currentMillis - previousMillis4 >= interval4) {
      previousMillis4 = currentMillis;
      ledState4 = !ledState4;
      if (ledState4) {
        ledcWrite(4, 255); // Turn on red LED at full brightness
        ledcWrite(5, 255); // Turn on blue LED at full brightness
      } else {
        ledcWrite(4, 0); // Turn off red LED
        ledcWrite(5, 0); // Turn off blue LED
      }
    }
  }

  // Turn off all LEDs after the action
  digitalWrite(I025, LOW);
  digitalWrite(I032, LOW);
  digitalWrite(I033, LOW);
  ledcWrite(4, 0); // Turn off red LED
  ledcWrite(5, 0); // Turn off blue LED
}