#include "driver/gpio.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/ledc.h"  // Include the LEDC library for PWM control

// LED Pins
#define LED_PINS {23, 22, 21, 19, 18}  // LED pin list
#define BUTTON_PIN1 33
#define BUTTON_PIN2 32

// Array of LED pin numbers
int led_pins[] = LED_PINS;

// Servo Pin
#define SERVO_PIN 17  // Servo control pin

// Function to move the servo to a specified angle (0–180)
void move_servo_to_angle(int angle) {
    // Servo duty range: ~500 (0°) to ~2500 (180°) for 50Hz PWM
    int duty = (angle * (2500 - 500) / 180) + 500;

    ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, duty);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
}

// LED + Servo sweep function
void sweep_leds_with_servo(int direction) {
    int start = (direction == 1) ? 0 : 4;   // Left-to-right or right-to-left
    int end = (direction == 1) ? 5 : -1;
    int step = (direction == 1) ? 1 : -1;

    for (int i = start; i != end; i += step) {
        // Turn on the LED
        gpio_set_level(led_pins[i], 1);

        // Map LED index (0–4) to servo angle (0–180)
        int angle = (i * 180) / 4;  
        move_servo_to_angle(angle);

        vTaskDelay(500 / portTICK_PERIOD_MS);  // Delay between steps
    }
}

// Turn off all LEDs
void turn_off_leds() {
    for (int i = 0; i < 5; i++) {
        gpio_set_level(led_pins[i], 0);
    }
}

void setup() {
    // Set LED pins as output
    for (int i = 0; i < 5; i++) {
        gpio_set_direction(led_pins[i], GPIO_MODE_OUTPUT);
    }

    // Set button pins as input
    gpio_set_direction(BUTTON_PIN1, GPIO_MODE_INPUT);
    gpio_set_direction(BUTTON_PIN2, GPIO_MODE_INPUT);

    gpio_pullup_en(BUTTON_PIN1);
    gpio_pullup_en(BUTTON_PIN2);

    // Configure LEDC Timer for Servo (PWM control)
    ledc_timer_config_t ledc_timer = {
        .duty_resolution = LEDC_TIMER_13_BIT,  // Resolution of PWM duty (8192 levels)
        .freq_hz = 50,                        // Servo frequency (50Hz)
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .timer_num = LEDC_TIMER_0,
    };
    ledc_timer_config(&ledc_timer);

    // Configure LEDC Channel for Servo
    ledc_channel_config_t ledc_channel = {
        .gpio_num = SERVO_PIN,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = LEDC_CHANNEL_0,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = 0,  
        .hpoint = 0,
    };
    ledc_channel_config(&ledc_channel);
}

void loop() {
    int button_state1 = gpio_get_level(BUTTON_PIN1);
    int button_state2 = gpio_get_level(BUTTON_PIN2);

    if (button_state1 == 0) {  // Button 1 pressed
        sweep_leds_with_servo(1);  // Left to right (0° → 180°)
        turn_off_leds();
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }

    if (button_state2 == 0) {  // Button 2 pressed
        sweep_leds_with_servo(-1);  // Right to left (180° → 0°)
        turn_off_leds();
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }

    vTaskDelay(100 / portTICK_PERIOD_MS);  // Debounce
}

void app_main() {
    setup();
    while (1) {
        loop();
    }
}