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

// Pin definitions for BTS7960 motor driver
#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 (we'll just use as GPIO for now)
#define PWM_L_PIN GPIO_NUM_22      // BTS7960 L_PWM pin (we'll just use as GPIO for now)

static const char* TAG = "MULTI_GPIO";

// Function to configure all GPIO pins
void configure_gpio_pins(void)
{
    ESP_LOGI(TAG, "Configuring GPIO pins...");
    
    // Create bit mask for all our output pins
    uint64_t pin_mask = (1ULL << LED_PIN) | 
                       (1ULL << MOTOR_R_EN) | 
                       (1ULL << MOTOR_L_EN) | 
                       (1ULL << PWM_R_PIN) | 
                       (1ULL << PWM_L_PIN);
    
    gpio_config_t io_conf = {
        .pin_bit_mask = pin_mask,               // Set all pins at once
        .mode = GPIO_MODE_OUTPUT,               // All are outputs
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE
    };
    
    gpio_config(&io_conf);
    
    // Initialize all pins to LOW (off)
    gpio_set_level(LED_PIN, 0);
    gpio_set_level(MOTOR_R_EN, 0);
    gpio_set_level(MOTOR_L_EN, 0);
    gpio_set_level(PWM_R_PIN, 0);
    gpio_set_level(PWM_L_PIN, 0);
    
    ESP_LOGI(TAG, "All GPIO pins configured and initialized to LOW");
}

// Simulate turning motor on (enable pins HIGH, direction set)
void motor_enable_exhaust(void)
{
    ESP_LOGI(TAG, "Motor ENABLE - Exhaust Mode");
    gpio_set_level(LED_PIN, 1);        // LED on for status
    
    // For exhaust: only enable R side, disable L side
    gpio_set_level(MOTOR_R_EN, 1);     // Enable R half-bridge for exhaust
    gpio_set_level(MOTOR_L_EN, 0);     // Disable L half-bridge (not needed)
    vTaskDelay(pdMS_TO_TICKS(50));     // Small delay for enable to settle
    
    gpio_set_level(PWM_R_PIN, 1);      // "PWM" on for exhaust direction
    gpio_set_level(PWM_L_PIN, 0);      // Other direction off
}

// Simulate changing to intake mode
void motor_enable_intake(void)
{
    ESP_LOGI(TAG, "Motor ENABLE - Intake Mode");
    gpio_set_level(LED_PIN, 1);        // LED on for status
    
    // For intake: only enable L side, disable R side
    gpio_set_level(MOTOR_R_EN, 0);     // Disable R half-bridge (not needed)
    gpio_set_level(MOTOR_L_EN, 1);     // Enable L half-bridge for intake
    vTaskDelay(pdMS_TO_TICKS(50));
    
    gpio_set_level(PWM_R_PIN, 0);      // Switch directions
    gpio_set_level(PWM_L_PIN, 1);      // "PWM" on for intake direction
}

// Turn motor completely off
void motor_disable(void)
{
    ESP_LOGI(TAG, "Motor DISABLE");
    gpio_set_level(LED_PIN, 0);        // LED off for status
    gpio_set_level(MOTOR_R_EN, 0);     // Disable motor driver
    gpio_set_level(MOTOR_L_EN, 0);
    gpio_set_level(PWM_R_PIN, 0);      // Turn off both "PWM" pins
    gpio_set_level(PWM_L_PIN, 0);
}

void app_main(void)
{
    ESP_LOGI(TAG, "Starting multi-GPIO motor control test!");
    
    // Configure all our pins
    configure_gpio_pins();
    
    ESP_LOGI(TAG, "Starting motor control sequence...");
    
    while(1) {
        // Test sequence: Off -> Exhaust -> Off -> Intake -> Off
        
        ESP_LOGI(TAG, "=== Motor OFF ===");
        motor_disable();
        vTaskDelay(pdMS_TO_TICKS(2000));    // Wait 2 seconds
        
        ESP_LOGI(TAG, "=== Motor EXHAUST Mode ===");
        motor_enable_exhaust();
        vTaskDelay(pdMS_TO_TICKS(3000));    // Run for 3 seconds
        
        ESP_LOGI(TAG, "=== Motor OFF ===");
        motor_disable();
        vTaskDelay(pdMS_TO_TICKS(2000));
        
        ESP_LOGI(TAG, "=== Motor INTAKE Mode ===");
        motor_enable_intake();
        vTaskDelay(pdMS_TO_TICKS(3000));    // Run for 3 seconds
        
        ESP_LOGI(TAG, "=== Cycle Complete - Restarting ===");
    }
}