/*
Final Assignment: Sad emotion
  Program controls motors and LED of robot to indicate the robot is sad.
  Its sad 'behavior' is slowly moving forward and backwards. 
  The LED should light up blue and slowly flash, potentially like a dimming light.

Team members: Maddie Lyle and CJ Chavez

Last update: 6/1/2024
 */

// Libraries
#include <Wire.h>
#include <FastLED.h>

// Declarations
//*The Motors
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18

//*The LED
#define NUM_LEDS 1
CRGB leds[NUM_LEDS];
#define WSLED 4

//*For controlling the LED with millis
unsigned long currentMillis = 0;
unsigned long previousMillis = 0;
unsigned long period = 500;
unsigned long ledPreviousMillis = 0;
unsigned long ledPeriod = 400;
int LEDbrightness = 0; // Variable to control LED brightness

// Function which controls motors and allows robot to move forward based on conditional speed
void forward(uint8_t mySpeed){
  analogWrite(M2B, mySpeed);
  analogWrite(M1B, mySpeed);
  digitalWrite(M2A, LOW);
  digitalWrite(M1A, LOW);
}
// Function which controls motors and allows robot to move forward based on conditional speed
void backwards(uint8_t mySpeed){
  analogWrite(M2A, mySpeed);
  analogWrite(M1A, mySpeed);
  digitalWrite(M2B, LOW);
  digitalWrite(M1B, LOW);
}

// Function which controls LED based on millis
//* LED should pulse on and off gradually
void blueLED(){
  leds[0] = CRGB(0, 0, LEDbrightness); // Blue with variable brightness
  FastLED.show();
  LEDbrightness += 10; // Increment brightness gradually
  if (LEDbrightness > 255) LEDbrightness = 0; // Reset brightness if maximum is reached
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, WSLED>(leds, NUM_LEDS);

  // Set the 4 motor control lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);
  
  forward(200); // Initial movement speed
}

void loop() {
  // put your main code here, to run repeatedly:
  currentMillis = millis();

  if (currentMillis - ledPreviousMillis >= ledPeriod) {
        blueLED(); // Activate blue LED with gradual brightness
        ledPreviousMillis = currentMillis;
      }
  //Move robot forward
  forward(190);
  delay(250);
  forward(0);
  delay(500);

  //Move robot backwards
  backwards(190);
  delay(250);
  backwards(0);
  delay(500); 
}