#include <Arduino.h>
#include <Wire.h>

// Set Motor Pins
#define MOTOR_M2A 17
#define MOTOR_M2B 16
#define MOTOR_M1A 19
#define MOTOR_M1B 18

// Motor speeds
#define SLOW 196
#define FAST 255

// Set Motor Speed util function
void set_motor(int l, int r) {
  if (l > 0) {
    ledcWrite(2, l);
    ledcWrite(3, 0);
  } else {
    ledcWrite(2, 0);
    ledcWrite(3, -l);
  }

  if (r > 0) {
    ledcWrite(0, r);
    ledcWrite(1, 0);
  } else {
    ledcWrite(0, 0);
    ledcWrite(1, -r);
  }
}

// Fast turn
void happy() {
  set_motor(255, -255);
  delay(1400);
  set_motor(-255, 255);
  delay(700);
  set_motor(0, 0);
  delay(700);

  set_motor(255, -255);
  delay(1400);
  set_motor(-255, 255);
  delay(700);
  set_motor(0, 0);
  delay(700);
}

// SLow turn
void sad() {
  set_motor(-SLOW, SLOW);
  delay(4000);
  set_motor(0, 0);
}

// Fast forward and bakcward
void angry() {
  set_motor(255, 255);
  delay(500);
  set_motor(-255, -255);
  delay(500);

  set_motor(255, 255);
  delay(500);
  set_motor(-255, -255);
  delay(500);

  set_motor(255, 255);
  delay(500);
  set_motor(-255, -255);
  delay(500);

  set_motor(0, 0);
  delay(500);
}

void scared() {
  // Shake the robot
  for (int i = 0; i < 150; i++) {
      set_motor(SLOW, -SLOW);
    delay(20);
      set_motor(-SLOW, SLOW);
      delay(20);
  }

  set_motor(0, 0);
  delay(500);

}

void setup() {
  // Setup Serial to a Baud rate
  Serial.begin(9600);

  // Setup Motor Pins
  pinMode(MOTOR_M1A, OUTPUT);
  pinMode(MOTOR_M1B, OUTPUT);
  pinMode(MOTOR_M2A, OUTPUT);
  pinMode(MOTOR_M2B, OUTPUT);

  // Setup PWM
  ledcSetup(0, 30000, 8);
  ledcAttachPin(MOTOR_M1A, 0);
  ledcSetup(1, 30000, 8);
  ledcAttachPin(MOTOR_M1B, 1);
  ledcSetup(2, 30000, 8);
  ledcAttachPin(MOTOR_M2A, 2);
  ledcSetup(3, 30000, 8);
  ledcAttachPin(MOTOR_M2B, 3);
}
void loop() {
  // Loop emptions
  happy();
  delay(1000);
  sad();
  delay(1000);
  angry();
  delay(1000);
  scared();
  delay(1000);
}