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

#define SERVO_PIN1       GPIO_NUM_2
#define SERVO_PIN2       GPIO_NUM_12
#define SERVO_MIN_PULSEWIDTH_US   200
#define SERVO_MAX_PULSEWIDTH_US 1250
#define SERVO_MAX_DEGREE   360

void servo_control_task(void *pvParameter) {
    ledc_timer_config_t ledc_timer = {
        .duty_resolution = LEDC_TIMER_13_BIT,
        .freq_hz = 50,
        .speed_mode = LEDC_HIGH_SPEED_MODE,
        .timer_num = LEDC_TIMER_0
    };
    ledc_timer_config(&ledc_timer);

    ledc_channel_config_t ledc_channel1 = {
        .channel    = LEDC_CHANNEL_0,
        .duty       = 0,
        .gpio_num   = SERVO_PIN1,
        .speed_mode = LEDC_HIGH_SPEED_MODE,
        .timer_sel  = LEDC_TIMER_0
    };
    ledc_channel_config(&ledc_channel1);

    ledc_channel_config_t ledc_channel2 = {
        .channel    = LEDC_CHANNEL_0,
        .duty       = 0,
        .gpio_num   = SERVO_PIN2,
        .speed_mode = LEDC_HIGH_SPEED_MODE,
        .timer_sel  = LEDC_TIMER_0
    };
    ledc_channel_config(&ledc_channel2);

    while (1) {
        // Sweep from 0 degree to 180 degree
        for (int i = 0; i <= SERVO_MAX_DEGREE; i+=10) {
            int duty_cycle = SERVO_MIN_PULSEWIDTH_US + ((SERVO_MAX_PULSEWIDTH_US - SERVO_MIN_PULSEWIDTH_US) / SERVO_MAX_DEGREE) * i;
            ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0, duty_cycle);
            ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0);
            vTaskDelay(250/ portTICK_PERIOD_MS);
        }

        // Sweep from 180 degree to 0 degree
        for (int i = SERVO_MAX_DEGREE; i >= 0; i-=10) {
            int duty_cycle = SERVO_MIN_PULSEWIDTH_US + ((SERVO_MAX_PULSEWIDTH_US - SERVO_MIN_PULSEWIDTH_US) / SERVO_MAX_DEGREE) * i;
            ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0, duty_cycle);
            ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0);
            vTaskDelay(250 / portTICK_PERIOD_MS);
        }
    }
}

void app_main() {
    xTaskCreate(&servo_control_task, "servo_control_task", configMINIMAL_STACK_SIZE * 4, NULL, 5, NULL);
}