#include "BLE_SP110E.h"
//#include "NimBLEDevice.h"
#include "esp_log.h"
#include "WiFi.h"
#include "ATALights.h"
#include "BleSettings.h"

static const char *tag = "BLE_SP110E";

//#ifndef BT_SERVICE
 //   #define BT_SERVICE "FFE0"
//#endif

//#ifndef BT_SP110E_CHARACTERISTIC        
//    #define BT_SP110E_CHARACTERISTIC "FFE1"
//#endif

NimBLECharacteristic *pSP110ECharacteristic = nullptr;

//#ifndef BT_STICK_CHARACTERISTIC
//    #define BT_STICK_CHARACTERISTIC "FFE2"
//#endif

NimBLECharacteristic *pStickCharacteristic = nullptr;

NimBLEClient* pStickClient;
NimBLERemoteCharacteristic* pRemoteCharacteristic;
bool masterFound = false;
NimBLEAdvertisedDevice* myDevice; 

TaskHandle_t LightStick_Client_Task_Handle = NULL;

//#define UPGRADE_SERVICE_UUID "abcdef01-2345-6789-1234-56789abcdef0"
//#define UPGRADE_CHARACTERISTIC_UUID "abcdef01-2345-6789-1234-56789abcdef1"

//typedef enum {SM16703,TM1804,UCS1903,WS2811,WS2801,SK6812,LPD6803,LPD8806,APA102,APA105,DMX512,TM1914,TM1913,P9813,INK1003,P943S,P9411,P9413,TX1812,TX1813,GS8206,GS8208,SK9822,TM1814,SK6812_RGBW,P9414,PG412,IC_MODEL_COUNT } IC_MODELS;
//typedef enum {RGB,RBG,GRB,GBR,BRG,BGR,SEQUENCE_COUNT} SEQUENCES;

typedef struct {
    uint8_t checksum;
    uint8_t enable;
    uint8_t preset;
    uint8_t speed;
    uint8_t bright;
    uint8_t ic_model;
    uint8_t channel; // sequence
    uint8_t count_msb; // 16bit number
    uint8_t count_lsb;
    uint8_t red;
    uint8_t green;
    uint8_t blue;
    uint8_t white;
} INFO_PACK;

INFO_PACK led_status = {};
uint8_t led_num = 1;

uint8_t calculateChecksum(const uint8_t bArr[]) {
    return (uint8_t)((bArr[2]) | ((bArr[0] << 1) & 254 & 105) | bArr[1]);
}

// Class for handling characteristic events
class SP110ECallbacks : public NimBLECharacteristicCallbacks {
    void onWrite(NimBLECharacteristic *pCharacteristic)  override {
        std::string rawValue = pCharacteristic->getValue();
        const uint8_t* value = reinterpret_cast<const uint8_t*>(rawValue.data());
        size_t length = rawValue.length();
        if (length >= 3) {
            ESP_LOGI(tag, "Data received 0x%02X, 0x%02X, 0x%02X (length %zu):", value[0], value[1], value[2], length);
        } else {
            ESP_LOGI(tag, "Data received (length %zu)", length);
        }

        sendToAllClients(value, length);
        process_BLE_SP110E_Command(value, length, pCharacteristic);
    }
};

class LightStickCallbacks : public NimBLECharacteristicCallbacks {
    void onRead(NimBLECharacteristic *pCharacteristic)  override {
        pCharacteristic->setValue("Hello...");
        pCharacteristic->notify();
    }
};


void sendToAllClients(const uint8_t *data, size_t len) {
    // Check if the characteristic is valid and has subscribed clients
    if (pStickCharacteristic != nullptr) {
        pStickCharacteristic->setValue(data, len);
        // This call notifies all connected clients with the same message
        pStickCharacteristic->notify();
    }
}


