#include <Arduino.h>
#include "servo.h"

// ===== SERVO CONFIG =====
#define SERVO_PIN 27
#define SERVO_CH  4

const uint32_t servoFreq = 50;
const uint8_t  servoRes  = 16;

// PWM диапазон (у тебя уже рабочий)
const uint32_t SERVO_MIN = 1638;
const uint32_t SERVO_MAX = 8192;

// ===== OFFSET CALIBRATION =====
// ПОЛОЖИТЕЛЬНОЕ значение → вправо
// ОТРИЦАТЕЛЬНОЕ → влево
const int SERVO_OFFSET_DEG = -6;   // ← подбирается опытно

// логические пределы
const int SERVO_LOGICAL_MIN = 0;
const int SERVO_LOGICAL_MAX = 180;

static int currentAngle = 90;

// угол → duty
static uint32_t angleToDuty(int logicalAngle) {
  // применяем offset
  int calibratedAngle = logicalAngle + SERVO_OFFSET_DEG;

  // защита от выхода за пределы
  calibratedAngle = constrain(calibratedAngle,
                               SERVO_LOGICAL_MIN,
                               SERVO_LOGICAL_MAX);

  return map(calibratedAngle,
             SERVO_LOGICAL_MIN, SERVO_LOGICAL_MAX,
             SERVO_MIN, SERVO_MAX);
}

bool servoInit() {
  ledcSetup(SERVO_CH, servoFreq, servoRes);
  ledcAttachPin(SERVO_PIN, SERVO_CH);
  
  Serial.println("Servo initialized");
  Serial.printf("  PIN → GPIO%d\n", SERVO_PIN);
  Serial.printf("  CH  → %d\n", SERVO_CH);
  
  servoCenter();
  return true;
}

void servoCenter() {
  currentAngle = 90;
  ledcWrite(SERVO_CH, angleToDuty(currentAngle));
}

void servoSetAngle(int angle) {
  currentAngle = constrain(angle, SERVO_LOGICAL_MIN, SERVO_LOGICAL_MAX);
  ledcWrite(SERVO_CH, angleToDuty(currentAngle));
}

int servoGetAngle() {
  return currentAngle;
}