//____________________________________________________________________________________________________________________________
//DESCRIPTION-----------------------------------------------------------------------------------------------------------------
//This code is for the clocks in the Solomon's Box exhibit. If someone triggers the proximity sensor in front of
//either clock, it spins the hand of the opposite clock and sends a signal over to the master Arduino (Solomon's box).

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


//____________________________________________________________________________________________________________________________
//GLOBAL VARIABLES------------------------------------------------------------------------------------------------------------

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

int motorPos1 = 0;
BLEService clocksService(deviceServiceUuid);
BLEIntCharacteristic clocksCharacteristic(deviceCharacteristicUuid, BLERead | BLEWrite);

//Ultrasonic Sensors
const int echoPin2 = 7;
const int trigPin2 = 6;
const int echoPin1 = 5;
const int trigPin1 = 4;

//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 = 0;

//____________________________________________________________________________________________________________________________
//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);

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

  BLE.setLocalName("clocks");
  BLE.setDeviceName("clocks");
  BLE.setAdvertisedService(clocksService);
  clocksService.addCharacteristic(clocksCharacteristic);
  clocksCharacteristic.writeValue(0);
  BLE.addService(clocksService);
  BLE.advertise();

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

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

  Serial.println("Advertising Clocks");
}




//____________________________________________________________________________________________________________________________
//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();
      }
      while (millis() - timeConnected >= 10000);
    }
  }
  else {
    timeConnected = millis();
    timeConnectedStarted = false;
    central.disconnect();
    Serial.println("Looking for central device to connect to");
    delay(500);
  }
  act();
}






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

//Used for detecting gesture and altering servo motor position
void act() {
  //Ultrasonic sensors
  int dist1 = getDist(trigPin1, echoPin1);
  int dist2 = getDist(trigPin2, echoPin2);
  //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) {
    analogWrite(motorPin2, motorSpeed);
    inView1 = true;
  }
  else {
    analogWrite(motorPin2, 0);
    inView1 = false;
  }

  //Tower 2
  if (dist2 < distanceLimit) {
    analogWrite(motorPin1, motorSpeed);
    inView2 = true;
  }
  else {
    analogWrite(motorPin1, 0);
    inView2 = false;
  }
  
  if (inView1 == true && inView2 == true) updatePos();

}

void updatePos() {
  int motorPos1 = clocksCharacteristic.value();
  clocksCharacteristic.writeValue(motorPos1 + 230);
  Serial.println(clocksCharacteristic.value());
}

//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;
}