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

#define SERVO_PIN GPIO_NUM_18
#define LEDC_CHANNEL LEDC_CHANNEL_0
#define LEDC_TIMER LEDC_TIMER_0
#define LEDC_MODE LEDC_LOW_SPEED_MODE
#define LEDC_FREQUENCY 50 // PWM

// 0~180
int wokwiServoAngleToDuty(int angle) {
    const int minDuty = (int)(0.023 * 65536);
    const int maxDuty = (int)(0.124 * 65536);
    return minDuty + (angle * (maxDuty - minDuty) / 180);
}

void moveServo(int angle) {
    int dutyCycle = wokwiServoAngleToDuty(angle);
    ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, dutyCycle);
    ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
}

void app_main(void) {
    ledc_timer_config_t ledc_timer = {
        .speed_mode = LEDC_MODE,
        .duty_resolution = LEDC_TIMER_16_BIT,
        .timer_num = LEDC_TIMER,
        .freq_hz = LEDC_FREQUENCY,
        .clk_cfg = LEDC_AUTO_CLK,
    };
    ledc_timer_config(&ledc_timer);

    ledc_channel_config_t ledc_channel = {
        .gpio_num = SERVO_PIN,
        .speed_mode = LEDC_MODE,
        .channel = LEDC_CHANNEL,
        .timer_sel = LEDC_TIMER,
        .duty = 0,
        .hpoint = 0
    };
    ledc_channel_config(&ledc_channel);

    while (1) {
        moveServo(0); // 0
        vTaskDelay(1000 / portTICK_PERIOD_MS); 
        moveServo(90); // 180
        vTaskDelay(1000 / portTICK_PERIOD_MS); 
        moveServo(180); // 180
        vTaskDelay(1000 / portTICK_PERIOD_MS);
        moveServo(90); // 180
        vTaskDelay(1000 / portTICK_PERIOD_MS);  
    }
}