#include <string.h>
#include <sys/time.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/timers.h"
#include "freertos/semphr.h"
#include "driver/gpio.h"
#include "driver/ledc.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "nvs_flash.h"
#include "nvs.h"
#include "motor_control.h"

static const char *TAG = "MOTOR_CONTROL";

// Motor control structure
typedef struct {
    motor_state_t state;
    motor_dir_t direction;
    uint8_t speed_percent;
    uint8_t target_speed;
    uint32_t max_runtime_ms;
    uint32_t min_interval_ms;
    uint8_t min_speed_percent;
    uint8_t max_speed_percent;
    
    // Runtime tracking
    int64_t start_time;
    int64_t last_stop_time;
    TimerHandle_t safety_timer;
    TimerHandle_t soft_start_timer;
    
    // Statistics
    motor_stats_t stats;
    
    // GPIO pins
    gpio_num_t in1_gpio;
    gpio_num_t in2_gpio;
    ledc_channel_t pwm_channel;
} motor_t;

// Global state
static motor_t s_motors[MOTOR_PUMP_MAX];
static SemaphoreHandle_t s_motor_mutex = NULL;
static bool s_initialized = false;

// Callbacks
static motor_state_callback_t s_state_callback = NULL;
static motor_error_callback_t s_error_callback = NULL;

// NVS namespace
#define MOTOR_NVS_NAMESPACE "motor_stats"

// Forward declarations
static esp_err_t motor_set_direction(motor_id_t id, motor_dir_t dir);
static esp_err_t motor_update_pwm(motor_id_t id, uint8_t duty);
static void motor_safety_timer_callback(TimerHandle_t xTimer);
static void motor_soft_start_timer_callback(TimerHandle_t xTimer);
static esp_err_t motor_save_stats(motor_id_t id);
static esp_err_t motor_load_stats(motor_id_t id);
static void motor_update_state(motor_id_t id, motor_state_t new_state);

// Utility functions
static int64_t get_time_ms(void)
{
    return esp_timer_get_time() / 1000;
}

static bool is_valid_motor_id(motor_id_t id)
{
    return (id == MOTOR_PUMP_1 || id == MOTOR_PUMP_2);
}

