#include "motor_control.h"
#include "config.h"
#include "esp_log.h"
#include "driver/gpio.h"
#include "driver/ledc.h"
#include <string.h>

// Private variables
static motor_state_t motor_state = {
    .mode = MOTOR_OFF,
    .pending_mode = MOTOR_OFF,
    .target_speed = 0,
    .pending_speed = 0,
    .current_speed = 0,
    .state = MOTOR_STATE_IDLE,
    .ramping = false,
    .ramp_timer = NULL,
    .cooldown_timer = NULL,
    .cooldown_remaining_ms = 0,
    .last_on_mode = MOTOR_EXHAUST,  // Default to exhaust for ON button
    .last_on_speed = 50,            // Default to 50% for ON button
    .user_turned_off = false
};

// Forward declarations for private functions
static void apply_motor_pwm(int speed_percent);
static void start_motor_operation(motor_mode_t mode, int speed_percent);
static void save_last_on_state(motor_mode_t mode, int speed);
static void motor_ramp_timer_callback(TimerHandle_t xTimer);
static void motor_cooldown_timer_callback(TimerHandle_t xTimer);

// Private function: Apply PWM to motor based on current mode and speed
static void apply_motor_pwm(int speed_percent) {
    // Clamp speed to valid range using config macro
    speed_percent = CLAMP_SPEED(speed_percent);
    
    uint32_t duty = SPEED_TO_DUTY(speed_percent);
    
    if (motor_state.mode == MOTOR_OFF || speed_percent == 0) {
        gpio_set_level(LED_PIN, 0);
        gpio_set_level(MOTOR_R_EN, 0);
        gpio_set_level(MOTOR_L_EN, 0);
        ledc_set_duty(PWM_SPEED_MODE, PWM_R_CHANNEL, 0);
        ledc_set_duty(PWM_SPEED_MODE, PWM_L_CHANNEL, 0);
        ledc_update_duty(PWM_SPEED_MODE, PWM_R_CHANNEL);
        ledc_update_duty(PWM_SPEED_MODE, PWM_L_CHANNEL);
        
    } else if (motor_state.mode == MOTOR_EXHAUST) {
        gpio_set_level(LED_PIN, 1);
        gpio_set_level(MOTOR_R_EN, 1);
        gpio_set_level(MOTOR_L_EN, 1);
        ledc_set_duty(PWM_SPEED_MODE, PWM_R_CHANNEL, duty);
        ledc_set_duty(PWM_SPEED_MODE, PWM_L_CHANNEL, 0);
        ledc_update_duty(PWM_SPEED_MODE, PWM_R_CHANNEL);
        ledc_update_duty(PWM_SPEED_MODE, PWM_L_CHANNEL);
        
    } else if (motor_state.mode == MOTOR_INTAKE) {
        gpio_set_level(LED_PIN, 1);
        gpio_set_level(MOTOR_R_EN, 1);
        gpio_set_level(MOTOR_L_EN, 1);
        ledc_set_duty(PWM_SPEED_MODE, PWM_R_CHANNEL, 0);
        ledc_set_duty(PWM_SPEED_MODE, PWM_L_CHANNEL, duty);
        ledc_update_duty(PWM_SPEED_MODE, PWM_R_CHANNEL);
        ledc_update_duty(PWM_SPEED_MODE, PWM_L_CHANNEL);
    }
}

// Private function: Motor ramp timer callback
static void motor_ramp_timer_callback(TimerHandle_t xTimer) {
    if (motor_state.state != MOTOR_STATE_RAMPING) {
        return;
    }
    
    int speed_diff = motor_state.target_speed - motor_state.current_speed;
    
    if (abs(speed_diff) <= RAMP_STEP_SIZE) {
        // Close enough to target, finish ramping
        motor_state.current_speed = motor_state.target_speed;
        motor_state.ramping = false;
        motor_state.state = MOTOR_STATE_IDLE;
        
        // Stop the timer
        xTimerStop(motor_state.ramp_timer, 0);
        
        ESP_LOGI(SYSTEM_TAG, "Ramping complete - Final speed: %d%%", motor_state.current_speed);
    } else {
        // Continue ramping
        if (speed_diff > 0) {
            motor_state.current_speed += RAMP_STEP_SIZE;
        } else {
            motor_state.current_speed -= RAMP_STEP_SIZE;
        }
        
        MOTOR_LOGD(SYSTEM_TAG, "Ramping: %d%% (target: %d%%)", motor_state.current_speed, motor_state.target_speed);
    }
    
    apply_motor_pwm(motor_state.current_speed);
}

