/*
Final Assignment: Fear emotion
  Program makes robot flash LED and move around to indicate fear in the robot.
  The fearful 'behavior' is quickly changing actions approximately every 0.5 seconds.
  This makes it look like the robot is speeding around in a panic.
  The LED is simultaneously flashing rapidly in the color purple for this 'behavior'.

Team members: Maddie Lyle and CJ Chavez

Last update: 6/7/2024
*/

//initial motor setup
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18

#define SLOWEST_SPEED 190

//Library setup
#include <Wire.h>
#include <FastLED.h>

//lighting setup
#define NUM_LEDS 1
CRGB leds [NUM_LEDS];
#define WSLED 4

//setup for time counters that use millis
unsigned long initialTime = 0;
unsigned long firstTime = 0;

void backward(uint8_t left, uint8_t right){ //function that makes the robot move backward
  left = map(left, 128, 0, SLOWEST_SPEED, 255);
  right = map(right, 128, 0, SLOWEST_SPEED, 255);
  ledcWrite(3, left); //sets M2B to pwm
  ledcWrite(2, 0);  //sets M2A to LOW
  ledcWrite(1, right);  //sets M1B to pwm
  ledcWrite(0, 0);  //sets M1A to LOW
}

void forward(uint8_t left, uint8_t right){  //function that makes the robot move forward
  left = map(left, 128, 0, SLOWEST_SPEED, 255);
  right = map(right, 128, 0, SLOWEST_SPEED, 255);
  ledcWrite(3, 0);  //sets M2B to LOW
  ledcWrite(2, left); //sets M2A to pwm
  ledcWrite(1, 0);  //sets M1B to LOW
  ledcWrite(0, right);  //sets M1A to pwm
}

void rightTurn(uint8_t left, uint8_t right){  //function for spinning the robot in a clockwise direction
  right = map(right, 128, 0, SLOWEST_SPEED, 255); //these 3 lines set right wheel to reverse
  ledcWrite(1, right);  //sets M1B to pwm
  ledcWrite(0, 0);  //sets M1A to LOW
  left = map(left, 128, 0, SLOWEST_SPEED, 255); //these 3 lines set left wheel to forward
  ledcWrite(3, 0); //sets M2B to LOW
  ledcWrite(2, left); //sets M2A to pwm
}

void leftTurn(uint8_t left, uint8_t right){ //function for spinning the robot in a counterclockwise direction
  left = map(left, 128, 0, SLOWEST_SPEED, 255); //these 3 lines set left wheel to reverse
  ledcWrite(3, left); //sets M2B to pwm
  ledcWrite(2, 0);  //sets M2A to LOW
  right = map(right, 128, 0, SLOWEST_SPEED, 255);//these 3 lines set right wheel to forward
  ledcWrite(1, 0);  //sets M1B to LOW
  ledcWrite(0, right);  //sets M1A to pwm
}

void setup() {
  //motor setup
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

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

  //setup for light
  FastLED.addLeds<NEOPIXEL, WSLED> (leds, NUM_LEDS);

  //serial monitor setup
  Serial.begin(115200);

  randomSeed(analogRead(0));  //random num generator setup
}

void loop() {
  unsigned long millisNow = millis(); //current time value for movement

  long randMoveVal = random(4); //generates random int between 0 and 3

  if((millisNow - initialTime) > 100){  //checks if it's been 100 milliseconds since last lighting change
    if(leds[0] == CRGB::Black){ //switches lighting from previous state (black to purple or purple to black)
      leds[0] = CRGB::Purple;
    }
    else{
      leds[0] = CRGB::Black;
    }
    FastLED.show();
    initialTime = millisNow;  //updates counter for lighting
  }

  if((millisNow - firstTime) > 500){  //checks if it's been 0.5 seconds since last direction change
    //randomly selects to move forward, backward, or spin left/right depending on random number generated at start of loop
    if(randMoveVal == 0){
      forward(300, 300);
    }
    else if(randMoveVal == 1){
      backward(300, 300);
    }
    else if(randMoveVal == 2){
      leftTurn(300, 300);
    }
    else if(randMoveVal == 3){
      rightTurn(300, 300);
    }

    firstTime = millisNow;  //counter update for current time for movement
  }
  
}