#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <Update.h>

// LED Pin Definitions
#define LED1_PIN D0  // D
#define LED2_PIN D2  // O
#define LED3_PIN D3  // R
#define LED4_PIN D5  // F

// OTA BLE Variables
BLEServer *pServer = NULL;
BLECharacteristic *pCharacteristic = NULL;
bool deviceConnected = false;
bool otaInProgress = false;
uint32_t otaFileSize = 0;
uint32_t otaReceived = 0;
int lastProgressPercent = -1;

// BLE UUIDs
#define SERVICE_UUID           "66443771-D481-49B0-BE32-8CE24AC0F09C"
#define CHARACTERISTIC_UUID    "66443772-D481-49B0-BE32-8CE24AC0F09C"

class MyServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* pServer) {
      deviceConnected = true;
          Serial.println("[BLE] Device connected.");
            }

              void onDisconnect(BLEServer* pServer) {
                  deviceConnected = false;
                      if (otaInProgress) {
                            Serial.println("[OTA] Update cancelled.");
                                  Update.end(false);
                                        otaInProgress = false;
                                            }
                                                pServer->getAdvertising()->start();
                                                    Serial.println("[BLE] Device disconnected. Re-advertising...");
                                                      }
                                                      };

                                                      class MyCallbacks : public BLECharacteristicCallbacks {
                                                        void onWrite(BLECharacteristic *pCharacteristic) {
                                                            uint8_t* data = pCharacteristic->getData();
                                                                size_t length = pCharacteristic->getLength();

                                                                    if (length == 0) return;

                                                                        if (!otaInProgress && length == 4 && memcmp(data, "OPEN", 4) == 0) {
                                                                              Serial.println("[OTA] Update started.");
                                                                                    otaInProgress = true;
                                                                                          otaFileSize = 0;
                                                                                                otaReceived = 0;
                                                                                                      lastProgressPercent = -1;
                                                                                                            return;
                                                                                                                }

                                                                                                                    if (otaInProgress && length == 4 && memcmp(data, "HALT", 4) == 0) {
                                                                                                                          Serial.println("[OTA] Update cancelled.");
                                                                                                                                Update.end(false);
                                                                                                                                      otaInProgress = false;
                                                                                                                                            return;
                                                                                                                                                }

                                                                                                                                                    if (otaInProgress) {
                                                                                                                                                          if (otaFileSize == 0 && length == 4) {
                                                                                                                                                                  memcpy(&otaFileSize, data, 4);
                                                                                                                                                                          Serial.printf("[OTA] Update size: %u bytes.\n", otaFileSize);
                                                                                                                                                                                  if (!Update.begin(UPDATE_SIZE_UNKNOWN)) {
                                                                                                                                                                                            Serial.println("[OTA] ERROR: Unable to start update.");
                                                                                                                                                                                                      otaInProgress = false;
                                                                                                                                                                                                              }
                                                                                                                                                                                                                      return;
                                                                                                                                                                                                                            }

                                                                                                                                                                                                                                  if (length == 4 && memcmp(data, "DONE", 4) == 0) {
                                                                                                                                                                                                                                          Serial.println("[OTA] Finalizing update.");
                                                                                                                                                                                                                                                  if (otaReceived != otaFileSize) {
                                                                                                                                                                                                                                                            Serial.printf("[OTA] ERROR: Size mismatch (%u/%u bytes).\n", otaReceived, otaFileSize);
                                                                                                                                                                                                                                                                      Update.end(false);
                                                                                                                                                                                                                                                                              } else if (Update.end(true) && Update.isFinished()) {
                                                                                                                                                                                                                                                                                        Serial.println("[OTA] Update successful. Rebooting...");
                                                                                                                                                                                                                                                                                                  ESP.restart();
                                                                                                                                                                                                                                                                                                          } else {
                                                                                                                                                                                                                                                                                                                    Serial.println("[OTA] ERROR: Finalization failed.");
                                                                                                                                                                                                                                                                                                                              Update.printError(Serial);
                                                                                                                                                                                                                                                                                                                                      }
                                                                                                                                                                                                                                                                                                                                              otaInProgress = false;
                                                                                                                                                                                                                                                                                                                                                      return;
                                                                                                                                                                                                                                                                                                                                                            }

                                                                                                                                                                                                                                                                                                                                                                  if (otaReceived < otaFileSize) {
                                                                                                                                                                                                                                                                                                                                                                          size_t written = Update.write(data, length);
                                                                                                                                                                                                                                                                                                                                                                                  if (written > 0) {
                                                                                                                                                                                                                                                                                                                                                                                            otaReceived += written;
                                                                                                                                                                                                                                                                                                                                                                                                      int progress = (otaReceived * 100) / otaFileSize;
                                                                                                                                                                                                                                                                                                                                                                                                                if (progress != lastProgressPercent) {
                                                                                                                                                                                                                                                                                                                                                                                                                            lastProgressPercent = progress;
                                                                                                                                                                                                                                                                                                                                                                                                                                        Serial.printf("[OTA] Progress: %d%%\n", progress);
                                                                                                                                                                                                                                                                                                                                                                                                                                                  }
                                                                                                                                                                                                                                                                                                                                                                                                                                                          }
                                                                                                                                                                                                                                                                                                                                                                                                                                                                  if (Update.hasError()) {
                                                                                                                                                                                                                                                                                                                                                                                                                                                                            Serial.println("[OTA] ERROR: Write failed.");
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      Update.end(false);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                otaInProgress = false;
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        }
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              }
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  } else {
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        Serial.print("[Console] ");
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              Serial.write(data, length);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    Serial.println();
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        }
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          }
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          };

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          // LED animation at startup only
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          void runStartupLEDSequence() {
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            Serial.println("Running initial LED animation...");

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              // Fade-in FORD one by one
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                for (int i = 0; i <= 255; i++) {
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    analogWrite(LED4_PIN, i);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        delay(2);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          }
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            delay(10);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              for (int i = 0; i <= 255; i++) {
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  analogWrite(LED3_PIN, i);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      delay(2);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        }
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          delay(10);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            for (int i = 0; i <= 255; i++) {
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                analogWrite(LED2_PIN, i);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    delay(2);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      }
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        delay(10);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          for (int i = 0; i <= 255; i++) {
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              analogWrite(LED1_PIN, i);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  delay(2);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    }

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      delay(100);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        analogWrite(LED1_PIN, 0); delay(100);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          analogWrite(LED2_PIN, 0); delay(100);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            analogWrite(LED3_PIN, 0); delay(100);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              analogWrite(LED4_PIN, 0); delay(100);

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                // Right to left chase
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  analogWrite(LED4_PIN, 255); delay(100); analogWrite(LED4_PIN, 0);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    analogWrite(LED3_PIN, 255); delay(100); analogWrite(LED3_PIN, 0);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      analogWrite(LED2_PIN, 255); delay(100); analogWrite(LED2_PIN, 0);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        analogWrite(LED1_PIN, 255); delay(100); analogWrite(LED1_PIN, 0);

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          delay(500);

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            // Fast fade-in FORD together
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              Serial.println("Step 4: Fast Power-Up Fade In (simultaneous)");
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                for (int i = 0; i <= 255; i++) {
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    analogWrite(LED1_PIN, i);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        analogWrite(LED2_PIN, i);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            analogWrite(LED3_PIN, i);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                analogWrite(LED4_PIN, i);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    delay(1);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      }

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        Serial.println("LEDs ON — Holding steady.");
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        }

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

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            // LED Setup
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              pinMode(LED1_PIN, OUTPUT);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                pinMode(LED2_PIN, OUTPUT);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  pinMode(LED3_PIN, OUTPUT);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    pinMode(LED4_PIN, OUTPUT);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      runStartupLEDSequence();

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        // BLE OTA Setup
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          BLEDevice::init("Wible");
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            pServer = BLEDevice::createServer();
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              pServer->setCallbacks(new MyServerCallbacks());

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                BLEService *pService = pServer->createService(SERVICE_UUID);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  pCharacteristic = pService->createCharacteristic(
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      CHARACTERISTIC_UUID,
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          BLECharacteristic::PROPERTY_READ |
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              BLECharacteristic::PROPERTY_WRITE |
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  BLECharacteristic::PROPERTY_WRITE_NR |
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      BLECharacteristic::PROPERTY_NOTIFY
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        );
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          pCharacteristic->addDescriptor(new BLE2902());
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            pCharacteristic->setCallbacks(new MyCallbacks());
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              pService->start();
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                BLEDevice::startAdvertising();
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  Serial.println("[BLE] Advertising started.");
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  }

                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  void loop() {
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    // Nothing for LEDs in loop — just BLE heartbeat if desired
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      if (deviceConnected && !otaInProgress) {
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          static unsigned long prevMillis = 0;
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              if (millis() - prevMillis > 1000) {
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    prevMillis = millis();
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          String msg = "Counter: " + String(millis() / 1000);
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                pCharacteristic->setValue(msg.c_str());
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      pCharacteristic->notify();
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          }
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            }
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            }