#include <Wire.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLE2902.h>

BLEDevice ble;
BLECharacteristic *measurement;
bool deviceConnected = false;


class MyServerCallbacks: public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      deviceConnected = true;
    };

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
    }
};


static char feat[2] = {0x03, 0x00};
static char measure[12] = {0x03, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00};
uint8_t pos = 6;

static uint8_t test_service_uuid128[32] = {
    /* LSB <--------------------------------------------------------------------------------> MSB */
    //first uuid, 16bit, [12],[13] is the value
    0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x00, 0xAB, 0xCD, 0x00, 0x00,
    //second uuid, 32bit, [12], [13], [14], [15] is the value
    0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x00, 0xAB, 0xCD, 0xAB, 0xCD,
};

/* LED pin */
byte ledPin = 14;
/* pin that is attached to interrupt */
byte interruptPin = 25;
/* hold the state of LED when toggling */
volatile byte state = LOW;

unsigned long timeold = 0;
unsigned long braketime = 0;
unsigned long pulses = 0;

//union { uint32_t longrotations; uint8_t rotations[4]; } wheel;

//union { uint16_t longrotations; uint8_t rotations[2]; } crank;

unsigned long wheel;

unsigned long crank;

int delta;
byte accel = 0;
byte brake = 0;

void setup() {
  Serial.begin(115200);
  Serial.println("Starting BLE work!");
  pinMode(ledPin, OUTPUT);
  /* set the interrupt pin as input pullup*/
  pinMode(interruptPin, INPUT_PULLUP);
  pinMode(26, INPUT_PULLUP);
  /* attach interrupt to the pin
  function blink will be invoked when interrupt occurs
  interrupt occurs whenever the pin change value */
  attachInterrupt(digitalPinToInterrupt(interruptPin), blink, RISING);
  Wire.begin(21,22);
  timeold = millis();


  ble.init("MyESP32");
  BLEServer *pServer = new BLEServer();
  pServer->setCallbacks(new MyServerCallbacks());

  BLEService *deviceinfo = pServer->createService(BLEUUID((uint16_t) 0x180A));

  BLECharacteristic *manu_name = deviceinfo->createCharacteristic(
                                         BLEUUID((uint16_t) 0x2A29),
                                         BLECharacteristic::PROPERTY_READ
                                       );


  BLEService *pService = pServer->createService(BLEUUID((uint16_t)0x1816));

  BLECharacteristic *feature = pService->createCharacteristic(
                                         BLEUUID((uint16_t)0x2A5C),
                                         BLECharacteristic::PROPERTY_READ
                                       );

  BLECharacteristic *location = pService->createCharacteristic(
                                         BLEUUID((uint16_t)0x2A5D),
                                         BLECharacteristic::PROPERTY_READ
                                       );

  BLECharacteristic *controlpoint = pService->createCharacteristic(
                                         BLEUUID((uint16_t) 0x2A55),
                                         BLECharacteristic::PROPERTY_WRITE |
                                         BLECharacteristic::PROPERTY_INDICATE
                                         );

  measurement = pService->createCharacteristic(
                                         BLEUUID((uint16_t) 0x2A5B),
                                         BLECharacteristic::PROPERTY_NOTIFY
                                       );

  measurement->addDescriptor(new BLE2902());
  manu_name->setValue("Jabberwock");
  feature->setValue((uint8_t*)feat, sizeof(feat));
  location->setValue(&pos, 1);
  measurement->setValue(std::string(measure, sizeof(measure)));

  pService->start();
  deviceinfo->start();

//  attr.icon = 0x485;
  //GAP_BleAttrDBUpdate(GATT_UUID_GAP_ICON, &attr);

  BLEAdvertising *pAdvertising = pServer->getAdvertising();
  pAdvertising->setAppearance(1157);
  Serial.println("Check");
  pAdvertising->setServiceUUID("00001816-0000-1000-8000-00805f9b34fb");
  pAdvertising->start();
  Serial.println("Characteristic defined! Now you can read it in your phone!!!");
 }

void loop() {

   if ((millis() % 1000) == 0) {
     if (deviceConnected) {
            measurement->setValue(std::string(measure, sizeof(measure)));
            measurement->notify();
          }
 }

  if (millis() - braketime > 200) {
    brake = 0;
  }
  if (millis() - timeold > 300) {
    delta = 300;
  }

  accel = map(constrain (delta, 100, 200), 100, 200, 0, 255);

  Wire.beginTransmission(0x28); // transmit to device #44 (0x2c)
  Wire.write(0xAA);            // sends instruction byte
  Wire.write(accel);             // sends potentiometer value byte
  Wire.endTransmission();     // stop transmitting

  Wire.beginTransmission(0x28); // transmit to device #44 (0x2c)
  Wire.write(0xA9);            // sends instruction byte
  Wire.write(brake);             // sends potentiometer value byte
  Wire.endTransmission();     // stop transmitting


}

/* interrupt function toggle the LED */
void blink() {
  state = !state;
  digitalWrite(ledPin, state);
  Serial.println("Interrupted!");

    if (digitalRead(26) == HIGH) {

        pulses = pulses + 1;

        if ((pulses % 3) == 0) {
          wheel = wheel + 1;
          measure[1]= wheel;
          measure[2]= wheel >> 8;
          measure[3]= wheel >> 16;
          measure[4]= wheel >> 24;
          measure[5]= millis();
          measure[6]= millis() >> 8;

          if (deviceConnected) {
            measurement->setValue(std::string(measure, sizeof(measure)));
            measurement->notify();
          }
        }

       if ((pulses % 8) == 0) {
         crank = crank + 1;
         measure[7]= crank;
         measure[8]= crank >> 8;
         measure[9]= millis();
         measure[10]= millis() >> 8;

         if (deviceConnected) {
            measurement->setValue(std::string(measure, sizeof(measure)));
            measurement->notify();
         }
       }
      delta = millis() - timeold;
      timeold = millis();
      brake = 0;
  }
  else
  {
     braketime = millis();
     if (brake > 127) {
      brake = 255;
    }
    else
    {
      brake = 128;
      //  accel = 0;
    }
  }
}