#include <Arduino.h>
#include <NimBLEDevice.h>
#include <Preferences.h>
#include <PowerLib.hpp>

// UUIDs für eigenen Service und Characteristics
#define SERVICE_UUID        "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
#define CHAR_MOTOR_UUID     "6e400002-b5a3-f393-e0a9-e50e24dcca9e"
#define CHAR_TEMP_UUID      "6e400003-b5a3-f393-e0a9-e50e24dcca9e"
#define CHAR_WIFI_UUID      "6e400004-b5a3-f393-e0a9-e50e24dcca9e"

#define LED_PIN 46

static NimBLEServer*         pServer      = nullptr;
static NimBLECharacteristic* pCharMotor   = nullptr;
static NimBLECharacteristic* pCharTemp    = nullptr;
static NimBLECharacteristic* pCharWifi    = nullptr;

static bool deviceConnected = false;
static uint8_t motorPosition = 0;

Preferences prefs;

// Callback: Verbindung auf-/abbauen
class ServerCallbacks : public NimBLEServerCallbacks {
  void onConnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo) override {
    deviceConnected = true;
    Serial.println("BLE: Client verbunden");
  }
  void onDisconnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo, int reason) override {
    deviceConnected = false;
    Serial.printf("BLE: Client getrennt (reason=%d), Advertising neu starten\n", reason);
    NimBLEDevice::startAdvertising();
  }
};

// Callback: Motor-Position geschrieben
class MotorCallbacks : public NimBLECharacteristicCallbacks {
  void onWrite(NimBLECharacteristic* pChar, NimBLEConnInfo& connInfo) override {
    std::string value = pChar->getValue();
    if (value.length() >= 1) {
      uint8_t pos = (uint8_t)value[0];
      if (pos > 100) pos = 100;
      motorPosition = pos;
      Serial.printf("Motor-Sollposition: %u %%\n", motorPosition);
      // TODO: Zielposition an Motor-Task weiterreichen
    }
  }
};

// Callback: WiFi-Credentials geschrieben (Format: "SSID\0PASSWORD")
class WifiCallbacks : public NimBLECharacteristicCallbacks {
  void onWrite(NimBLECharacteristic* pChar, NimBLEConnInfo& connInfo) override {
    std::string value = pChar->getValue();
    size_t nullPos = value.find('\0');
    if (nullPos == std::string::npos || nullPos == value.length() - 1) {
      Serial.println("WiFi: ungueltiges Format, erwarte SSID\\0PASSWORT");
      return;
    }
    std::string ssid = value.substr(0, nullPos);
    std::string pass = value.substr(nullPos + 1);

    prefs.begin("wifi", false);
    prefs.putString("ssid", ssid.c_str());
    prefs.putString("pass", pass.c_str());
    prefs.end();

    Serial.printf("WiFi-Credentials gespeichert: SSID='%s'\n", ssid.c_str());
    // TODO: WiFi-Task anstossen, neu zu verbinden
  }
};

void setup() {
  Serial.begin(115200);
  delay(200);
  Serial.println("\n=== BLE-Schrittmotor-Steuerung startet ===");

  // T-FPGA Spannungsprofil programmieren
  if (Axp2101PwrOn() != ESP_OK) {
    Serial.println("FEHLER: T-FPGA Initialisierung fehlgeschlagen!");
  }

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);

  // BLE initialisieren
  NimBLEDevice::init("ESP32-Stepper");
  NimBLEDevice::setPower(ESP_PWR_LVL_P9);

  pServer = NimBLEDevice::createServer();
  pServer->setCallbacks(new ServerCallbacks());

  NimBLEService* pService = pServer->createService(SERVICE_UUID);

  // Motor-Position: Write, uint8 (0..100)
  pCharMotor = pService->createCharacteristic(
      CHAR_MOTOR_UUID,
      NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_NR);
  pCharMotor->setCallbacks(new MotorCallbacks());
  uint8_t initMotor = 0;
  pCharMotor->setValue(&initMotor, 1);

  // Temperatur: Read + Notify, float (4 Byte, little-endian)
  pCharTemp = pService->createCharacteristic(
      CHAR_TEMP_UUID,
      NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY);
  float initTemp = 0.0f;
  pCharTemp->setValue((uint8_t*)&initTemp, sizeof(initTemp));

  // WiFi-Credentials: Write (SSID\0PASS)
  pCharWifi = pService->createCharacteristic(
      CHAR_WIFI_UUID,
      NIMBLE_PROPERTY::WRITE);
  pCharWifi->setCallbacks(new WifiCallbacks());

  pService->start();

  // Advertising konfigurieren
  NimBLEAdvertising* pAdv = NimBLEDevice::getAdvertising();
  pAdv->addServiceUUID(SERVICE_UUID);
  pAdv->setName("ESP32-Stepper");
  pAdv->enableScanResponse(true);
  NimBLEDevice::startAdvertising();

  Serial.println("BLE-Advertising laeuft, warte auf Client...");
}

void loop() {
  static uint32_t lastNotify = 0;
  uint32_t now = millis();

  // Dummy-Temperatur alle 1000 ms versenden (spaeter durch echten Sensor ersetzen)
  if (deviceConnected && (now - lastNotify >= 1000)) {
    lastNotify = now;
    float temp = 25.0f + (float)(now % 5000) / 1000.0f;  // Platzhalter
    pCharTemp->setValue((uint8_t*)&temp, sizeof(temp));
    pCharTemp->notify();
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
  }

  delay(10);
}