void process_BLE_SP110E_Command(const uint8_t* val, uint8_t len, NimBLECharacteristic* bleChar) {

    if (len >= 4) {
        uint8_t command = val[3];
        ESP_LOGI(tag, "Command received: 0x%02X", command);

        uint8_t response[sizeof(INFO_PACK)]; // Use a single response buffer

        // Handle different commands
        switch (command) {
            case TURN_ON:
                Lights_Set_ON();
                led_status.enable = 1;
                //ESP_LOGI(tag, "Lights ON");
                break;
            case TURN_OFF:
                Lights_Set_OFF();
                led_status.enable = 0;
                //ESP_LOGI(tag, "Lights OFF");
                break;
            case SET_STATIC_COLOR:
                led_status.red = val[1];
                led_status.green = val[2];
                led_status.blue = val[0];
                Lights_Set_Animation(SOLID_COLOR_INDEX, val[0], val[1], val[2]);
                //ESP_LOGI(tag, "Color set to R:%d G:%d B:%d", led_status.red, led_status.green, led_status.blue);
                break;
            case SET_BRIGHT:
                led_status.bright = val[0];
                Lights_Set_Brightness(val[0]);
                //ESP_LOGI(tag, "Bright set to %d", led_status.bright);
                break;
            case SET_WHITE:
                led_status.white = val[0];
                Lights_Set_White(val[0]);
                //ESP_LOGI(tag, "White set to %d", led_status.white);
                break;
            case SET_PRESET:
                led_status.preset = val[0];
                Lights_Set_Animation(val[0], val[1], val[2], 0);
                //ESP_LOGI(tag, "Animation set to %d", led_status.preset);
                break;
            case SET_SPEED:
                led_status.speed = val[0];
                ESP_LOGI(tag, "Mode set to %d", led_status.speed);
                break;
            case GET_CHECK_DEVICE: // This prepends a checksum
                led_status.checksum = calculateChecksum(val);
                if(bleChar != nullptr){
                    bleChar->setValue((uint8_t *)&led_status, sizeof(INFO_PACK));
                    bleChar->notify(); // Send the data immediately
                }
                ESP_LOGI(tag, "Check Device");
                break;
            case GET_DEVICE_INFO: // No checksum
                led_status.checksum = 0;
                if(bleChar != nullptr){
                    bleChar->setValue(((uint8_t *)&led_status) + 1, sizeof(INFO_PACK) - 1);
                    bleChar->notify(); // Send the data immediately
                }
                ESP_LOGI(tag, "Get Device Info");
                break;
            case SET_IC_MODEL:
                led_status.ic_model = val[0];
                ESP_LOGI(tag, "IC Model set to %d", led_status.ic_model);
                break;
            case SET_RGB_SEQUENCE:
                led_status.channel = 0;
                ESP_LOGI(tag, "Set RGB Sequence");
                break;
            case SET_LED_NUM:
                led_status.count_msb = 0;
                led_status.count_lsb = 100;
                ESP_LOGI(tag, "Set LED Num");
                break;
            case SET_DEVICE_NAME:
                ESP_LOGI(tag, "Set Device Name");
                break;
            default:
                ESP_LOGW(tag, "Unknown command: 0x%02X", command);
                    break;
        }
    }
}


void Init_BLE_SP110E(NimBLEServer* pServer) {
    led_status.speed = 10;
    led_status.bright = 50;
    led_status.ic_model = 0;
    led_status.count_lsb = 20;

    // Create BLE Service
    NimBLEService *pService = pServer->createService( BTServiceUUID.c_str() ); // Use the defined service UUID

    // Create FFE1 Characteristic with WRITE and NOTIFY properties
    pSP110ECharacteristic = pService->createCharacteristic(
        BTSP110ECharacteristicUUID.c_str(),
        NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::NOTIFY
    );

    // Register the callback with the characteristic
    pSP110ECharacteristic->setCallbacks(new SP110ECallbacks());
    ESP_LOGI(tag, "Custom callback registered!");

    /************* Light Stick Characteristic ***************/
    pStickCharacteristic = pService->createCharacteristic(
        BTStickCharacteristicUUID.c_str(),
        NIMBLE_PROPERTY::NOTIFY
    );

    // Register the callback with the characteristic
    pStickCharacteristic->setCallbacks(new LightStickCallbacks());
    ESP_LOGI(tag, "Custom callback registered!");

    /****************************************************/
        

    // Start the Service
    pService->start();

    // Configure Advertising
    NimBLEAdvertising *pAdvertising = NimBLEDevice::getAdvertising();
    pAdvertising->addServiceUUID( BTServiceUUID.c_str() ); // Advertise the FFE0 service UUID

    const uint8_t manufacturerData[] = {0x00, 0x00, 0x38, 0x93, 0x0E, 0x12, 0xAA, 0x08};  // Example Manufacturer Data
    pAdvertising->setManufacturerData(std::string((char *)manufacturerData, sizeof(manufacturerData)));

}


