/*Final Assignment
Emotion Challenge
Hannah Wold
Isabel Curtin*/
 
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18

#define SLOWEST_SPEED 150

void motor(uint8_t left, uint8_t right) {
  if (left == 128){ //control left motor
    ledcWrite(3, 0); //stop forward motion
    ledcWrite(2, 0); //stop backward motion
  } else if (left > 128){
    left = map(left, 128, 255, SLOWEST_SPEED, 255); //map location for forward motion
    ledcWrite(3, 0);
    ledcWrite(2, left);
  } else {
    left = map(left, 128, 0, SLOWEST_SPEED, 255); //map location for backward motion
    ledcWrite(3, left);
    ledcWrite(2, 0);
  }

  if (right == 128){ //control right motor
    ledcWrite(1, 0);
    ledcWrite(0, 0);
  } else if (right > 128){
    right = map(right, 128, 255, SLOWEST_SPEED, 255);
    ledcWrite(1, 0);
    ledcWrite(0, right);
  } else {
    right = map(right, 128, 0, SLOWEST_SPEED, 255);
    ledcWrite(1, right);
    ledcWrite(0, 0);
  }
}

//configure pins and LED settings
void setup() {
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  ledcSetup(0, 30000, 8);
  ledcAttachPin(M1A, 0); //attach MIA pin to LEDC channel 0
  ledcSetup(1, 30000, 8);
  ledcAttachPin(M1B, 1);
  ledcSetup(2, 30000, 8);
  ledcAttachPin(M2A, 2);
  ledcSetup(3, 30000, 8);
  ledcAttachPin(M2B, 3);
}

//happy function
void happy() {
  unsigned long startTime = millis();
  while (millis() - startTime < 7000) {
    motor(255, 128); // Spin faster in a circle
    delay(50); // Adjust the delay for smoother movement
  }
  motor(128, 128); // Stop
  delay(1000); // Pause after the movement
}

//angry function
void angry() {
  for (int i = 0; i < 3; i++) { // Repeat the pattern 3 times
    unsigned long startTime = millis();
    while (millis() - startTime < 1500) { // Move at angle 110 for 1.5 seconds
      motor(255, 166); // Forward with angle
      delay(25); // Faster direction change
    }
    startTime = millis();
    while (millis() - startTime < 1500) { // Move at angle 30 for 1.5 seconds
      motor(166, 255); // Forward with angle
      delay(25); // Faster direction change
    }
  }
  motor(128, 128); // Stop
  delay(1000); // Pause after the movement
}

//scared function
void scared() {
  for (int i = 0; i < 3; i++) { // Repeat the pattern 3 times
    unsigned long startTime = millis();
    while (millis() - startTime < 3000) {
      motor(255, 255); // Move forward quickly for 3 seconds
      delay(100);
    }
    startTime = millis();
    while (millis() - startTime < 1000) { // Retreat quickly for 1 second
      motor(0, 0);
      delay(50);
    }
  }
  motor(128, 128); // Stop
}

//sad function
void sad() {
  unsigned long startTime = millis();
  while (millis() - startTime < 7000) {
    motor(200, 255); // Move with wider and slower turns
    delay(400);
    motor(255, 200);
    delay(400);
  }
  motor(128, 128); // Stop
}


void loop() {
  happy();
  delay(2000); // Add extra delay between emotions for clarity
  angry();
  delay(2000);
  scared();
  delay(2000);
  sad();
  delay(2000);
}