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


#define LED1 GPIO_NUM_13
#define LED2 GPIO_NUM_12
#define LED3 GPIO_NUM_14
#define LED4 GPIO_NUM_27
#define LED5 GPIO_NUM_26

#define BTN_GREEN GPIO_NUM_32
#define BTN_RED   GPIO_NUM_33

#define SERVO_PIN GPIO_NUM_25

static int leds[] = {LED1, LED2, LED3, LED4, LED5};
static int currentIndex = 0;

// Store state in flash
void save_state(int index) {
    nvs_handle_t handle;
    nvs_open("storage", NVS_READWRITE, &handle);
    nvs_set_i32(handle, "index", index);
    nvs_commit(handle);
    nvs_close(handle);
}

int load_state() {
    nvs_handle_t handle;
    int value = 0;
    nvs_open("storage", NVS_READONLY, &handle);
    nvs_get_i32(handle, "index", &value);
    nvs_close(handle);
    return value;
}

void set_led(int index) {
    for (int i=0; i<5; i++) {
        gpio_set_level(leds[i], i == index ? 1 : 0);
    }
}

void set_servo(int index) {
    int duty = 1638 + (index * 819); // map 0–4 to ~0.5ms–2.5ms pulse
    ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, duty);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
}

void app_main(void) {
    nvs_flash_init();

    // Init LEDs
    for (int i=0; i<5; i++) {
        gpio_reset_pin(leds[i]);
        gpio_set_direction(leds[i], GPIO_MODE_OUTPUT);
    }

    // Init buttons
    gpio_reset_pin(BTN_GREEN);
    gpio_set_direction(BTN_GREEN, GPIO_MODE_INPUT);
    gpio_set_pull_mode(BTN_GREEN, GPIO_PULLUP_ONLY);

    gpio_reset_pin(BTN_RED);
    gpio_set_direction(BTN_RED, GPIO_MODE_INPUT);
    gpio_set_pull_mode(BTN_RED, GPIO_PULLUP_ONLY);

    // Init servo with LEDC PWM
    ledc_timer_config_t timer_conf = {
        .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_conf);

    ledc_channel_config_t channel_conf = {
        .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(&channel_conf);

    // Load saved state
    currentIndex = load_state();
    set_led(currentIndex);
    set_servo(currentIndex);

    while (1) {
        if (gpio_get_level(BTN_GREEN) == 0) {
            currentIndex--;
            if (currentIndex < 0) currentIndex = 4;
            set_led(currentIndex);
            set_servo(currentIndex);
            save_state(currentIndex);
            vTaskDelay(300 / portTICK_PERIOD_MS);
        }
        if (gpio_get_level(BTN_RED) == 0) {
            currentIndex++;
            if (currentIndex > 4) currentIndex = 0;
            set_led(currentIndex);
            set_servo(currentIndex);
            save_state(currentIndex);
            vTaskDelay(300 / portTICK_PERIOD_MS);
        }
        vTaskDelay(50 / portTICK_PERIOD_MS);
    }
}