#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "freertos/timers.h"
#include "esp_log.h"
#include "esp_err.h"
#include "driver/gpio.h"

#include "socket_lock.h"
#include "board_config.h"

// NEW:
#include "storage_service.h"

#define NVS_NAMESPACE           "socket_lock"
#define NVS_OPERATING_TIME      "op_time"
#define NVS_BREAK_TIME          "break_time"
#define NVS_RETRY_COUNT         "retry_count"
#define NVS_DETECTION_HIGH      "detect_hi"

#define OPERATING_TIME_MIN      100
#define OPERATING_TIME_MAX      1000
#define LOCK_DELAY              500

#define LOCK_BIT                BIT0
#define UNLOCK_BIT              BIT1
#define REPEAT_LOCK_BIT         BIT2
#define REPEAT_UNLOCK_BIT       BIT3

static const char* TAG = "socket_lock";

// Storage timeouts (ajusta se quiseres)
#define STORE_TO        pdMS_TO_TICKS(800)
#define STORE_FLUSH_TO  pdMS_TO_TICKS(2000)

static uint16_t operating_time = 300;
static uint16_t break_time = 1000;
static bool detection_high = false;
static uint8_t retry_count = 5;

static socket_lock_status_t status;
static TaskHandle_t socket_lock_task;

// -----------------------------------------------------------------------------
// Helpers storage (best effort) - iguais ao estilo do wifi.c
// -----------------------------------------------------------------------------
static esp_err_t store_flush_best_effort(void)
{
    esp_err_t e = storage_flush_sync(STORE_FLUSH_TO);
    if (e != ESP_OK)
        ESP_LOGW(TAG, "storage_flush_sync failed: %s", esp_err_to_name(e));
    return e;
}

static esp_err_t store_set_u8_best_effort(const char *ns, const char *key, uint8_t v)
{
    for (int attempt = 0; attempt < 3; ++attempt)
    {
        esp_err_t e = storage_set_u8_async(ns, key, v);
        if (e == ESP_OK) return ESP_OK;

        if (e == ESP_ERR_TIMEOUT)
        {
            (void)store_flush_best_effort();
            vTaskDelay(pdMS_TO_TICKS(10));
            continue;
        }
        return e;
    }
    return ESP_ERR_TIMEOUT;
}

static esp_err_t store_set_u16_best_effort(const char *ns, const char *key, uint16_t v)
{
    for (int attempt = 0; attempt < 3; ++attempt)
    {
        esp_err_t e = storage_set_u16_async(ns, key, v);
        if (e == ESP_OK) return ESP_OK;

        if (e == ESP_ERR_TIMEOUT)
        {
            (void)store_flush_best_effort();
            vTaskDelay(pdMS_TO_TICKS(10));
            continue;
        }
        return e;
    }
    return ESP_ERR_TIMEOUT;
}

// -----------------------------------------------------------------------------
// Lock logic
// -----------------------------------------------------------------------------
static bool is_locked(void)
{
    gpio_set_level(board_config.socket_lock_a_gpio, 1);
    gpio_set_level(board_config.socket_lock_b_gpio, 1);

    vTaskDelay(pdMS_TO_TICKS(board_config.socket_lock_detection_delay));

    return gpio_get_level(board_config.socket_lock_detection_gpio) == detection_high;
}

bool socket_lock_is_locked_state(void)
{
    return is_locked();
}

