//MARKO ZIRDUM, ETHAN BERGAN 6/7/2024 EMOTIONS CODE FOR FINAL ASSIGNMENT
//BASED OFF SIMPLEMOTOR CODE BY DONALD HEER, WITH PARTS TAKEN FROM THE BUMPBOT CODE BY DONALD HEER

/*
4/27/2024
Donald Heer
Test code for the esp32_rbt.0 board
Tests motors, WS2812, and I2C bus with a VLX530X attached.
Expected serial output is a list of distances based on the VLX530X module

GPIO22 SCL
GPIO21 SDA
GPIO19 M1A
GPIO18 M1B
GPIO17 M2A
GPIO16 M2B
GPIO4 WS2812
*/

//includes some libraries for the sensor and globally defines int "count" for use later
#include <Wire.h>
#include <VL53L0X.h>
VL53L0X sensor;
int count;
#include <FastLED.h>
//Each motor (M1 and M2) has two pins connected.
//Setting one pin HIGH and the other LOW will make the motor turn one way
//Setting them in reverse will make it turn the opposite way
//Seting them both HIGH or both LOW stops the motor
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18
#define WSLED 4
//more globally defined variables
int emotion;
//this one is for the sensor
uint32_t lastTime = 0;


//This function sets B LOW and A LOW for both motors
void off(){
  digitalWrite(M2B, LOW); //Set M2B to LOW
  digitalWrite(M2A, LOW); //Set M2A to LOW
  digitalWrite(M1B, LOW);//Set M1B to LOW
  digitalWrite(M1A, LOW); //Set M1A to LOW
}

//This function sets B HIGH and A LOW for both motors
void reverse(){
  digitalWrite(M2B, HIGH); //Set M2B to HIGH
  digitalWrite(M2A, LOW); //Set M2A to LOW
  digitalWrite(M1B, HIGH);//Set M1B to HIGH
  digitalWrite(M1A, LOW); //Set M1A to LOW
}

//This function sets B LOW and A HIGH for both motors
void forward(){
  digitalWrite(M2B, LOW); //Set M2B to LOW
  digitalWrite(M2A, HIGH); //Set M2A to HIGH
  digitalWrite(M1B, LOW);//Set M1B to LOW
  digitalWrite(M1A, HIGH); //Set M1A to HIGH
}

//This function sets B HIGH and A LOW for M2 and the opposite for M1
void cw(){
  digitalWrite(M2B, HIGH); //Set M2B to HIGH
  digitalWrite(M2A, LOW); //Set M2A to LOW
  digitalWrite(M1B, LOW);//Set M1B to LOW
  digitalWrite(M1A, HIGH); //Set M1A to HIGH
}

//This function sets B HIGH and A LOW for M1 and the opposite for M2
void ccw(){
  digitalWrite(M2B, LOW); //Set M2B to LOW
  digitalWrite(M2A, HIGH); //Set M2A to HIGH
  digitalWrite(M1B, HIGH);//Set M1B to HIGH
  digitalWrite(M1A, LOW); //Set M1A to LOW
}

void setup() {
  Serial.begin(115200);
  // Set the 4 motor controll lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);
  //initializes some values
  emotion = 1;
  count = 0;
  //sets up the sensor and checks if it's actually there
  Wire.begin();
  sensor.setTimeout(500);
  if (!sensor.init())
  {
    Serial.println("Failed to detect and initialize sensor!");
  }
  
  // This section creates the I2C (Wire) connection and quieries for a VL53LX
  
}

void loop() {
  
int distance = sensor.readRangeSingleMillimeters();
  //Serial.println("the distance is:");
  //Serial.println(distance);
  if(emotion == 1){ //angry emotion
    off();
    forward();
    delay(300);
    off();
    reverse();
    delay(300);
    count++;
    if(count > 4){
      emotion = 2;
      count = 0;
    }
  }
  if(emotion == 2){ //sad emotion
    off();
    forward();
    delay(100);
    off();
    delay(random(1000));
    forward();
    delay(150);
    off();
    delay(random(3000));
    cw();
    delay(random(500));
    off();
    forward();
    delay(50);
    off();
    delay(1000);
    forward();
    delay(75);
    off();
    delay(3500);
    ccw();
    delay(random(500));
    off();
    delay(random(300));
    count++;
    if(count > 2){
      emotion = 3;
      count = 0;
    }
  }
  if(emotion == 3){ //happy emotion
    ccw();
    delay(8000);
    emotion = 4;
  }
  if(emotion == 4){ //scared emotion
   
    Serial.println(distance);
    if(distance < 150){
      reverse();
    }
    else{
    off();
    delay(random(500));
    cw();
    delay(random(200));
    off();
    delay(random(500));
    ccw();
    delay(random(200));
    off();
    }
  }
  Serial.println(emotion);
}