/*
 * Emotion Expressing Robot
 * Team Members: Colby Spear, Seth Looney
 * Description: This program controls a robot to express four different emotions:
 * Angry, Sad, Scared, and Happy. Each emotion is represented by unique movements.
 */

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

// Sensor instance
VL53L0X sensor;

// Motor control pins
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18

// Define the slowest speed the robot can move
#define SLOWEST_SPEED 190

// Motor control function credits Donald Heer
void motor(uint8_t left, uint8_t right) {
  if (left == 128) { // Set left to stop
    ledcWrite(3, 0); // Set M2B to LOW
    ledcWrite(2, 0); // Set M2A to LOW
  } else if (left > 128) { // Set left to forward
    left = map(left, 128, 255, SLOWEST_SPEED, 255);
    ledcWrite(3, 0); // Set M2B to LOW
    ledcWrite(2, left); // Set M2A to pwm
  } else { // Set left to reverse
    left = map(left, 128, 0, SLOWEST_SPEED, 255);
    ledcWrite(3, left); // Set M2B to pwm
    ledcWrite(2, 0); // Set M2A to LOW
  }

  if (right == 128) { // Set right to stop
    ledcWrite(1, 0); // Set M2B to LOW
    ledcWrite(0, 0); // Set M2A to LOW
  } else if (right > 128) { // Set right to forward
    right = map(right, 128, 255, SLOWEST_SPEED, 255);
    ledcWrite(1, 0); // Set M2B to LOW
    ledcWrite(0, right); // Set M2A to pwm
  } else { // Set right to reverse
    right = map(right, 128, 0, SLOWEST_SPEED, 255);
    ledcWrite(1, right); // Set M2B to pwm
    ledcWrite(0, 0); // Set M2A to LOW
  }
}

void setup() {
  Serial.begin(115200);

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

  // Connect 4 PWM 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);

  // Initialize the sensor
  Wire.begin();
  sensor.init();
  sensor.setTimeout(500);
  if (!sensor.init()) {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) {}
  }
}

void expressAngry() {
  // Move forward rapidly and then shake left and right
  for (int i = 0; i < 5; i++) {
    motor(255, 255); // Move forward fast
    delay(200);
    motor(128, 128); // Stop
    delay(100);
    motor(255, 0); // Turn right
    delay(100);
    motor(0, 255); // Turn left
    delay(100);
  }
  motor(128, 128); // Stop
}

void expressSad() {
  // Move slowly backwards then do a slow circle
  motor(128, 128); // Stop
  delay(1000);
  motor(100, 100); // Move backward slowly
  delay(2000);
  motor(128, 128); // Stop
    motor(110, 0); // Move backward slowly in a circle
  delay(4000);
  motor(128, 128); // Stop
}

void expressScared() {
  // Quick back and forth movements then move backwards
  for (int i = 0; i < 3; i++) {
    int distance = sensor.readRangeSingleMillimeters();
    Serial.println(distance);
    
    if (distance < 50) {
      motor(255, 0); // Turn right quickly
      delay(300);
      motor(0, 255); // Turn left quickly
      delay(150);
      motor(255, 0); // Turn right quickly
      delay(150);
      motor(0, 255); // Turn left quickly
      delay(150);
      motor(255, 0); // Turn right quickly
      delay(150);
      motor(0, 255); // Turn left quickly
      delay(150);
      motor(70, 70);
      delay(2000);
      motor(128,128);
      delay(300);
    } 
  }
}

void expressHappy() {
  // Move forward, spin in a circle, then move forward again
  motor(200, 200); // Move forward
  delay(2000);
  motor(255, 128); // Spin in a circle
  delay(2000);
  motor(200, 200); // Move forward
  delay(2000);
  motor(128, 128); // Stop
}

void loop() {
  int distance = sensor.readRangeSingleMillimeters();
  Serial.print("Loop - Distance: ");
  Serial.println(distance);

  if (distance < 250) {
    expressScared();
    delay(3000); // Pause between emotions
   }else if(distance < 500 && distance > 251){
    expressAngry();
    delay(3000); // Pause between emotions
   }else if( distance < 1000 && distance > 501){
    expressSad();
    delay(3000); // Pause between emotions
   }else if(distance < 1500 && distance > 1001){
    expressHappy();
    delay(3000); // Pause between emotions
    }else {
       motor(200, 200);
    }
 }