#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <Servo.h>

Servo myServo;
BLECharacteristic *pCharacteristic;
bool deviceConnected = false;
int position = 90; // Initial position of the servo

class MyServerCallbacks : public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      deviceConnected = true;
    };

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
    }
};

void setup() {
  Serial.begin(115200);
  BLEDevice::init("ESP32-C3 Servo Control");
  BLEServer *pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  BLEService *pService = pServer->createService(BLEUUID((uint16_t)0x180D));

  pCharacteristic = pService->createCharacteristic(
                      BLEUUID((uint16_t)0x2A37),
                      BLECharacteristic::PROPERTY_READ |
                      BLECharacteristic::PROPERTY_WRITE
                    );

  pService->start();
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(BLEUUID((uint16_t)0x180D));
  pAdvertising->setScanResponse(true);
  pAdvertising->start();

  myServo.attach(6); // Servo signal pin on GPIO 6 (pin 6)
}

void loop() {
  if (deviceConnected) {
    if (pCharacteristic->canRead()) {
      pCharacteristic->setValue((uint8_t*)&position, 1);
      pCharacteristic->notify();
    }
    if (pCharacteristic->canWrite()) {
      uint8_t *data = pCharacteristic->getData();
      position = data[0];
      position = map(position, 0, 180, 0, 180); // Map the received value to servo's range (0-180 degrees)
      myServo.write(position);
      Serial.print("Received Position: ");
      Serial.println(position);
    }
    delay(10); // Small delay
  }
}