static void socket_lock_task_func(void* param)
{
    (void)param;

    uint32_t notification;
    TickType_t previous_tick = 0;
    uint8_t attempt = 0;

    while (true)
    {
        if (xTaskNotifyWait(0x00, 0xff, &notification, portMAX_DELAY))
        {
            if (notification & (LOCK_BIT | UNLOCK_BIT))
            {
                attempt = retry_count;
            }

            if (notification & (UNLOCK_BIT | REPEAT_UNLOCK_BIT))
            {
                gpio_set_level(board_config.socket_lock_a_gpio, 0);
                gpio_set_level(board_config.socket_lock_b_gpio, 1);
                vTaskDelay(pdMS_TO_TICKS(operating_time));

                if (!is_locked())
                {
                    ESP_LOGI(TAG, "Unlock OK");
                    status = SOCKED_LOCK_STATUS_IDLE;
                }
                else
                {
                    if (attempt > 1)
                    {
                        ESP_LOGW(TAG, "Not unlocked yet, repeating...");
                        attempt--;
                        xTaskNotify(socket_lock_task, REPEAT_UNLOCK_BIT, eSetBits);
                    }
                    else
                    {
                        ESP_LOGE(TAG, "Not unlocked");
                        status = SOCKED_LOCK_STATUS_UNLOCKING_FAIL;
                    }
                }

                gpio_set_level(board_config.socket_lock_a_gpio, 0);
                gpio_set_level(board_config.socket_lock_b_gpio, 0);
            }
            else if (notification & (LOCK_BIT | REPEAT_LOCK_BIT))
            {
                if (notification & LOCK_BIT)
                {
                    vTaskDelay(pdMS_TO_TICKS(LOCK_DELAY)); // delay before first lock attempt
                }

                gpio_set_level(board_config.socket_lock_a_gpio, 1);
                gpio_set_level(board_config.socket_lock_b_gpio, 0);
                vTaskDelay(pdMS_TO_TICKS(operating_time));

                if (is_locked())
                {
                    ESP_LOGI(TAG, "Lock OK");
                    status = SOCKED_LOCK_STATUS_IDLE;
                }
                else
                {
                    if (attempt > 1)
                    {
                        ESP_LOGW(TAG, "Not locked yet, repeating...");
                        attempt--;
                        xTaskNotify(socket_lock_task, REPEAT_LOCK_BIT, eSetBits);
                    }
                    else
                    {
                        ESP_LOGE(TAG, "Not locked");
                        status = SOCKED_LOCK_STATUS_LOCKING_FAIL;
                    }
                }

                gpio_set_level(board_config.socket_lock_a_gpio, 0);
                gpio_set_level(board_config.socket_lock_b_gpio, 0);
            }

            TickType_t delay_tick = xTaskGetTickCount() - previous_tick;
            if (delay_tick < pdMS_TO_TICKS(break_time))
            {
                vTaskDelay(pdMS_TO_TICKS(break_time) - delay_tick);
            }
            previous_tick = xTaskGetTickCount();
        }
    }
}

// -----------------------------------------------------------------------------
// Init / API pública
// -----------------------------------------------------------------------------
void socket_lock_init(void)
{
    if (!board_config.socket_lock)
        return;

    // garante storage pronto
    esp_err_t se = storage_service_init();
    if (se != ESP_OK)
        ESP_LOGW(TAG, "storage_service_init failed: %s", esp_err_to_name(se));

    // Load config (best effort; se não existir, fica default)
    {
        uint16_t u16 = 0;
        uint8_t u8 = 0;

        esp_err_t e = storage_get_u16_sync(NVS_NAMESPACE, NVS_OPERATING_TIME, &u16, STORE_TO);
        if (e == ESP_OK) operating_time = u16;
        else if (e != ESP_ERR_NOT_FOUND)
            ESP_LOGW(TAG, "load %s failed: %s", NVS_OPERATING_TIME, esp_err_to_name(e));

        e = storage_get_u16_sync(NVS_NAMESPACE, NVS_BREAK_TIME, &u16, STORE_TO);
        if (e == ESP_OK) break_time = u16;
        else if (e != ESP_ERR_NOT_FOUND)
            ESP_LOGW(TAG, "load %s failed: %s", NVS_BREAK_TIME, esp_err_to_name(e));

        e = storage_get_u8_sync(NVS_NAMESPACE, NVS_RETRY_COUNT, &u8, STORE_TO);
        if (e == ESP_OK) retry_count = u8;
        else if (e != ESP_ERR_NOT_FOUND)
            ESP_LOGW(TAG, "load %s failed: %s", NVS_RETRY_COUNT, esp_err_to_name(e));

        e = storage_get_u8_sync(NVS_NAMESPACE, NVS_DETECTION_HIGH, &u8, STORE_TO);
        if (e == ESP_OK) detection_high = (u8 != 0);
        else if (e != ESP_ERR_NOT_FOUND)
            ESP_LOGW(TAG, "load %s failed: %s", NVS_DETECTION_HIGH, esp_err_to_name(e));
    }

    gpio_config_t io_conf = {};

    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pin_bit_mask = BIT64(board_config.socket_lock_a_gpio) | BIT64(board_config.socket_lock_b_gpio);
    ESP_ERROR_CHECK(gpio_config(&io_conf));

    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pin_bit_mask = BIT64(board_config.socket_lock_detection_gpio);
    ESP_ERROR_CHECK(gpio_config(&io_conf));

    xTaskCreate(socket_lock_task_func, "socket_lock_task", 2 * 1024, NULL, 2, &socket_lock_task);
}