esp_err_t motor_control_init(void)
{
    if (s_initialized) {
        ESP_LOGW(TAG, "Motor control already initialized");
        return ESP_OK;
    }
    
    esp_err_t ret = ESP_OK;
    
    // Create mutex
    s_motor_mutex = xSemaphoreCreateMutex();
    if (s_motor_mutex == NULL) {
        ESP_LOGE(TAG, "Failed to create mutex");
        return ESP_ERR_NO_MEM;
    }
    
    // Configure standby pin (active low, so HIGH = enabled)
    gpio_config_t io_conf = {
        .mode = GPIO_MODE_OUTPUT,
        .pin_bit_mask = (1ULL << MOTOR_STBY_GPIO),
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .pull_up_en = GPIO_PULLUP_DISABLE,
    };
    ret = gpio_config(&io_conf);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to configure STBY pin");
        goto error;
    }
    
    // Disable motors initially
    gpio_set_level(MOTOR_STBY_GPIO, 0);
    
    // Configure direction pins for both motors
    io_conf.pin_bit_mask = (1ULL << MOTOR_AIN1_GPIO) | (1ULL << MOTOR_AIN2_GPIO) |
                          (1ULL << MOTOR_BIN1_GPIO) | (1ULL << MOTOR_BIN2_GPIO);
    ret = gpio_config(&io_conf);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to configure direction pins");
        goto error;
    }
    
    // Configure LEDC for PWM
    ledc_timer_config_t ledc_timer = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .timer_num = LEDC_TIMER_0,
        .duty_resolution = MOTOR_PWM_RESOLUTION,
        .freq_hz = MOTOR_PWM_FREQ_HZ,
        .clk_cfg = LEDC_AUTO_CLK
    };
    ret = ledc_timer_config(&ledc_timer);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to configure LEDC timer");
        goto error;
    }
    
    // Configure PWM channels
    ledc_channel_config_t ledc_channel[2] = {
        {
            .speed_mode = LEDC_LOW_SPEED_MODE,
            .channel = LEDC_CHANNEL_0,
            .timer_sel = LEDC_TIMER_0,
            .gpio_num = MOTOR_PWMA_GPIO,
            .duty = 0,
            .hpoint = 0
        },
        {
            .speed_mode = LEDC_LOW_SPEED_MODE,
            .channel = LEDC_CHANNEL_1,
            .timer_sel = LEDC_TIMER_0,
            .gpio_num = MOTOR_PWMB_GPIO,
            .duty = 0,
            .hpoint = 0
        }
    };
    
    for (int i = 0; i < 2; i++) {
        ret = ledc_channel_config(&ledc_channel[i]);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Failed to configure LEDC channel %d", i);
            goto error;
        }
    }
    
    // Initialize motor structures
    memset(s_motors, 0, sizeof(s_motors));
    
    // Motor 1 (Pump 1)
    s_motors[0].in1_gpio = MOTOR_AIN1_GPIO;
    s_motors[0].in2_gpio = MOTOR_AIN2_GPIO;
    s_motors[0].pwm_channel = LEDC_CHANNEL_0;
    s_motors[0].max_runtime_ms = MOTOR_MAX_RUNTIME_MS;
    s_motors[0].min_interval_ms = MOTOR_MIN_INTERVAL_MS;
    s_motors[0].min_speed_percent = MOTOR_MIN_SPEED;
    s_motors[0].max_speed_percent = 100;
    s_motors[0].state = MOTOR_STATE_STOPPED;
    s_motors[0].direction = MOTOR_DIR_FORWARD;
    
    // Motor 2 (Pump 2)
    s_motors[1].in1_gpio = MOTOR_BIN1_GPIO;
    s_motors[1].in2_gpio = MOTOR_BIN2_GPIO;
    s_motors[1].pwm_channel = LEDC_CHANNEL_1;
    s_motors[1].max_runtime_ms = MOTOR_MAX_RUNTIME_MS;
    s_motors[1].min_interval_ms = MOTOR_MIN_INTERVAL_MS;
    s_motors[1].min_speed_percent = MOTOR_MIN_SPEED;
    s_motors[1].max_speed_percent = 100;
    s_motors[1].state = MOTOR_STATE_STOPPED;
    s_motors[1].direction = MOTOR_DIR_FORWARD;
    
    // Create safety timers
    for (int i = 0; i < MOTOR_PUMP_MAX - 1; i++) {
        char timer_name[32];
        snprintf(timer_name, sizeof(timer_name), "motor_safety_%d", i + 1);
        
        s_motors[i].safety_timer = xTimerCreate(timer_name,
                                               pdMS_TO_TICKS(1000),
                                               pdFALSE,
                                               (void*)(i + 1),
                                               motor_safety_timer_callback);
        if (s_motors[i].safety_timer == NULL) {
            ESP_LOGE(TAG, "Failed to create safety timer for motor %d", i + 1);
            ret = ESP_ERR_NO_MEM;
            goto error;
        }
        
        snprintf(timer_name, sizeof(timer_name), "motor_soft_%d", i + 1);
        s_motors[i].soft_start_timer = xTimerCreate(timer_name,
                                                   pdMS_TO_TICKS(50),
                                                   pdTRUE,
                                                   (void*)(i + 1),
                                                   motor_soft_start_timer_callback);
        if (s_motors[i].soft_start_timer == NULL) {
            ESP_LOGE(TAG, "Failed to create soft start timer for motor %d", i + 1);
            ret = ESP_ERR_NO_MEM;
            goto error;
        }
    }
    
    // Load statistics from NVS
    for (int i = 0; i < MOTOR_PUMP_MAX - 1; i++) {
        motor_load_stats(i + 1);
    }
    
    // Enable motor driver
    gpio_set_level(MOTOR_STBY_GPIO, 1);
    
    s_initialized = true;
    ESP_LOGI(TAG, "Motor control initialized successfully");
    return ESP_OK;
    
error:
    if (s_motor_mutex) {
        vSemaphoreDelete(s_motor_mutex);
        s_motor_mutex = NULL;
    }
    return ret;
}

