//Jonathan Ruiz, Hadrian Fox 
//ENGR 103 Final Assignment 
//Emotion Challenge

#include <FastLED.h>

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

#define SLOW_SPEED 200 // Defined slow speed

CRGB leds[NUM_LEDS];

//Function to move robot backwards 
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 robot foward 
void forward(uint8_t mySpeed) {
  Serial.print("Forward speed set to: ");
  Serial.println(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 make robot spin in a circle 
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 make robot stop
void stop() {
  Serial.println("Stopping motors");
  ledcWrite(3, 0); // Set M2B to LOW
  ledcWrite(2, 0); // Set M2A to LOW
  ledcWrite(1, 0); // Set M1B to LOW
  ledcWrite(0, 0); // Set M1A to LOW
}

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

  // Set the 4 motor control lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  // These lines connect 4 Pulse Width Modulation 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() {
  // Move forward fast with red LED
  leds[0] = CRGB::Red;
  leds[1] = CRGB::Red;
  FastLED.show();
  forward(255);
  delay(6000);
  stop();
  delay(3000);

  // Move backward fast with blue LED
  leds[0] = CRGB::Blue;
  leds[1] = CRGB::Blue;
  FastLED.show();
  reverse(255);
  delay(3000);
  stop();
  delay(4000);

  // Move forward slowly with purple LED
  leds[0] = CRGB::Purple;
  leds[1] = CRGB::Purple;
  FastLED.show();
  forward(SLOW_SPEED); // This uses the defined slow speed set to 200
  delay(7000);
  stop();
  delay(4000);

  // Spin in a circle fast with green LED
  leds[0] = CRGB::Green;
  leds[1] = CRGB::Green;
  FastLED.show();
  cw(255);
  delay(7000);
  stop();
  delay(4000);

  // Delay before restarting the loop
  delay(10000);
}