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

// Pin definitions
#define LED_PIN GPIO_NUM_13        // Onboard LED for status
#define MOTOR_R_EN GPIO_NUM_18     // BTS7960 R_EN pin
#define MOTOR_L_EN GPIO_NUM_19     // BTS7960 L_EN pin
#define PWM_R_PIN GPIO_NUM_21      // BTS7960 R_PWM pin
#define PWM_L_PIN GPIO_NUM_22      // BTS7960 L_PWM pin

// PWM configuration
#define PWM_FREQUENCY 1000         // 1kHz PWM frequency
#define PWM_RESOLUTION LEDC_TIMER_8_BIT  // 8-bit resolution (0-255)
#define PWM_R_CHANNEL LEDC_CHANNEL_0
#define PWM_L_CHANNEL LEDC_CHANNEL_1

static const char* TAG = "PWM_MOTOR";

// Motor state
typedef enum {
    MOTOR_OFF,
    MOTOR_EXHAUST,
    MOTOR_INTAKE
} motor_mode_t;

static motor_mode_t current_mode = MOTOR_OFF;
static int current_speed = 0;  // 0-100%

void configure_gpio_pins(void)
{
    ESP_LOGI(TAG, "Configuring GPIO pins...");
    
    // Configure enable pins as regular GPIO
    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
    };
    
    gpio_config(&io_conf);
    
    // Initialize enable pins to LOW
    gpio_set_level(LED_PIN, 0);
    gpio_set_level(MOTOR_R_EN, 0);
    gpio_set_level(MOTOR_L_EN, 0);
    
    ESP_LOGI(TAG, "GPIO pins configured");
}

void configure_pwm(void)
{
    ESP_LOGI(TAG, "Configuring PWM...");
    
    // Configure PWM timer
    ledc_timer_config_t timer_conf = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .timer_num = LEDC_TIMER_0,
        .duty_resolution = PWM_RESOLUTION,
        .freq_hz = PWM_FREQUENCY,
        .clk_cfg = LEDC_AUTO_CLK
    };
    ledc_timer_config(&timer_conf);
    
    // Configure PWM channel for R_PWM
    ledc_channel_config_t channel_conf = {
        .channel = PWM_R_CHANNEL,
        .duty = 0,
        .gpio_num = PWM_R_PIN,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .hpoint = 0,
        .timer_sel = LEDC_TIMER_0
    };
    ledc_channel_config(&channel_conf);
    
    // Configure PWM channel for L_PWM
    channel_conf.channel = PWM_L_CHANNEL;
    channel_conf.gpio_num = PWM_L_PIN;
    ledc_channel_config(&channel_conf);
    
    ESP_LOGI(TAG, "PWM configured - Frequency: %dHz, Resolution: %d-bit", 
             PWM_FREQUENCY, PWM_RESOLUTION);
}