esp_err_t motor_control_deinit(void)
{
    if (!s_initialized) {
        return ESP_OK;
    }
    
    // Stop all motors
    motor_stop_all();
    
    // Disable motor driver
    gpio_set_level(MOTOR_STBY_GPIO, 0);
    
    // Delete timers
    for (int i = 0; i < MOTOR_PUMP_MAX - 1; i++) {
        if (s_motors[i].safety_timer) {
            xTimerDelete(s_motors[i].safety_timer, 0);
        }
        if (s_motors[i].soft_start_timer) {
            xTimerDelete(s_motors[i].soft_start_timer, 0);
        }
    }
    
    // Delete mutex
    if (s_motor_mutex) {
        vSemaphoreDelete(s_motor_mutex);
        s_motor_mutex = NULL;
    }
    
    s_initialized = false;
    ESP_LOGI(TAG, "Motor control deinitialized");
    return ESP_OK;
}

static esp_err_t motor_set_direction(motor_id_t id, motor_dir_t dir)
{
    if (!is_valid_motor_id(id)) {
        return ESP_ERR_INVALID_ARG;
    }
    
    motor_t *motor = &s_motors[id - 1];
    
    if (dir == MOTOR_DIR_FORWARD) {
        gpio_set_level(motor->in1_gpio, 1);
        gpio_set_level(motor->in2_gpio, 0);
    } else {
        gpio_set_level(motor->in1_gpio, 0);
        gpio_set_level(motor->in2_gpio, 1);
    }
    
    motor->direction = dir;
    return ESP_OK;
}

static esp_err_t motor_update_pwm(motor_id_t id, uint8_t duty)
{
    if (!is_valid_motor_id(id)) {
        return ESP_ERR_INVALID_ARG;
    }
    
    motor_t *motor = &s_motors[id - 1];
    
    esp_err_t ret = ledc_set_duty(LEDC_LOW_SPEED_MODE, motor->pwm_channel, duty);
    if (ret != ESP_OK) {
        return ret;
    }
    
    return ledc_update_duty(LEDC_LOW_SPEED_MODE, motor->pwm_channel);
}

static void motor_update_state(motor_id_t id, motor_state_t new_state)
{
    motor_t *motor = &s_motors[id - 1];
    motor_state_t old_state = motor->state;
    
    motor->state = new_state;
    
    if (old_state != new_state && s_state_callback) {
        s_state_callback(id, new_state);
    }
}

esp_err_t motor_start(motor_id_t id, uint8_t speed_percent)
{
    if (!s_initialized) {
        return ESP_ERR_INVALID_STATE;
    }
    
    if (!is_valid_motor_id(id)) {
        return ESP_ERR_INVALID_ARG;
    }
    
    xSemaphoreTake(s_motor_mutex, portMAX_DELAY);
    
    motor_t *motor = &s_motors[id - 1];
    
    // Check if already running
    if (motor->state == MOTOR_STATE_RUNNING) {
        xSemaphoreGive(s_motor_mutex);
        ESP_LOGW(TAG, "Motor %d already running", id);
        return ESP_OK;
    }
    
    // Check cooldown period
    int64_t now = get_time_ms();
    if (motor->last_stop_time > 0) {
        int64_t elapsed = now - motor->last_stop_time;
        if (elapsed < motor->min_interval_ms) {
            motor_update_state(id, MOTOR_STATE_COOLDOWN);
            xSemaphoreGive(s_motor_mutex);
            if (s_error_callback) {
                s_error_callback(id, "Cooldown period not elapsed");
            }
            ESP_LOGW(TAG, "Motor %d in cooldown, %lld ms remaining", 
                    id, motor->min_interval_ms - elapsed);
            return ESP_ERR_INVALID_STATE;
        }
    }
    
    // Clamp speed to configured limits
    if (speed_percent < motor->min_speed_percent) {
        speed_percent = motor->min_speed_percent;
    }
    if (speed_percent > motor->max_speed_percent) {
        speed_percent = motor->max_speed_percent;
    }
    
    // Set direction
    motor_set_direction(id, motor->direction);
    
    // Store target speed for soft start
    motor->target_speed = speed_percent;
    motor->speed_percent = 0;
    
    // Start with 0 PWM for soft start
    motor_update_pwm(id, 0);
    
    // Record start time
    motor->start_time = now;
    motor_update_state(id, MOTOR_STATE_RUNNING);
    
    // Start soft start timer
    xTimerStart(motor->soft_start_timer, 0);
    
    // Start safety timer
    xTimerChangePeriod(motor->safety_timer, pdMS_TO_TICKS(motor->max_runtime_ms), 0);
    xTimerStart(motor->safety_timer, 0);
    
    xSemaphoreGive(s_motor_mutex);
    
    ESP_LOGI(TAG, "Motor %d started at %d%% speed", id, speed_percent);
    return ESP_OK;
}