bool socket_lock_is_detection_high(void)
{
    return detection_high;
}

void socket_lock_set_detection_high(bool _detection_high)
{
    detection_high = _detection_high;

    esp_err_t e = store_set_u8_best_effort(NVS_NAMESPACE, NVS_DETECTION_HIGH, detection_high ? 1 : 0);
    if (e != ESP_OK)
        ESP_LOGW(TAG, "persist detect_hi failed: %s", esp_err_to_name(e));

    (void)store_flush_best_effort();
}

uint16_t socket_lock_get_operating_time(void)
{
    return operating_time;
}

esp_err_t socket_lock_set_operating_time(uint16_t _operating_time)
{
    if (_operating_time < OPERATING_TIME_MIN || _operating_time > OPERATING_TIME_MAX)
    {
        ESP_LOGE(TAG, "Operating time out of range");
        return ESP_ERR_INVALID_ARG;
    }

    operating_time = _operating_time;

    esp_err_t e = store_set_u16_best_effort(NVS_NAMESPACE, NVS_OPERATING_TIME, operating_time);
    if (e != ESP_OK)
    {
        ESP_LOGW(TAG, "persist op_time failed: %s", esp_err_to_name(e));
        return e;
    }

    (void)store_flush_best_effort();
    return ESP_OK;
}

uint8_t socket_lock_get_retry_count(void)
{
    return retry_count;
}

void socket_lock_set_retry_count(uint8_t _retry_count)
{
    retry_count = _retry_count;

    esp_err_t e = store_set_u8_best_effort(NVS_NAMESPACE, NVS_RETRY_COUNT, retry_count);
    if (e != ESP_OK)
        ESP_LOGW(TAG, "persist retry_count failed: %s", esp_err_to_name(e));

    (void)store_flush_best_effort();
}

uint16_t socket_lock_get_break_time(void)
{
    return break_time;
}

esp_err_t socket_lock_set_break_time(uint16_t _break_time)
{
    if (_break_time < board_config.socket_lock_min_break_time)
    {
        ESP_LOGE(TAG, "Break time out of range");
        return ESP_ERR_INVALID_ARG;
    }

    break_time = _break_time;

    esp_err_t e = store_set_u16_best_effort(NVS_NAMESPACE, NVS_BREAK_TIME, break_time);
    if (e != ESP_OK)
    {
        ESP_LOGW(TAG, "persist break_time failed: %s", esp_err_to_name(e));
        return e;
    }

    (void)store_flush_best_effort();
    return ESP_OK;
}

void socket_lock_set_locked(bool locked)
{
    ESP_LOGI(TAG, "Set locked %d", locked);

    xTaskNotify(socket_lock_task, locked ? LOCK_BIT : UNLOCK_BIT, eSetBits);
    status = SOCKED_LOCK_STATUS_OPERATING;
}

socket_lock_status_t socket_lock_get_status(void)
{
    return status;
}
