#include <Arduino.h>
#include <NimBLEDevice.h>
#include <NimBLEHIDDevice.h>
#include <HIDKeyboardTypes.h>

#define DEVICE_NAME "MyGamepad"
#define BUTTON_NFC 0x01
#define RESET_PIN 12  // Pin for the physical reset button

NimBLEHIDDevice* hidDevice;
NimBLECharacteristic* inputReport;
NimBLEServer* pServer;
NimBLECharacteristic* customCharacteristic;
NimBLEService* customService;  // Declare customService


void restartAdvertising() {
    NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising();
    if (pAdvertising->isAdvertising()) {
        pAdvertising->stop();
        delay(500);  // Increased delay to ensure advertising fully stops
    }

    pAdvertising->setAppearance(HID_GAMEPAD);
    pAdvertising->addServiceUUID(hidDevice->hidService()->getUUID());
    pAdvertising->addServiceUUID(customService->getUUID());  // Add the custom service UUID

    pAdvertising->start();
    Serial.println("Advertising restarted.");
}


class MyServerCallbacks : public NimBLEServerCallbacks {

    void onConnect(NimBLEServer* pServer) {
        Serial.println("Device connected.");
        
    }

  void onDisconnect(NimBLEServer* pServer) {
    Serial.println("Device disconnected.");
    restartAdvertising();
  }

};


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

    pinMode(RESET_PIN, INPUT_PULLUP);  // Configure the reset button

    // Initialize NimBLE with the device name
    NimBLEDevice::init(DEVICE_NAME);

    // Set up security for bonding
    NimBLESecurity* pSecurity = new NimBLESecurity();
    pSecurity->setAuthenticationMode(ESP_LE_AUTH_REQ_SC_MITM_BOND);  // Require bonding and MITM protection
    pSecurity->setCapability(ESP_IO_CAP_IO);  // Require a passkey
    pSecurity->setInitEncryptionKey(ESP_BLE_ENC_KEY_MASK | ESP_BLE_ID_KEY_MASK);  // Enable encryption

    // Create a NimBLE server
    pServer = NimBLEDevice::createServer();
    pServer->setCallbacks(new MyServerCallbacks());

    // Create the HID device
    hidDevice = new NimBLEHIDDevice(pServer);
    inputReport = hidDevice->inputReport(1);

    // Set manufacturer name and HID info
    hidDevice->manufacturer()->setValue("My Company");
    hidDevice->pnp(0x01, 0x02E5, 0xABCD, 0x0110);
    hidDevice->hidInfo(0x00, 0x01);

    // Define the HID report map for a single button game controller
    const uint8_t reportMap[] = {
        0x05, 0x01,  // USAGE_PAGE (Generic Desktop)
        0x09, 0x05,  // USAGE (Gamepad)
        0xA1, 0x01,  // COLLECTION (Application)
        0x05, 0x09,  // USAGE_PAGE (Button)
        0x19, 0x01,  // USAGE_MINIMUM (Button 1)
        0x29, 0x01,  // USAGE_MAXIMUM (Button 1)
        0x15, 0x00,  // LOGICAL_MINIMUM (0)
        0x25, 0x01,  // LOGICAL_MAXIMUM (1)
        0x75, 0x01,  // REPORT_SIZE (1)
        0x95, 0x01,  // REPORT_COUNT (1)
        0x81, 0x02,  // INPUT (Data,Var,Abs)
        0x75, 0x07,  // REPORT_SIZE (7)
        0x95, 0x01,  // REPORT_COUNT (1)
        0x81, 0x03,  // INPUT (Cnst,Var,Abs)
        0xC0         // END_COLLECTION
    };

    hidDevice->reportMap((uint8_t*)reportMap, sizeof(reportMap));
    hidDevice->startServices();

    // Create a custom service and characteristic for sending data
    NimBLEService* customService = pServer->createService("12345678-1234-1234-1234-123456789abc");
    customCharacteristic = customService->createCharacteristic(
        "87654321-4321-4321-4321-abc123456789",
        NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::NOTIFY
    );
    customCharacteristic->setValue("Initial Value");
    customService->start();

    // Start advertising initially
    NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising();
    pAdvertising->setAppearance(HID_GAMEPAD);
    pAdvertising->addServiceUUID(hidDevice->hidService()->getUUID());
    pAdvertising->addServiceUUID(customService->getUUID());  // Add the custom service UUID
    pAdvertising->start();

    Serial.println("Waiting for a client connection...");
}

void loop() {
    // Check if the reset button is pressed
    if (digitalRead(RESET_PIN) == LOW) {
        // Clear all bonding information and reset BLE stack
        NimBLEDevice::deleteAllBonds();
        Serial.println("Bonding information cleared.");

        // Restart advertising
        pServer->getAdvertising()->start();
        Serial.println("Advertising restarted after reset.");

        delay(1000);  // Debounce delay
    }

    // Example: Send data to the connected device via the custom characteristic
    String dataToSend = "T001:Lamp:Tool";
    customCharacteristic->setValue((uint8_t*)dataToSend.c_str(), dataToSend.length());

    customCharacteristic->notify();

    Serial.println("Sent data: " + dataToSend);

    delay(5000);  // Wait before sending the next data
}