#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

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

//LED
#define LED_PIN 21   // adjust if needed for your board

// =========================
// PCA9685 Servo Setup
// =========================
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVO_CHANNEL 0   // LED0 output = channel 0
#define SERVO_MIN 90     // pulse length for minimum angle
#define SERVO_MAX 650     // pulse length for maximum angle

#define POS1 SERVO_MIN    // servo target position for '0'
#define POS2 SERVO_MAX    // servo target position for '1'

// =========================
// BLE UART UUIDs
// =========================
#define UART_SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
#define UART_RX_UUID      "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
#define UART_TX_UUID      "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"

BLECharacteristic *txChar;

// =========================
// BLE RX Callback
// =========================
class RXCallback : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *characteristic) {
    String val = characteristic->getValue();

    if (val.length() == 0) return;

    char c = val[0];
    Serial.print("BLE Received: ");
    Serial.println(c);

    if (c == '1') {
      // Move servo to position 2
      for (uint16_t pulselen = SERVO_MIN; pulselen < SERVO_MAX; pulselen++) {
        pwm.setPWM(SERVO_CHANNEL, 0, pulselen);
      }
      delay(500);
      Serial.println("Servo → Position 2");
      digitalWrite(LED_PIN, HIGH);
      Serial.println("LED ON");
      txChar->setValue("POS2, LED ON");
      txChar->notify();
    }
    else if (c == '0') {
      // Move servo to position 1
      for (uint16_t pulselen = SERVO_MAX; pulselen > SERVO_MIN; pulselen--) {
        pwm.setPWM(SERVO_CHANNEL, 0, pulselen);
      }
      delay(500);
      Serial.println("Servo → Position 1");
      digitalWrite(LED_PIN, LOW);
      Serial.println("LED OFF");      
      txChar->setValue("POS1, LED OFF");
      txChar->notify();
    }
  }
};

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

  //LED Setup
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);

  // -------------------------
  // PCA9685 Setup
  // -------------------------
  Serial.println("Initializing PCA9685...");
  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(50);  // Standard servo frequency (50 Hz)
  delay(10);

  // Move servo initially to POS1
  pwm.setPWM(SERVO_CHANNEL, 0, POS1);

  // -------------------------
  // BLE Setup
  // -------------------------
  Serial.println("Starting BLE...");
  BLEDevice::init("ESP32S3-Servo");
  BLEServer *server = BLEDevice::createServer();

  BLEService *uart = server->createService(UART_SERVICE_UUID);

  txChar = uart->createCharacteristic(
    UART_TX_UUID,
    BLECharacteristic::PROPERTY_NOTIFY
  );

  BLECharacteristic *rxChar = uart->createCharacteristic(
    UART_RX_UUID,
    BLECharacteristic::PROPERTY_WRITE
  );
  rxChar->setCallbacks(new RXCallback());

  uart->start();

  BLEAdvertising *adv = BLEDevice::getAdvertising();
  adv->addServiceUUID(UART_SERVICE_UUID);
  adv->start();

  Serial.println("BLE Ready — connect with Bluefruit LE Connect → UART.");
}

void loop() {
  // nothing required here
}