//____________________________________________________________________________________________________________________________
//DESCRIPTION-----------------------------------------------------------------------------------------------------------------
//This code is for the jewelry box in the Solomon's Box exhibit. If someone triggers the gesture sensor in front of
//the box, it spins its design and sends a signal over to the master Arduino (Solomon's box).

//____________________________________________________________________________________________________________________________
//LIBRARIES AND DEFINITIONS---------------------------------------------------------------------------------------------------
#include <ArduinoBLE.h>
#include <Arduino_APDS9960.h>
#include <Servo.h>
#include <SPI.h>



//____________________________________________________________________________________________________________________________
//GLOBAL VARIABLES------------------------------------------------------------------------------------------------------------
//Servo
Servo motor;

//BLE
const char* deviceServiceUuid = "19b10000-e8f2-537e-4f6c-d104768a2008";
const char* deviceCharacteristicUuid = "19b10000-e8f2-537e-4f6c-d104768a1990";

int gesture = -1;
int motorPos1 = 0;
BLEService jewelryBoxService(deviceServiceUuid);
BLEIntCharacteristic jewelryBoxCharacteristic(deviceCharacteristicUuid, BLERead | BLEWrite);

//PINS----------------------
//LED
const int ledPin = 11;

//Servo Motor
const int servoPin = 8;

//Ultrasonic Sensors
const int echoPin3 = 10;
const int trigPin3 = 9;
const int echoPin2 = 7;
const int trigPin2 = 6;
const int echoPin1 = 5;
const int trigPin1 = 4;

//DC Motors
const int motorPin1 = 3;
const int motorPin2 = 2;
const int motorSpeed = 100;



//Other Stuff
bool inView1 = false; //Keep track of whether someone is in front of the tower or not
bool inView2 = false;
unsigned long notInViewTimerStart1 = millis();
unsigned long notInViewTimerStart2 = millis();
bool timerStarted1 = false;
bool timerStarted2 = false;
bool twinClockTowers; //tracks whether or not a message should be sent to Solomon's box
int timeLimit = 200;
int distanceLimit = 30;
bool timeConnectedStarted = false;
int timeConnected = millis();



//____________________________________________________________________________________________________________________________
//INITIALIZATION--------------------------------------------------------------------------------------------------------------
void setup() {
  Serial.begin(115200);
  delay(10000);
  
  //APDS------------------------------------------------------------
  while (!Serial);
  if (!APDS.begin()) {
    Serial.println("Error initializing APDS9960 sensor!");
  }
  // for setGestureSensitivity(..) a value between 1 and 100 is required.
  // Higher values makes the gesture recognition more sensible but less accurate
  // (a wrong gesture may be detected). Lower values makes the gesture recognition
  // more accurate but less sensible (some gestures may be missed).
  // Default is 80
  //APDS.setGestureSensitivity(80);

  //Servo------------------------------------------------------------
  motor.attach(servoPin);

  //Ultrasonic sensor initialization
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);

  //Motor initialization
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);

  //BLE--------------------------------------------------------------
  BLE.begin();
  if (!BLE.begin()) {                                //if bluetooth initiation fails, stop running code
    Serial.println("Bluetooth initiation failed");
    while(1);
  }

  BLE.setLocalName("Jewelry Box");
  BLE.setDeviceName("Jewelry Box");
  BLE.setAdvertisedService(jewelryBoxService);
  jewelryBoxService.addCharacteristic(jewelryBoxCharacteristic);
  jewelryBoxCharacteristic.writeValue(0);
  BLE.addService(jewelryBoxService);
  BLE.advertise();

  Serial.println("Advertising Jewelry Box");
}




//____________________________________________________________________________________________________________________________
//LOOP------------------------------------------------------------------------------------------------------------------------
void loop() {
  BLEDevice central = BLE.central();
  
  if (central) {
    //While connected to central, detect gesture, turn servo, alter characteristic value
    if (central.connect()) {
      if (!timeConnectedStarted) {
        timeConnectedStarted = true;
        timeConnected = millis();
      }
      if (millis() - timeConnected >= 15000) act();
    }
  }
  else {
    timeConnected = millis();
    timeConnectedStarted = false;
    central.disconnect();
    Serial.println("Looking for central device to connect to");
    delay(500);
  }
}






//____________________________________________________________________________________________________________________________
//HELPER FUNCTIONS------------------------------------------------------------------------------------------------------------

