#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

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

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

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 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 showHappyEmotion() {
  // LED Animation
  animateLEDs();

  // Dance Sequence
  danceSequence();

  // Figure Eight Movement
  figureEight();

  // Tail Wag
  tailWag();

  // Stop the motors after the happy emotion display
  ledcWrite(0, 0);
  ledcWrite(1, 0);
  ledcWrite(2, 0);
  ledcWrite(3, 0);
}

void animateLEDs() {
  for (int i = 0; i < 5; i++) {
    leds[0] = CRGB::Yellow;
    leds[1] = CRGB::Yellow;
    FastLED.show();
    delay(200);
    leds[0] = CRGB::Green;
    leds[1] = CRGB::Green;
    FastLED.show();
    delay(200);
  }
}

void danceSequence() {
  for (int i = 0; i < 3; i++) {
    forward(200);
    delay(500);
    cw(200);
    delay(500);
    ccw(200);
    delay(500);
    reverse(200);
    delay(500);
  }
}

void figureEight() {
  for (int i = 0; i < 2; i++) {
    forward(200);
    delay(1000);
    cw(200);
    delay(1000);
    forward(200);
    delay(1000);
    ccw(200);
    delay(1000);
  }
}

void tailWag() {
  for (int i = 0; i < 5; i++) {
    cw(200);
    delay(200);
    ccw(200);
    delay(200);
  }
}