esp_err_t motor_stop(motor_id_t id)
{
    if (!s_initialized) {
        return ESP_ERR_INVALID_STATE;
    }
    
    if (!is_valid_motor_id(id)) {
        return ESP_ERR_INVALID_ARG;
    }
    
    xSemaphoreTake(s_motor_mutex, portMAX_DELAY);
    
    motor_t *motor = &s_motors[id - 1];
    
    // Stop timers
    xTimerStop(motor->safety_timer, 0);
    xTimerStop(motor->soft_start_timer, 0);
    
    // Set PWM to 0
    motor_update_pwm(id, 0);
    motor->speed_percent = 0;
    
    // Update runtime statistics
    if (motor->state == MOTOR_STATE_RUNNING) {
        int64_t runtime = get_time_ms() - motor->start_time;
        motor->stats.last_run_duration_ms = runtime;
        motor->stats.total_runtime_ms += runtime;
        motor->stats.last_run_timestamp = motor->start_time;
        motor->stats.run_count++;
        
        // Save stats to NVS periodically (every 10 runs)
        if (motor->stats.run_count % 10 == 0) {
            motor_save_stats(id);
        }
    }
    
    motor->last_stop_time = get_time_ms();
    motor_update_state(id, MOTOR_STATE_STOPPED);
    
    xSemaphoreGive(s_motor_mutex);
    
    ESP_LOGI(TAG, "Motor %d stopped", id);
    return ESP_OK;
}

esp_err_t motor_stop_all(void)
{
    esp_err_t ret = ESP_OK;
    
    for (motor_id_t id = MOTOR_PUMP_1; id < MOTOR_PUMP_MAX; id++) {
        esp_err_t err = motor_stop(id);
        if (err != ESP_OK) {
            ret = err;
        }
    }
    
    return ret;
}

esp_err_t motor_emergency_stop(void)
{
    if (!s_initialized) {
        return ESP_ERR_INVALID_STATE;
    }
    
    // Disable motor driver immediately
    gpio_set_level(MOTOR_STBY_GPIO, 0);
    
    // Stop all motors
    motor_stop_all();
    
    // Re-enable motor driver
    gpio_set_level(MOTOR_STBY_GPIO, 1);
    
    ESP_LOGW(TAG, "Emergency stop executed");
    return ESP_OK;
}

esp_err_t motor_start_timed(motor_id_t id, uint8_t speed_percent, uint32_t duration_ms)
{
    if (duration_ms > MOTOR_MAX_RUNTIME_MS) {
        duration_ms = MOTOR_MAX_RUNTIME_MS;
    }
    
    esp_err_t ret = motor_start(id, speed_percent);
    if (ret != ESP_OK) {
        return ret;
    }
    
    // Override the safety timer with the requested duration
    xSemaphoreTake(s_motor_mutex, portMAX_DELAY);
    xTimerChangePeriod(s_motors[id - 1].safety_timer, pdMS_TO_TICKS(duration_ms), 0);
    xSemaphoreGive(s_motor_mutex);
    
    return ESP_OK;
}

