#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(255); // Set LED brightness to full

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

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 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 stopMotors(){
  ledcWrite(0, 0);
  ledcWrite(1, 0);
  ledcWrite(2, 0);
  ledcWrite(3, 0);
}

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 M1BA to mySpeed
}

void showAnger() {
  // LED Animation
  animateAngryLEDs();

  // Pacing Movements
  pacingMovements();

  // Check for wall and slam into it
  slamIntoWall();
}

void animateAngryLEDs() {
  for (int i = 0; i < 10; i++) {
    leds[0] = CRGB::Red;
    leds[1] = CRGB::Red;
    FastLED.show();
    delay(100); // Rapid blinking
    leds[0] = CRGB::Black;
    leds[1] = CRGB::Black;
    FastLED.show();
    delay(100);
  }
}

void pacingMovements() {
  for (int i = 0; i < 4; i++) {
    forward(200);  // Move forward
    delay(1000);   // Move for 1 second
    ccw(255);      // Turn 180 degrees
    delay(1000);   // Turn for 1 second
    forward(200);  // Move forward
    delay(1000);   // Move for 1 second
    ccw(255);      // Turn 180 degrees
    delay(1000);   // Turn for 1 second
  }

  // Stop halfway through a pace, turn 90 degrees, and move forward
  forward(200);
  delay(500);     // Move for 0.5 second
  cw(255);        // Turn 90 degrees
  delay(500);     // Turn for 0.5 second
  forward(200);   // Move forward
  delay(1000);    // Move for 1 second
}

void slamIntoWall() {
  int distanceForward = sensorForward.readRangeSingleMillimeters();
  
  if (distanceForward < 100) {
    // Wall detected, slam into it
    forward(255);  // Full speed ahead
    delay(500);    // Slam into the wall for 0.5 seconds
    reverse(255);  // Move backward quickly
    delay(500);    // Reverse for 0.5 seconds
  }
}