#ifndef Servo_ESP32_h
#define Servo_ESP32_h

#include <Arduino.h>

class Servo_ESP32 {

  private:
  
    uint8_t servoPin;
    uint8_t pwmChannel;
    int minPulseWidth;
    int maxPulseWidth;
    int currentAngle;
    const int freq = 50;  // 50Hz for standard servos
    const int resolution = 16;  // 16-bit resolution for smoother control
    bool _invert = false;
    uint8_t _min_pos = 0;
    uint8_t _max_pos = 180;
    uint8_t _init_pos = 90;
    uint8_t _prv_pos = 0;

    bool verify(uint8_t input) {
      if (input != _prv_pos) {
        _prv_pos = input;
        return true;
      }
      return false;
    }

  public:

    Servo_ESP32() {}

    Servo_ESP32(uint8_t pin, uint8_t channel, int minPulse = 500, int maxPulse = 2500) :
      servoPin(pin), pwmChannel(channel), minPulseWidth(minPulse), maxPulseWidth(maxPulse), currentAngle(0) {}

    void begin() {
      ledcAttachChannel(servoPin, freq, resolution, pwmChannel);
      write(_init_pos);
    }

    void setPin(uint8_t pin) {
      servoPin = pin;
    }

    void invert() {
      _invert = !_invert;
    }

    void setMin(uint8_t start) {
      _min_pos = start;
    }

    void setMax(uint8_t stop) {
      _max_pos = stop;
    }

    void setInit(uint8_t init) {
      _init_pos = init;
    }

    void goMax() {
      write(_max_pos);
    }

    void goMin() {
      write(_min_pos);
    }

    void goInit() {
      write(_init_pos);
    }

    void write(int angle) {
      if (!verify(angle)) return;

      currentAngle = _invert ? 180 - angle : angle;
      currentAngle = constrain(currentAngle, 0, 180);
      int dutyCycle = map(currentAngle, 0, 180,
                          minPulseWidth * (65536 / 20000),
                          maxPulseWidth * (65536 / 20000));
      ledcWrite(pwmChannel, dutyCycle);
    }

    int read() {
      return currentAngle;
    }

    void command(Stream &serial) {
      serial.print("Enter position value: ");
      while (!serial.available()) {}
      if (serial.available()) {
        String x = serial.readStringUntil('\n');
        while (serial.available()) {
          serial.read();
        }
        serial.println();
        x.trim();
        serial.println(x);
        write(x.toInt());
      }
    }

    void move(uint8_t pos) {
      pos = constrain(pos, _min_pos, _max_pos);
      write(pos);
    }

    void percent(uint8_t pos) {
      pos = map(pos, 0, 100, _min_pos, _max_pos);
      write(pos);
    }

    void sweep(uint8_t start, uint8_t stop, unsigned int mDelay) {
      if (start < stop) {
        for (int i = start; i <= stop; i++) {
          move(i);
          delay(mDelay);
        }
      } else if (start > stop) {
        for (int i = start; i >= stop; i--) {
          move(i);
          delay(mDelay);
        }
      }
    }

    void override(uint8_t pos) {
      write(pos);
    }

};

#endif