esp_err_t motor_set_speed(motor_id_t id, uint8_t speed_percent)
{
    if (!s_initialized) {
        return ESP_ERR_INVALID_STATE;
    }
    
    if (!is_valid_motor_id(id)) {
        return ESP_ERR_INVALID_ARG;
    }
    
    xSemaphoreTake(s_motor_mutex, portMAX_DELAY);
    
    motor_t *motor = &s_motors[id - 1];
    
    if (motor->state != MOTOR_STATE_RUNNING) {
        xSemaphoreGive(s_motor_mutex);
        return ESP_ERR_INVALID_STATE;
    }
    
    // Clamp speed
    if (speed_percent < motor->min_speed_percent) {
        speed_percent = motor->min_speed_percent;
    }
    if (speed_percent > motor->max_speed_percent) {
        speed_percent = motor->max_speed_percent;
    }
    
    motor->speed_percent = speed_percent;
    motor->target_speed = speed_percent;
    
    uint8_t duty = (speed_percent * MOTOR_PWM_MAX_DUTY) / 100;
    motor_update_pwm(id, duty);
    
    xSemaphoreGive(s_motor_mutex);
    
    return ESP_OK;
}

motor_state_t motor_get_state(motor_id_t id)
{
    if (!is_valid_motor_id(id)) {
        return MOTOR_STATE_ERROR;
    }
    
    return s_motors[id - 1].state;
}

bool motor_is_running(motor_id_t id)
{
    return motor_get_state(id) == MOTOR_STATE_RUNNING;
}

bool motor_is_cooldown(motor_id_t id)
{
    if (!is_valid_motor_id(id)) {
        return false;
    }
    
    motor_t *motor = &s_motors[id - 1];
    
    if (motor->last_stop_time == 0) {
        return false;
    }
    
    int64_t elapsed = get_time_ms() - motor->last_stop_time;
    return elapsed < motor->min_interval_ms;
}

uint32_t motor_get_runtime_ms(motor_id_t id)
{
    if (!is_valid_motor_id(id)) {
        return 0;
    }
    
    motor_t *motor = &s_motors[id - 1];
    
    if (motor->state == MOTOR_STATE_RUNNING) {
        return get_time_ms() - motor->start_time;
    }
    
    return 0;
}

esp_err_t motor_get_stats(motor_id_t id, motor_stats_t *stats)
{
    if (!is_valid_motor_id(id) || stats == NULL) {
        return ESP_ERR_INVALID_ARG;
    }
    
    xSemaphoreTake(s_motor_mutex, portMAX_DELAY);
    memcpy(stats, &s_motors[id - 1].stats, sizeof(motor_stats_t));
    xSemaphoreGive(s_motor_mutex);
    
    return ESP_OK;
}

esp_err_t motor_set_max_runtime(motor_id_t id, uint32_t max_runtime_ms)
{
    if (!is_valid_motor_id(id)) {
        return ESP_ERR_INVALID_ARG;
    }
    
    s_motors[id - 1].max_runtime_ms = max_runtime_ms;
    return ESP_OK;
}

esp_err_t motor_set_min_interval(motor_id_t id, uint32_t min_interval_ms)
{
    if (!is_valid_motor_id(id)) {
        return ESP_ERR_INVALID_ARG;
    }
    
    s_motors[id - 1].min_interval_ms = min_interval_ms;
    return ESP_OK;
}

esp_err_t motor_set_speed_limits(motor_id_t id, uint8_t min_speed, uint8_t max_speed)
{
    if (!is_valid_motor_id(id)) {
        return ESP_ERR_INVALID_ARG;
    }
    
    if (min_speed > max_speed || max_speed > 100) {
        return ESP_ERR_INVALID_ARG;
    }
    
    s_motors[id - 1].min_speed_percent = min_speed;
    s_motors[id - 1].max_speed_percent = max_speed;
    
    return ESP_OK;
}

void motor_register_state_callback(motor_state_callback_t callback)
{
    s_state_callback = callback;
}

void motor_register_error_callback(motor_error_callback_t callback)
{
    s_error_callback = callback;
}

esp_err_t motor_test_run(motor_id_t id, uint32_t duration_ms)
{
    ESP_LOGI(TAG, "Starting test run for motor %d, duration: %lu ms", id, duration_ms);
    return motor_start_timed(id, MOTOR_DEFAULT_SPEED, duration_ms);
}

