#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/ledc.h"
#include "esp_err.h"

#define SERVO_GPIO      2
#define SERVO_MIN_US    500     // 0.5ms
#define SERVO_MAX_US    2500    // 2.5ms
#define SERVO_FREQ_HZ   50      // 50Hz = 20ms chu kỳ

// Độ phân giải PWM 20ms với 16 bit
#define SERVO_TIMER_RESOLUTION LEDC_TIMER_16_BIT

// Tính duty từ micro giây
uint32_t servo_us_to_duty(uint32_t us, uint32_t freq_hz) {
    uint32_t max_duty = (1 << SERVO_TIMER_RESOLUTION) - 1;
    uint32_t period = 1000000 / freq_hz;
    return (us * max_duty) / period;
}

void app_main(void)
{
    // 1. Cấu hình Timer cho PWM
    ledc_timer_config_t timer = {
        .speed_mode       = LEDC_HIGH_SPEED_MODE,
        .duty_resolution  = SERVO_TIMER_RESOLUTION,
        .timer_num        = LEDC_TIMER_0,
        .freq_hz          = SERVO_FREQ_HZ,
        .clk_cfg          = LEDC_AUTO_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&timer));

    // 2. Cấu hình kênh PWM
    ledc_channel_config_t channel = {
        .speed_mode     = LEDC_HIGH_SPEED_MODE,
        .channel        = LEDC_CHANNEL_0,
        .timer_sel      = LEDC_TIMER_0,
        .intr_type      = LEDC_INTR_DISABLE,
        .gpio_num       = SERVO_GPIO,
        .duty           = 0,
        .hpoint         = 0
    };
    ESP_ERROR_CHECK(ledc_channel_config(&channel));

    // 3. Quay servo từ 0° -> 180° -> 0°
    while (1) {
        for (int angle = 0; angle <= 180; angle += 10) {
            // Map góc sang micro giây
            uint32_t pulse_us = SERVO_MIN_US + (angle * (SERVO_MAX_US - SERVO_MIN_US)) / 180;
            uint32_t duty = servo_us_to_duty(pulse_us, SERVO_FREQ_HZ);
            ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0, duty);
            ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0);
            vTaskDelay(pdMS_TO_TICKS(300));
        }
        for (int angle = 180; angle >= 0; angle -= 10) {
            uint32_t pulse_us = SERVO_MIN_US + (angle * (SERVO_MAX_US - SERVO_MIN_US)) / 180;
            uint32_t duty = servo_us_to_duty(pulse_us, SERVO_FREQ_HZ);
            ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0, duty);
            ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0);
            vTaskDelay(pdMS_TO_TICKS(300));
        }
    }
}