/*
 * File:      BLEControllerPeripheral.ino
 * Description: BLE peripheral device script to comunicate with a DIY controller installed BLEControllerCentral.ino 
 *            This code outputs the controller status to the serial monitor.         
 * Date:      11/13/2023
 * Author:    Sayaka
 * Hardware   MCU:  ESP32 DEVKIT V4 Board
 */
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLE2902.h>

#define SPI_SPEED 115200

/* BLE */
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/
#define SERVER_NAME "BLE_CONTROLLER_MONITOR"
#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID_RX "beb5483e-36e1-4688-b7f5-ea07361b26a8"

BLECharacteristic *pCharacteristicRX;
bool deviceConnected = false;
bool oldDeviceConnected = false;

/* recieve data */
struct tmpData {
  uint8_t abcdBtn;
  uint8_t efkBtn;
  uint16_t xyJoy[2];
};
struct tmpData data;

const char buttonName[7] = { 'A', 'B', 'C', 'D', 'E', 'F', 'K' };
uint16_t joyOffset[2];
int joyThreshold = 30; // Threshold value to send data


/******************** < Callback classes > ********************/
class ServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer *pServer) {
    deviceConnected = true;
  }
  void onDisconnect(BLEServer *pServer) {
    deviceConnected = false;
  }
};

class ReceiveCallback : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pCharacteristicRX) {

    std::string rxValue = pCharacteristicRX->getValue();
    // if (rxValue.length() > 0) {
    //   Serial.print("Received data: ");
    //   for (int i = 0; i < rxValue.length(); i++) {
    //     Serial.print(rxValue[i], BIN);
    //     Serial.print(",");
    //   }
    //   Serial.println();
    // }

    memcpy(&data, &rxValue[0], sizeof(tmpData));

    if (data.abcdBtn == 0xFF && data.efkBtn == 0xFF) {  // get offset data
      joyOffset[0] = data.xyJoy[0];
      joyOffset[1] = data.xyJoy[1];
    } else {
      //
      if (abs(data.xyJoy[0] - joyOffset[0]) > joyThreshold || abs(data.xyJoy[1] - joyOffset[1]) > joyThreshold){
        Serial.print("Joy X: ");
        Serial.print(data.xyJoy[0] - joyOffset[0]);
        Serial.print(", Joy Y: ");
        Serial.println(data.xyJoy[1] - joyOffset[1]);
      }
      

      for (int i = 0; i < 4; i++) {
        switch (data.abcdBtn >> 2 * (3 - i) & B00000011) {
          case 1:
            Serial.print(buttonName[i]);
            Serial.println(" button is pushed!");
            break;
          case 2:  // stop
            Serial.print(buttonName[i]);
            Serial.println(" button is released!");
            break;
          default:
            break;
        }
      }

      for (int i = 0; i < 3; i++) {
        switch (data.efkBtn >> 2 * (3 - i) & B00000011) {
          case 1:
            Serial.print(buttonName[i + 4]);
            Serial.println(" button is pushed!");
            break;
          case 2:  // stop
            Serial.print(buttonName[i + 4]);
            Serial.println(" button is released!");
            break;
          default:
            break;
        }
      }
    }
  }
};

/******************** < Mandatory functions > ********************/
void setup() {
  Serial.begin(SPI_SPEED);
  // Create the BLE Device
  BLEDevice::init(SERVER_NAME);
  // Create the BLE Server
  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new ServerCallbacks());
  // Create the BLE Service
  BLEService *pService = pServer->createService(SERVICE_UUID);
  // Create a BLE Characteristic
  pCharacteristicRX = pService->createCharacteristic(
                      CHARACTERISTIC_UUID_RX,
                      BLECharacteristic::PROPERTY_WRITE
                      );
  pCharacteristicRX->setCallbacks(new ReceiveCallback());

  // Start the service
  pService->start();
  // Start advertising
  BLEAdvertising *pAdvertising = pServer->getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->start();
  Serial.println("Waiting a central to connect...");
}

void loop() {
  // disconnecting -> reconnecting
  if (!deviceConnected && oldDeviceConnected) {
    Serial.println("BLE disconnected!!");

    // BLE reconnecting
    delay(500);                     // give the bluetooth stack the chance to get things ready
    BLEDevice::startAdvertising();  // restart advertising
    Serial.println("start advertising");
    oldDeviceConnected = deviceConnected;
  }
  // connecting
  if (deviceConnected && !oldDeviceConnected) {
    oldDeviceConnected = deviceConnected;

    Serial.println("BLE connected!");
  }

  delay(10);
}