#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <WiFi.h>
#include <esp_bt_main.h>
#include <esp_bt_device.h>

void print_BT_DeviceAddress(){
  const uint8_t* point = esp_bt_dev_get_address();
  for (int i = 0; i < 6; i++) {
    char str[3];
    sprintf(str, "%02X", (int)point[i]);
    Serial.print(str);
    if (i < 5){
      Serial.print(":");
    } 
  }
}

bool deviceConnected = false;

// Service name to the broadcasted to outside world
#define PERIPHERAL_NAME "PWB Device"
#define SERVICE_UUID "CD9CFC21-0ECC-42E5-BF22-48AA715CA112"
#define CHARACTERISTIC_INPUT_UUID "66E5FFCE-AA96-4DC9-90C3-C62BBCCD29AC"
#define CHARACTERISTIC_OUTPUT_UUID "142F29DD-B1F0-4FA8-8E55-5A2D5F3E2471"

// Output characteristic is used to send the response back to the connected phone
BLECharacteristic *pOutputChar;
// Curren t value of output characteristic persisted here
static uint8_t outputData[1];

// Class defines methods called when a device connects and disconnects from the service
class ServerCallbacks: public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
        deviceConnected = true;
        Serial.println("BLE Client Connected");
    }
    void onDisconnect(BLEServer* pServer) {
        deviceConnected = false;
        BLEDevice::startAdvertising();
        Serial.println("BLE Client Disconnected");
    }
};

class InputReceivedCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharWriteState) {
        Serial.print("Recived: ");
        //Serial.println((char*)pCharWriteState->getData());
        String myString = String((char*)pCharWriteState->getData());
        Serial.println(myString);  
        /*
        uint8_t *inputValues = pCharWriteState->getData();
        switch(inputValues[0]) {
          case 0x00: //
            Serial.printf("0x00:   %02x \r\n", inputValues[0]);  
            outputData[0] = inputValues[0];  
            break;
          case 0x01: //
            Serial.printf("0x01:   %02x \r\n", inputValues[0]);
            outputData[0] = inputValues[0];  
            break;
          default:
            Serial.printf("default:   %02x \r\n", inputValues[0]);  
            outputData[0] = inputValues[0];  
        }
         
        Serial.printf("Sending response:   %02x\r\n", outputData[0]);  
        */
        //pOutputChar->setValue((uint8_t *)outputData, 1);
        uint8_t data[] = "Hello World";
        pOutputChar->setValue((uint8_t *)data,sizeof(data));
        pOutputChar->notify();
      /*std::string inputValue = pCharWriteState->getValue();
        if (inputValue.length() > 0) {
          Serial.print("Received Value: ");
          for (int i = 0; i < inputValue.length(); i++) {
            Serial.print(inputValue[i]);
          }
          Serial.println();
          // Send data to client
          delay(1000);
          std::string outputData = "Last received: " + inputValue;
          pOutputChar->setValue(outputData);
          pOutputChar->notify();
        }*/
    }
};

void setup() {
  Serial.begin(115200);

  // Initialize BLE
  BLEDevice::init(PERIPHERAL_NAME);

  // Create the server
  BLEServer *pServer = BLEDevice::createServer();
  
  // Create the service
  BLEService *pService = pServer->createService(SERVICE_UUID);

  // Handle inputs (sent from app)
  BLECharacteristic *pInputChar 
      = pService->createCharacteristic(
                    CHARACTERISTIC_INPUT_UUID,                                        
                    BLECharacteristic::PROPERTY_WRITE_NR |
                    BLECharacteristic::PROPERTY_WRITE);

  pOutputChar = pService->createCharacteristic(
                        CHARACTERISTIC_OUTPUT_UUID,
                        BLECharacteristic::PROPERTY_READ |
                        BLECharacteristic::PROPERTY_NOTIFY);

  pServer->setCallbacks(new ServerCallbacks());                  
  pInputChar->setCallbacks(new InputReceivedCallbacks());

  // Initial characteristic value
  outputData[0] = 0x00;
  pOutputChar->setValue((uint8_t *)outputData, 1);
  
  // Start service
  pService->start();

  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  BLEDevice::startAdvertising();

  Serial.println("BLE Service is advertising");
  
  Serial.print("ESP Board Bluetooth MAC Address:  ");
  print_BT_DeviceAddress();
  
  Serial.println();
  Serial.print("ESP Board WiFi MAC Address:  ");
  Serial.println(WiFi.macAddress()); 
}

void loop() { 
  if (deviceConnected) {
    uint8_t data[] = "Super test...";
    pOutputChar->setValue((uint8_t *)data,sizeof(data));
    pOutputChar->notify();
  }
  delay(2000);
        
  // ESP32-CAM RED LED
  //pinMode(33, OUTPUT);
  //RED LED ON
  //digitalWrite(33, LOW);   
  //delay(1000);                     
  //RED LED OFF
  //digitalWrite(33, HIGH);  
  //delay(1000);    
   
  // ESP32 BLUE LED
  //pinMode(2, OUTPUT);
  //RED LED ON
  //digitalWrite(2, LOW);   
  //delay(1000);                     
  //RED LED OFF
  //digitalWrite(2, HIGH);  
  //delay(1000);       
  }