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

#define BUTTON_GPIO 4
#define pinServo_forChannel_1 2

volatile bool is_running = false;  // Trạng thái servo

void init_button() {
    gpio_config_t button_config = {
        .pin_bit_mask = (1ULL << BUTTON_GPIO),  // Chọn GPIO 4
        .mode = GPIO_MODE_INPUT,                // Cấu hình làm đầu vào
        .pull_up_en = GPIO_PULLUP_ENABLE,       // Kích hoạt pull-up
        .pull_down_en = GPIO_PULLDOWN_DISABLE,  // Không dùng pull-down
        .intr_type = GPIO_INTR_DISABLE          // Không sử dụng ngắt
    };
    gpio_config(&button_config);
}

int buttonPress() {
    return gpio_get_level(BUTTON_GPIO);
}

void init_servo() {
    // Cấu hình timer LEDC
    ledc_timer_config_t Timer = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .timer_num = LEDC_TIMER_0,
        .duty_resolution = LEDC_TIMER_13_BIT,
        .freq_hz = 50,
        .clk_cfg = LEDC_AUTO_CLK
    };
    ledc_timer_config(&Timer);

    // Cấu hình kênh LEDC
    ledc_channel_config_t ledc_channel = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = LEDC_CHANNEL_1,
        .timer_sel = LEDC_TIMER_0,
        .intr_type = LEDC_INTR_FADE_END,  // Kích hoạt ngắt khi kết thúc fade
        .gpio_num = pinServo_forChannel_1,
        .duty = 0,
        .hpoint = 0
    };
    ledc_channel_config(&ledc_channel);

    // Đăng ký hàm ngắt
    ledc_isr_register(ledc_isr_callback, (void*) LEDC_CHANNEL_1, 0, NULL);
    
    // Đăng ký fade function
    ledc_fade_func_install(0);  // Cấp phát bộ nhớ cho interrupt
}

void IRAM_ATTR ledc_isr_callback(void *arg) {
    uint32_t channel = (uint32_t)arg;

    if (channel == LEDC_CHANNEL_1) {
        if (is_running) {
            // Servo hoàn thành hành động, quay lại vị trí cũ
            uint32_t duty = 200;
            ledc_set_fade_with_time(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1, duty, 1000);  // Quay lại với fade
            ledc_fade_start(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1, LEDC_FADE_NO_WAIT);
        } else {
            // Dừng servo
            ledc_stop(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1, 0);
        }
    }
}

void start_servo_action() {
    if (!is_running) {
        is_running = true;

        // Quay servo đến vị trí 410
        uint32_t duty = 410;
        ledc_set_fade_with_time(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1, duty, 1000);  // Fade đến duty trong 1 giây
        ledc_fade_start(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1, LEDC_FADE_NO_WAIT);
    }
}

void app_main() {
    init_button();  // Cấu hình nút nhấn
    init_servo();   // Cấu hình servo

    int last_button_state = 0;

    while (true) {
        int current_button_state = buttonPress();

        // Phát hiện nhấn nút
        if (current_button_state == 1 && last_button_state == 0) {
            start_servo_action();
        }

        last_button_state = current_button_state;
        vTaskDelay(100 / portTICK_PERIOD_MS);
    }
}