void set_motor_speed(motor_mode_t mode, int speed_percent)
{
    if (speed_percent < 0) speed_percent = 0;
    if (speed_percent > 100) speed_percent = 100;
    
    current_mode = mode;
    current_speed = speed_percent;
    
    // Convert percentage to PWM duty value (0-255 for 8-bit)
    uint32_t duty = (speed_percent * 255) / 100;
    
    if (mode == MOTOR_OFF || speed_percent == 0) {
        ESP_LOGI(TAG, "Motor OFF");
        
        // Turn off LED
        gpio_set_level(LED_PIN, 0);
        
        // Disable both enables
        gpio_set_level(MOTOR_R_EN, 0);
        gpio_set_level(MOTOR_L_EN, 0);
        
        // Set both PWM to 0
        ledc_set_duty(LEDC_LOW_SPEED_MODE, PWM_R_CHANNEL, 0);
        ledc_set_duty(LEDC_LOW_SPEED_MODE, PWM_L_CHANNEL, 0);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, PWM_R_CHANNEL);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, PWM_L_CHANNEL);
        
    } else if (mode == MOTOR_EXHAUST) {
        ESP_LOGI(TAG, "Motor EXHAUST - Speed: %d%% (PWM duty: %d/255)", 
                 speed_percent, duty);
        
        // Turn on LED
        gpio_set_level(LED_PIN, 1);
        
        // Enable R side, disable L side
        gpio_set_level(MOTOR_R_EN, 1);
        gpio_set_level(MOTOR_L_EN, 0);
        
        // Small delay for enables to settle
        vTaskDelay(pdMS_TO_TICKS(10));
        
        // Set PWM: R side active, L side off
        ledc_set_duty(LEDC_LOW_SPEED_MODE, PWM_R_CHANNEL, duty);
        ledc_set_duty(LEDC_LOW_SPEED_MODE, PWM_L_CHANNEL, 0);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, PWM_R_CHANNEL);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, PWM_L_CHANNEL);
        
    } else if (mode == MOTOR_INTAKE) {
        ESP_LOGI(TAG, "Motor INTAKE - Speed: %d%% (PWM duty: %d/255)", 
                 speed_percent, duty);
        
        // Turn on LED
        gpio_set_level(LED_PIN, 1);
        
        // Enable L side, disable R side
        gpio_set_level(MOTOR_R_EN, 0);
        gpio_set_level(MOTOR_L_EN, 1);
        
        // Small delay for enables to settle
        vTaskDelay(pdMS_TO_TICKS(10));
        
        // Set PWM: L side active, R side off
        ledc_set_duty(LEDC_LOW_SPEED_MODE, PWM_R_CHANNEL, 0);
        ledc_set_duty(LEDC_LOW_SPEED_MODE, PWM_L_CHANNEL, duty);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, PWM_R_CHANNEL);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, PWM_L_CHANNEL);
    }
}

void app_main(void)
{
    ESP_LOGI(TAG, "Starting PWM motor control test!");
    
    // Configure hardware
    configure_gpio_pins();
    configure_pwm();
    
    ESP_LOGI(TAG, "Starting motor speed test sequence...");
    
    while(1) {
        // Test sequence: demonstrate variable speed control
        
        ESP_LOGI(TAG, "\n=== EXHAUST MODE SPEED TEST ===");
        
        // Exhaust: ramp up from 0 to 100%
        for (int speed = 0; speed <= 100; speed += 25) {
            set_motor_speed(MOTOR_EXHAUST, speed);
            vTaskDelay(pdMS_TO_TICKS(2000));  // 2 seconds at each speed
        }
        
        // Turn off
        set_motor_speed(MOTOR_OFF, 0);
        vTaskDelay(pdMS_TO_TICKS(2000));
        
        ESP_LOGI(TAG, "\n=== INTAKE MODE SPEED TEST ===");
        
        // Intake: ramp up from 0 to 100%
        for (int speed = 0; speed <= 100; speed += 25) {
            set_motor_speed(MOTOR_INTAKE, speed);
            vTaskDelay(pdMS_TO_TICKS(2000));  // 2 seconds at each speed
        }
        
        // Turn off
        set_motor_speed(MOTOR_OFF, 0);
        vTaskDelay(pdMS_TO_TICKS(2000));
        
        ESP_LOGI(TAG, "\n=== SMOOTH SPEED CHANGES ===");
        
        // Demonstrate smooth speed changes in exhaust mode
        set_motor_speed(MOTOR_EXHAUST, 30);
        vTaskDelay(pdMS_TO_TICKS(2000));
        
        set_motor_speed(MOTOR_EXHAUST, 60);
        vTaskDelay(pdMS_TO_TICKS(2000));
        
        set_motor_speed(MOTOR_EXHAUST, 90);
        vTaskDelay(pdMS_TO_TICKS(2000));
        
        set_motor_speed(MOTOR_EXHAUST, 40);
        vTaskDelay(pdMS_TO_TICKS(2000));
        
        set_motor_speed(MOTOR_OFF, 0);
        vTaskDelay(pdMS_TO_TICKS(3000));
        
        ESP_LOGI(TAG, "\n=== Cycle Complete - Restarting ===");
    }
}