// Callback for notifications from characteristic2
class LightStickClientCallback : public NimBLEClientCallbacks {
    void onNotify(NimBLERemoteCharacteristic* pRemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify) {
      Serial.print("Notification received from characteristic2: ");
      //Serial.write(pData, length);
      //Serial.println();
      process_BLE_SP110E_Command(pData, length, nullptr);
    }
};

void Init_BLE_LightStick_Client(){
    if(LightStick_Client_Task_Handle != NULL) {
        ESP_LOGW(tag, "Light Stick Client Task already running");
        return;
    }
    xTaskCreate(BLE_LightStick_Client_Task, "VersionCheckTask", 1024*8, NULL, 1, &LightStick_Client_Task_Handle);
}

// Task for the BLE LightStick client
// This task connects to the BLE server and waits for notifications
void BLE_LightStick_Client_Task(void *parameter) {
    static const char *tag = "BLE_LightStick_Client_Task";
    ESP_LOGI(tag, "BLE LightStick Client Task started");

    while (true) {
        // Only try to connect if we're not already connected and a device is set.
        if ((pStickClient == nullptr || !pStickClient->isConnected()) && myDevice != nullptr) {
            // Create a new client instance if needed.
            if (pStickClient == nullptr) {
                pStickClient = NimBLEDevice::createClient();
                pStickClient->setClientCallbacks(new LightStickClientCallback());
            }
            ESP_LOGI(tag, "Attempting to connect to the server");
            if (pStickClient->connect(myDevice)) {
                ESP_LOGI(tag, "Connected to the server");

                // Get the service.
                NimBLERemoteService* pRemoteService = pStickClient->getService(BTServiceUUID.c_str());
                if (pRemoteService == nullptr) {
                    ESP_LOGE(tag, "Failed to find service UUID: %s", BTServiceUUID.c_str());
                    pStickClient->disconnect();
                } else {
                    // Get the characteristic.
                    pRemoteCharacteristic = pRemoteService->getCharacteristic(BTStickCharacteristicUUID.c_str());
                    if (pRemoteCharacteristic != nullptr && pRemoteCharacteristic->canNotify()) {
                        pRemoteCharacteristic->subscribe(true, [](NimBLERemoteCharacteristic* pRemoteCharacteristic, 
                                                                    uint8_t* pData, size_t length, bool isNotify) {
                            Serial.print("Notification received: ");
                            Serial.write(pData, length);
                            Serial.println();
                            process_BLE_SP110E_Command(pData, length, nullptr);
                        });
                    } else {
                        ESP_LOGE(tag, "Failed to register for notifications");
                        pStickClient->disconnect();
                    }
                }
            } else {
                ESP_LOGE(tag, "Failed to connect to the server");
                // Delete the client instance so that a new one is created next time.
                if (pStickClient != nullptr) {
                    NimBLEDevice::deleteClient(pStickClient);
                    pStickClient = nullptr;
                }
                // Wait before retrying.
                vTaskDelay(pdMS_TO_TICKS(5000));
                continue;
            }
        }
        
        // If connected, monitor the connection.
        if (pStickClient != nullptr && pStickClient->isConnected()) {
            // Here you could add additional connection health checks if needed.
            vTaskDelay(pdMS_TO_TICKS(1000));
        }
        else {
            // Not connected. Wait a while before the next connection attempt.
            vTaskDelay(pdMS_TO_TICKS(5000));
        }
    }
}



// Callback for BLE scan results
class MyAdvertisedDeviceCallbacks : public NimBLEAdvertisedDeviceCallbacks {
    void onResult(NimBLEAdvertisedDevice* advertisedDevice) {
      if (advertisedDevice->getName() == "ESP32_BLE_Master") { // Replace with your server's name
        NimBLEDevice::getScan()->stop();
        myDevice = advertisedDevice;
        masterFound = true;
        Serial.println("Device found!");
      }
    }
};