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

#define SERVO_PIN 14
#define SERVO_CHANNEL 0
#define PWM_FREQ_HZ 50
#define PWM_RESOLUTION 14
#define MIN_PULSE_WIDTH_US 500
#define MAX_PULSE_WIDTH_US 2500
// #define MIN_PULSE_WIDTH_US 410
// #define MAX_PULSE_WIDTH_US 2038

void setup_servo() {
    ledc_timer_config_t timer_conf = {
        .duty_resolution = PWM_RESOLUTION,
        .freq_hz = PWM_FREQ_HZ,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .timer_num = LEDC_TIMER_0
    };
    ledc_timer_config(&timer_conf);

    ledc_channel_config_t ledc_conf = {
        .gpio_num = SERVO_PIN,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = SERVO_CHANNEL,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = 0,
        .hpoint = 0
    };
    ledc_channel_config(&ledc_conf);
}

void write_servo_angle(int angle) {
    int duty = ((angle - 0) * (MAX_PULSE_WIDTH_US - MIN_PULSE_WIDTH_US)) / (180 - 0) + MIN_PULSE_WIDTH_US;
    printf("duty:%d\n", duty);
    ledc_set_duty(LEDC_LOW_SPEED_MODE, SERVO_CHANNEL, duty);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, SERVO_CHANNEL);
}

void loop_direct() {
    write_servo_angle(0);
    vTaskDelay(1000 / portTICK_PERIOD_MS);

    write_servo_angle(90);
    vTaskDelay(1000 / portTICK_PERIOD_MS);

    write_servo_angle(180);
    vTaskDelay(1000 / portTICK_PERIOD_MS);
}

void app_main() {
    printf("Hello, ESP32-S2! \n");
    setup_servo();
    
    while (1) {
        loop_direct();
    }
}