//____________________________________________________________________________________________________________________________
//DESCRIPTION-----------------------------------------------------------------------------------------------------------------
//This code is for the rabbit in the Solomon's Box exhibit. If someone triggers the proximity sensor in front of
//it, it lights up 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>

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

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

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

//LED
#define LED_PIN    4
#define LED_COUNT 12
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);
bool stripWhite = true;

//Other Stuff
int distanceLimit = 36;
bool inView; //tracks whether or not somebody is in front of the box
const int echoPin1 = 3;
const int trigPin1 = 2;
bool timeConnectedStarted = false;
int timeConnected = 0;
bool goingUp = true;

//____________________________________________________________________________________________________________________________
//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("Bear");
  BLE.setDeviceName("Bear");
  BLE.setAdvertisedService(bearService);
  bearService.addCharacteristic(bearCharacteristic);
  bearCharacteristic.writeValue(0);
  BLE.addService(bearService);
  BLE.advertise();

  //LED----------------------------------------------------------------
  strip.begin();           // INITIALIZE NeoPixel strip object (REQUIRED)
  strip.show();            // Turn OFF all pixels ASAP
  strip.setBrightness(50);
  for(int i=0; i<12; i++) {
    strip.setPixelColor(i, 0, 0, 0);
  }
  strip.show();

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

  Serial.println("Advertising Bear");
}




//____________________________________________________________________________________________________________________________
//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() {
  //Ultrasonic sensors
  int dist1 = getDist(trigPin1, echoPin1);
  //Serial.print(dist1);
  //Serial.println("cm");

  if (dist1 <= distanceLimit) {
    for(int i=0; i <= strip.numPixels(); i++) {
      strip.setPixelColor(i, 0, 0, 255);
    }
    strip.show();
    if (!inView) {
      updatePos();
    }
    inView = true;
  }
  else {
    for(int i=0; i <= strip.numPixels(); i++) {
      strip.setPixelColor(i, 0, 0, 0);
    }
    strip.show();
    inView = false;
  }
}

void updatePos() {
  int motorPos1 = bearCharacteristic.value();
  bearCharacteristic.writeValue(motorPos1 + 77);
  Serial.println(bearCharacteristic.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;
}