// Private function: Motor cooldown timer callback
static void motor_cooldown_timer_callback(TimerHandle_t xTimer) {
    ESP_LOGI(SYSTEM_TAG, "Cooldown complete - Starting motor in %s mode at %d%%",
             motor_state.pending_mode == MOTOR_EXHAUST ? "EXHAUST" : "INTAKE",
             motor_state.pending_speed);
    
    // Reset cooldown tracking
    motor_state.cooldown_remaining_ms = 0;
    
    // Start the motor in the pending mode
    start_motor_operation(motor_state.pending_mode, motor_state.pending_speed);
}

// Private function: Save the last ON state (for ON button functionality)
static void save_last_on_state(motor_mode_t mode, int speed) {
    if (mode != MOTOR_OFF && speed > 0) {
        motor_state.last_on_mode = mode;
        motor_state.last_on_speed = speed;
        ESP_LOGI(SYSTEM_TAG, "Last ON state updated: %s @ %d%%", 
                 mode == MOTOR_EXHAUST ? "EXHAUST" : "INTAKE", speed);
    }
}

// Private function: Start motor operation (internal function)
static void start_motor_operation(motor_mode_t mode, int speed_percent) {
    // Clamp speed using config macro
    speed_percent = CLAMP_SPEED(speed_percent);
    
    motor_state.mode = mode;
    motor_state.target_speed = speed_percent;
    motor_state.state = MOTOR_STATE_RAMPING;
    motor_state.ramping = true;
    
    if (mode == MOTOR_OFF || speed_percent == 0) {
        // Immediate stop
        motor_state.current_speed = 0;
        motor_state.target_speed = 0;
        motor_state.state = MOTOR_STATE_IDLE;
        motor_state.ramping = false;
        apply_motor_pwm(0);
        ESP_LOGI(SYSTEM_TAG, "Motor stopped immediately");
    } else {
        // Save last ON state for future ON button use
        save_last_on_state(mode, speed_percent);
        
        // Start from minimum speed if currently off
        if (motor_state.current_speed == 0) {
            int start_speed = (speed_percent < MIN_MOTOR_SPEED) ? speed_percent : MIN_MOTOR_SPEED;
            motor_state.current_speed = start_speed;
            apply_motor_pwm(start_speed);
            ESP_LOGI(SYSTEM_TAG, "Motor starting at %d%%, ramping to %d%%", start_speed, speed_percent);
        }
        
        // Start ramping if needed
        if (motor_state.current_speed != motor_state.target_speed) {
            xTimerStart(motor_state.ramp_timer, 0);
        } else {
            motor_state.state = MOTOR_STATE_IDLE;
            motor_state.ramping = false;
        }
    }
}

// Public API Implementation

