/**
 * @file    servo.c
 * @brief   180° PWM 舵机控制实现 (LEDC, 50Hz, 0.5~2.5ms)
 */

#include "servo.h"
#include "driver/ledc.h"
#include "esp_log.h"

#define SERVO_FREQ_HZ     50
#define SERVO_TIMER       LEDC_TIMER_0
#define SERVO_MODE        LEDC_LOW_SPEED_MODE
#define SERVO_DUTY_RES    LEDC_TIMER_16_BIT   /* 0~65535 */
#define SERVO_MAX_DUTY    65535

/* 脉宽范围 (微秒) */
#define PULSE_MIN_US  500
#define PULSE_MAX_US  2500
#define PERIOD_US     20000  /* 50Hz = 20ms */

static const char *TAG = "SERVO";
static ledc_channel_t s_channel;
static float s_limit_up   = 180.0f;
static float s_limit_down = 0.0f;

/* 角度 → 占空比 */
static uint32_t angle_to_duty(float angle)
{
    if (angle < 0.0f)   angle = 0.0f;
    if (angle > 180.0f) angle = 180.0f;

    float pulse_us = PULSE_MIN_US + (angle / 180.0f) * (PULSE_MAX_US - PULSE_MIN_US);
    return (uint32_t)((pulse_us / PERIOD_US) * SERVO_MAX_DUTY);
}

void servo_init(int gpio)
{
    /* 分配 LEDC 通道 */
    static int chan_idx = 0;
    s_channel = (chan_idx++ == 0) ? LEDC_CHANNEL_0 : LEDC_CHANNEL_1;

    ledc_timer_config_t timer_cfg = {
        .speed_mode      = SERVO_MODE,
        .duty_resolution = SERVO_DUTY_RES,
        .timer_num       = SERVO_TIMER,
        .freq_hz         = SERVO_FREQ_HZ,
        .clk_cfg         = LEDC_AUTO_CLK,
    };
    ESP_ERROR_CHECK(ledc_timer_config(&timer_cfg));

    ledc_channel_config_t chan_cfg = {
        .gpio_num   = gpio,
        .speed_mode = SERVO_MODE,
        .channel    = s_channel,
        .intr_type  = LEDC_INTR_DISABLE,
        .timer_sel  = SERVO_TIMER,
        .duty       = angle_to_duty(0),  /* 初始: 0° */
        .hpoint     = 0,
        .flags.output_invert = 0,
    };
    ESP_ERROR_CHECK(ledc_channel_config(&chan_cfg));

    ESP_LOGI(TAG, "初始化完成 (GPIO%d, 50Hz, 通道%d)", gpio, s_channel);
}

void servo_set_angle(float angle)
{
    uint32_t duty = angle_to_duty(angle);
    ESP_ERROR_CHECK(ledc_set_duty(SERVO_MODE, s_channel, duty));
    ESP_ERROR_CHECK(ledc_update_duty(SERVO_MODE, s_channel));
    ESP_LOGD(TAG, "角度: %.1f° (duty=%lu)", angle, duty);
}

void servo_set_up(void)
{
    servo_set_angle(s_limit_up);
}

void servo_set_down(void)
{
    servo_set_angle(s_limit_down);
}

void servo_set_limits(float up, float down)
{
    s_limit_up   = up;
    s_limit_down = down;
    ESP_LOGI(TAG, "限位: 上=%.0f° 下=%.0f°", up, down);
}