// Timer callbacks
static void motor_safety_timer_callback(TimerHandle_t xTimer)
{
    motor_id_t id = (motor_id_t)(intptr_t)pvTimerGetTimerID(xTimer);
    motor_t *motor = &s_motors[id - 1];
    
    ESP_LOGW(TAG, "Safety timer expired for motor %d", id);
    
    // Do minimal work in timer callback to avoid stack overflow
    // Just stop the PWM and update state
    ledc_set_duty(LEDC_LOW_SPEED_MODE, motor->pwm_channel, 0);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, motor->pwm_channel);
    
    // Update basic state
    motor->speed_percent = 0;
    motor->state = MOTOR_STATE_STOPPED;
    
    // Stats update can be deferred or done in main context
    // For now, just record the stop time
    motor->last_stop_time = get_time_ms();
}

static void motor_soft_start_timer_callback(TimerHandle_t xTimer)
{
    motor_id_t id = (motor_id_t)(intptr_t)pvTimerGetTimerID(xTimer);
    motor_t *motor = &s_motors[id - 1];
    
    if (motor->state != MOTOR_STATE_RUNNING) {
        xTimerStop(xTimer, 0);
        return;
    }
    
    // Calculate increment based on soft start time
    // We want to go from 0 to target in MOTOR_SOFT_START_TIME_MS
    // Timer runs every 50ms, so number of steps = MOTOR_SOFT_START_TIME_MS / 50
    int steps = MOTOR_SOFT_START_TIME_MS / 50;
    int increment = motor->target_speed / steps;
    if (increment < 1) increment = 1;
    
    // Ramp up speed
    if (motor->speed_percent < motor->target_speed) {
        motor->speed_percent += increment;
        if (motor->speed_percent > motor->target_speed) {
            motor->speed_percent = motor->target_speed;
        }
        
        uint8_t duty = (motor->speed_percent * MOTOR_PWM_MAX_DUTY) / 100;
        
        // Update PWM directly without taking mutex (atomic operation)
        ledc_set_duty(LEDC_LOW_SPEED_MODE, motor->pwm_channel, duty);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, motor->pwm_channel);
        
        ESP_LOGD(TAG, "Soft start motor %d: %d%% (target: %d%%)", 
                 id, motor->speed_percent, motor->target_speed);
        
        // Stop timer when target reached
        if (motor->speed_percent >= motor->target_speed) {
            xTimerStop(xTimer, 0);
            ESP_LOGD(TAG, "Soft start complete for motor %d", id);
        }
    }
}

// NVS functions
static esp_err_t motor_save_stats(motor_id_t id)
{
    nvs_handle_t nvs_handle;
    esp_err_t ret;
    char key[16];
    
    ret = nvs_open(MOTOR_NVS_NAMESPACE, NVS_READWRITE, &nvs_handle);
    if (ret != ESP_OK) {
        return ret;
    }
    
    snprintf(key, sizeof(key), "motor%d_stats", id);
    ret = nvs_set_blob(nvs_handle, key, &s_motors[id - 1].stats, sizeof(motor_stats_t));
    
    if (ret == ESP_OK) {
        nvs_commit(nvs_handle);
    }
    
    nvs_close(nvs_handle);
    return ret;
}

static esp_err_t motor_load_stats(motor_id_t id)
{
    nvs_handle_t nvs_handle;
    esp_err_t ret;
    char key[16];
    size_t length = sizeof(motor_stats_t);
    
    ret = nvs_open(MOTOR_NVS_NAMESPACE, NVS_READONLY, &nvs_handle);
    if (ret != ESP_OK) {
        return ret;
    }
    
    snprintf(key, sizeof(key), "motor%d_stats", id);
    ret = nvs_get_blob(nvs_handle, key, &s_motors[id - 1].stats, &length);
    
    nvs_close(nvs_handle);
    
    if (ret == ESP_OK) {
        ESP_LOGI(TAG, "Loaded stats for motor %d: total runtime %lu ms, %lu runs",
                id, s_motors[id - 1].stats.total_runtime_ms, s_motors[id - 1].stats.run_count);
    }
    
    return ret;
}