esp_err_t motor_control_init(void) {
    ESP_LOGI(SYSTEM_TAG, "Initializing motor control system...");
    
    // Configure GPIO pins
    ESP_LOGI(SYSTEM_TAG, "Configuring GPIO pins...");
    
    uint64_t pin_mask = (1ULL << LED_PIN) | 
                       (1ULL << MOTOR_R_EN) | 
                       (1ULL << MOTOR_L_EN);
    
    gpio_config_t io_conf = {
        .pin_bit_mask = pin_mask,
        .mode = GPIO_MODE_OUTPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE
    };
    
    esp_err_t ret = gpio_config(&io_conf);
    if (ret != ESP_OK) {
        ESP_LOGE(SYSTEM_TAG, "Failed to configure GPIO pins: %s", esp_err_to_name(ret));
        return ret;
    }
    
    // Set initial pin states
    gpio_set_level(LED_PIN, 0);
    gpio_set_level(MOTOR_R_EN, 0);
    gpio_set_level(MOTOR_L_EN, 0);
    
    ESP_LOGI(SYSTEM_TAG, "GPIO pins configured");
    
    // Configure PWM
    ESP_LOGI(SYSTEM_TAG, "Configuring PWM...");
    
    ledc_timer_config_t timer_conf = {
        .speed_mode = PWM_SPEED_MODE,
        .timer_num = PWM_TIMER,
        .duty_resolution = PWM_RESOLUTION,
        .freq_hz = PWM_FREQUENCY,
        .clk_cfg = LEDC_AUTO_CLK
    };
    ret = ledc_timer_config(&timer_conf);
    if (ret != ESP_OK) {
        ESP_LOGE(SYSTEM_TAG, "Failed to configure PWM timer: %s", esp_err_to_name(ret));
        return ret;
    }
    
    ledc_channel_config_t channel_conf = {
        .channel = PWM_R_CHANNEL,
        .duty = 0,
        .gpio_num = PWM_R_PIN,
        .speed_mode = PWM_SPEED_MODE,
        .hpoint = 0,
        .timer_sel = PWM_TIMER
    };
    ret = ledc_channel_config(&channel_conf);
    if (ret != ESP_OK) {
        ESP_LOGE(SYSTEM_TAG, "Failed to configure PWM right channel: %s", esp_err_to_name(ret));
        return ret;
    }
    
    channel_conf.channel = PWM_L_CHANNEL;
    channel_conf.gpio_num = PWM_L_PIN;
    ret = ledc_channel_config(&channel_conf);
    if (ret != ESP_OK) {
        ESP_LOGE(SYSTEM_TAG, "Failed to configure PWM left channel: %s", esp_err_to_name(ret));
        return ret;
    }
    
    ESP_LOGI(SYSTEM_TAG, "PWM configured");
    
    // Create timers
    motor_state.ramp_timer = xTimerCreate(
        "MotorRampTimer",                    // Timer name
        pdMS_TO_TICKS(RAMP_STEP_MS),        // Timer period
        pdTRUE,                             // Auto-reload
        (void*)0,                           // Timer ID
        motor_ramp_timer_callback           // Callback function
    );
    
    motor_state.cooldown_timer = xTimerCreate(
        "MotorCooldownTimer",               // Timer name
        pdMS_TO_TICKS(DIRECTION_CHANGE_COOLDOWN_MS), // Timer period
        pdFALSE,                            // One-shot
        (void*)0,                           // Timer ID
        motor_cooldown_timer_callback       // Callback function
    );
    
    if (motor_state.ramp_timer == NULL || motor_state.cooldown_timer == NULL) {
        ESP_LOGE(SYSTEM_TAG, "Failed to create motor timers");
        return ESP_FAIL;
    }
    
    ESP_LOGI(SYSTEM_TAG, "Motor control system initialized with direction change safety");
    return ESP_OK;
}

void motor_set_speed(motor_mode_t mode, int speed_percent) {
    // Clamp speed to valid range using config macro
    speed_percent = CLAMP_SPEED(speed_percent);
    
    ESP_LOGI(SYSTEM_TAG, "Motor command: %s - Speed: %d%% (Current mode: %s, Current speed: %d%%, State: %s)", 
             motor_mode_to_string(mode), speed_percent,
             motor_mode_to_string(motor_state.mode), motor_state.current_speed,
             motor_state_to_string(motor_state.state));
    
    // Track if user manually turned off
    if (mode == MOTOR_OFF && motor_state.mode != MOTOR_OFF) {
        motor_state.user_turned_off = true;
        ESP_LOGI(SYSTEM_TAG, "User manually turned OFF - will stay off after restart");
    } else if (mode != MOTOR_OFF) {
        motor_state.user_turned_off = false;
        ESP_LOGI(SYSTEM_TAG, "Motor turned ON - will resume after power loss");
    }
    
    // If we're in cooldown, update the pending command
    if (motor_state.state == MOTOR_STATE_COOLDOWN) {
        motor_state.pending_mode = mode;
        motor_state.pending_speed = speed_percent;
        ESP_LOGI(SYSTEM_TAG, "Motor in cooldown - command queued for execution");
        return;
    }
    
    // Check if this is a direction change that requires cooldown using config macro
    bool requires_cooldown = false;
    if (motor_state.current_speed > 0 && motor_state.mode != MOTOR_OFF) {
        requires_cooldown = IS_DIRECTION_CHANGE(motor_state.mode, mode);
    }
    
    if (requires_cooldown) {
        ESP_LOGI(SYSTEM_TAG, "Direction change detected - initiating safety cooldown sequence");
        
        // Stop any current ramping
        if (motor_state.ramping) {
            xTimerStop(motor_state.ramp_timer, 0);
            motor_state.ramping = false;
        }
        
        // Stop the motor immediately
        motor_state.mode = MOTOR_OFF;
        motor_state.current_speed = 0;
        motor_state.target_speed = 0;
        motor_state.state = MOTOR_STATE_COOLDOWN;
        motor_state.cooldown_remaining_ms = DIRECTION_CHANGE_COOLDOWN_MS;
        apply_motor_pwm(0);
        
        // Store the pending command
        motor_state.pending_mode = mode;
        motor_state.pending_speed = speed_percent;
        
        // Start cooldown timer
        xTimerStart(motor_state.cooldown_timer, 0);
        
        ESP_LOGI(SYSTEM_TAG, "Motor stopped for direction change - %d second cooldown started", 
                 DIRECTION_CHANGE_COOLDOWN_MS / 1000);
    } else {
        // No direction change required, proceed normally
        
        // Stop any current ramping
        if (motor_state.ramping) {
            xTimerStop(motor_state.ramp_timer, 0);
            motor_state.ramping = false;
        }
        
        // Stop cooldown timer if running
        if (motor_state.state == MOTOR_STATE_COOLDOWN) {
            xTimerStop(motor_state.cooldown_timer, 0);
            motor_state.cooldown_remaining_ms = 0;
        }
        
        start_motor_operation(mode, speed_percent);
    }
}

