#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <ESP32Servo.h>

#define LEDPIN 13


#define T1LEDPIN 33
#define T1IRLED 32
#define T1SENSOR 35
#define T1SERVO 27
#define T2LEDPIN 18 
#define T2IRLED 19
#define T2SENSOR 4
#define T2SERVO 16

#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"

BLEServer* pServer = NULL;
BLECharacteristic* pCharacteristic = NULL;
boolean ReadyToRace = false;
long T1high = 0;
long T1low = 0;
long T2high = 0;
long T2low = 0;
long T1diff = 0;
long T2diff = 0;
Servo T1Servo; 
Servo T2Servo;

class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string value = pCharacteristic->getValue();

      if (value.length() > 0) {
        // should be value != "go" but app inventor adds a /0 to the end and I can't seem to detect it
        if(value.length()!=3){
//          Serial.println("*********");
//          Serial.print("New value: ");
//          for (int i = 0; i < value.length(); i++)
//            Serial.print(value[i]);
//  
//          Serial.println();
//          Serial.println("*********");
          Serial.println("Ready To Race!");
          ReadyToRace = true;
          digitalWrite(T1LEDPIN,LOW);
          digitalWrite(T2LEDPIN,LOW);
          digitalWrite(LEDPIN,LOW);
          T1Servo.write(0);
          T2Servo.write(0);
        }
        else{ //if go
          T1Servo.write(180);
          T2Servo.write(180);
          Serial.println("GO!");
        }
      }
    }
};

void setup() {
   pinMode(T1LEDPIN,OUTPUT);
   pinMode(T1IRLED,OUTPUT);
   pinMode(T1SENSOR,INPUT);
   pinMode(T2LEDPIN,OUTPUT);
   pinMode(T2IRLED,OUTPUT);
   pinMode(T2SENSOR,INPUT);
   pinMode(LEDPIN,OUTPUT);
   T2Servo.attach(16);
   T1Servo.attach(27);
  
  
   Serial.begin(9600);
   Serial.println("starting up");
   T1Servo.write(180);
  T2Servo.write(180);
  BLEDevice::init("RaceTrackControl");
  pServer = BLEDevice::createServer();

  BLEService *pService = pServer->createService(SERVICE_UUID);

  pCharacteristic = pService->createCharacteristic(
                                         CHARACTERISTIC_UUID,
                                         BLECharacteristic::PROPERTY_READ |
                                         BLECharacteristic::PROPERTY_WRITE
                                       );

  pCharacteristic->setCallbacks(new MyCallbacks());

  pCharacteristic->setValue("Ready To Race");
  pService->start();

  BLEAdvertising *pAdvertising = pServer->getAdvertising();
  pAdvertising->start();
}

void loop() {
    if(ReadyToRace){
      for(int x = 0;x<100;x++){
        digitalWrite(T1IRLED,HIGH);
        digitalWrite(T2IRLED,HIGH);
        delay(1);
        T1high += analogRead(T1SENSOR);
        T2high += analogRead(T2SENSOR);
        //Serial.println(T2high);
        digitalWrite(T1IRLED,LOW);
        digitalWrite(T2IRLED,LOW);
        delay(1);
        T1low += analogRead(T1SENSOR);
        T2low += analogRead(T2SENSOR);
        //Serial.println(T2low);
      }
//      Serial.print("T1: ");
//      Serial.print(T1low-T1high);
//      Serial.print(",T2: ");
//      Serial.println(T2low-T2high);
      T1diff = T1low-T1high;
      T2diff = T2low-T2high;
      
      // if either has a spike time to stop
      if(T2diff > 100 || T1diff > 100){
        Serial.println("We have a winner!");
        ReadyToRace = false;
        if(T2diff > 100 && T1diff < 100){
          digitalWrite(T2LEDPIN,HIGH);
          Serial.println("T2 Wins");
          pCharacteristic->setValue("1");
        } 
        if(T1diff > 100 && T2diff < 100){
          digitalWrite(T1LEDPIN,HIGH);
          Serial.println("T1 Wins");
          pCharacteristic->setValue("2");
        }
        
      }
      T1high = 0;
      T1low = 0;
      T2high = 0;
      T2low = 0;
      delay(50);
    }
    else{
      digitalWrite(LEDPIN,HIGH);
      delay(50);
    }
}