//Used for detecting gesture and altering servo motor position
void act() {
  box();
  clocks();
}

void box() {
  if (APDS.gestureAvailable()) {
    // a gesture was detected, read and print to serial monitor
    int gesture = APDS.readGesture();
    switch (gesture) {
      case GESTURE_UP:
        Serial.println("Detected UP gesture");
        motor.write(90);
        motorPos1 = jewelryBoxCharacteristic.value();
        Serial.println(motorPos1);
        jewelryBoxCharacteristic.writeValue(motorPos1 + 100);
        Serial.print("Jewelry box characteristic val is now: ");
        Serial.println(jewelryBoxCharacteristic.value());
        break;
      case GESTURE_DOWN:
        Serial.println("Detected DOWN gesture");
        motor.write(270);
        motorPos1 = jewelryBoxCharacteristic.value();
        jewelryBoxCharacteristic.writeValue(motorPos1 + 100);
        Serial.println("Just rewrote motor 1 value");
        break;
      case GESTURE_LEFT:
        Serial.println("Detected LEFT gesture");
        motor.write(180);
        motorPos1 = jewelryBoxCharacteristic.value();
        jewelryBoxCharacteristic.writeValue(motorPos1 + 100);
        Serial.println("Just rewrote motor 1 value");
        break;
      case GESTURE_RIGHT:
        Serial.println("Detected RIGHT gesture");
        motor.write(0);
        motorPos1 = jewelryBoxCharacteristic.value();
        jewelryBoxCharacteristic.writeValue(motorPos1 + 100);
        Serial.println("Just rewrote motor 1 value");
        break;
      default:
        break;
    }
  }
}

void clocks() {
  //Ultrasonic sensors
  int dist1 = getDist(trigPin1, echoPin1);
  int dist2 = getDist(trigPin2, echoPin2);
  //Serial.print("Left: ");
  //Serial.print(dist1);
  //Serial.print("cm            ");
  //Serial.print("Right: ");
  //Serial.print(dist2);
  //Serial.println("cm            ");

  //Tower 1
  //If out of view of tower 1, start a timer and stop spinning after certain duration. Else, someone is in view so spin.
  if (dist1 > distanceLimit) {
    if (!timerStarted1) {             //if a timer hasn't already started, start timer
      notInViewTimerStart1 = millis();
      timerStarted1 = true;
    }
    if (millis() - notInViewTimerStart1 >= timeLimit) {
      inView1 = false;
      digitalWrite(motorPin2, 0);
      //for (int i=0; i < 12; i++) {
      //  strip2.setPixelColor(i, 255, 255, 255);
      //}
      //strip2.show();
    }
  }
  else {
    digitalWrite(motorPin2, motorSpeed);
    timerStarted1 = false;
    inView1 = true;
    //for (int i=0; i < 12; i++) {
    //  strip2.setPixelColor(i, 0, 0, 255);
    //}
    //strip2.show();
  }

  //Tower 2
  if (dist2 > distanceLimit) {
    if (!timerStarted2) {             //if a timer hasn't already started, start timer
      notInViewTimerStart2 = millis();
      timerStarted2 = true;
    }
    if (millis() - notInViewTimerStart2 >= timeLimit) {
      inView2 = false;
      digitalWrite(motorPin1, 0);
      //for (int i=0; i < 12; i++) {
      //  strip1.setPixelColor(i, 255, 255, 255);
      //}
      //strip1.show();
    }
  }
  else {
    digitalWrite(motorPin1, motorSpeed);
    timerStarted2 = false;
    inView2 = true;
  //  for (int i=0; i < 12; i++) {
  //    strip1.setPixelColor(i, 0, 0, 255);
  //  }
  //  strip1.show();
  }

  //If both towers are on, send signal to Solomon's Box
  if (inView1 == true && inView2 == true) twinClockTowers = true;
  else twinClockTowers = false;
  
}

//Returns distance detected by ultrasonic sensor
int getDist(int trigPin, int echoPin) {
  long duration, cm;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  cm = microsecondsToCentimeters(duration);
  return cm;
}

//For Ultrasonic Sensor From https://www.tutorialspoint.com/arduino/arduino_ultrasonic_sensor.htm
long microsecondsToInches(long microseconds) {
   return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds) {
   return microseconds / 29 / 2;
}