const motor_state_t* motor_get_state(void) {
    return &motor_state;
}

void motor_update_cooldown_time(void) {
    if (motor_state.state == MOTOR_STATE_COOLDOWN && motor_state.cooldown_remaining_ms > 0) {
        if (motor_state.cooldown_remaining_ms >= STATUS_UPDATE_INTERVAL_MS) {
            motor_state.cooldown_remaining_ms -= STATUS_UPDATE_INTERVAL_MS;
        } else {
            motor_state.cooldown_remaining_ms = 0;
        }
    }
}

const char* motor_mode_to_string(motor_mode_t mode) {
    switch (mode) {
        case MOTOR_OFF: return "OFF";
        case MOTOR_EXHAUST: return "EXHAUST";
        case MOTOR_INTAKE: return "INTAKE";
        default: return "UNKNOWN";
    }
}

const char* motor_state_to_string(motor_state_enum_t state) {
    switch (state) {
        case MOTOR_STATE_IDLE: return "IDLE";
        case MOTOR_STATE_RAMPING: return "RAMPING";
        case MOTOR_STATE_STOPPING: return "STOPPING";
        case MOTOR_STATE_COOLDOWN: return "COOLDOWN";
        case MOTOR_STATE_RESTARTING: return "RESTARTING";
        default: return "UNKNOWN";
    }
}

bool motor_is_ramping(void) {
    return motor_state.ramping;
}

bool motor_is_in_cooldown(void) {
    return motor_state.state == MOTOR_STATE_COOLDOWN;
}

uint32_t motor_get_cooldown_remaining(void) {
    return motor_state.cooldown_remaining_ms;
}

void motor_set_last_on_state(motor_mode_t mode, int speed) {
    if (mode != MOTOR_OFF && IS_VALID_SPEED(speed) && speed > 0) {
        motor_state.last_on_mode = mode;
        motor_state.last_on_speed = speed;
        ESP_LOGI(SYSTEM_TAG, "Last ON state set: %s @ %d%%", 
                 motor_mode_to_string(mode), speed);
    }
}

void motor_get_last_on_state(motor_mode_t* mode, int* speed) {
    if (mode) *mode = motor_state.last_on_mode;
    if (speed) *speed = motor_state.last_on_speed;
}

void motor_resume_last_state(void) {
    ESP_LOGI(SYSTEM_TAG, "Resuming last state: %s @ %d%%", 
             motor_mode_to_string(motor_state.last_on_mode), motor_state.last_on_speed);
    motor_set_speed(motor_state.last_on_mode, motor_state.last_on_speed);
}

void motor_set_user_turned_off(bool turned_off) {
    motor_state.user_turned_off = turned_off;
}

bool motor_get_user_turned_off(void) {
    return motor_state.user_turned_off;
}