#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "driver/gpio.h"
#include "driver/ledc.h"

// WiFi credentials - CHANGE THESE TO YOUR NETWORK
#define WIFI_SSID "YOUR_WIFI_NAME"
#define WIFI_PASS "YOUR_WIFI_PASSWORD"
#define WIFI_MAXIMUM_RETRY 5

// Pin definitions (same as before)
#define LED_PIN GPIO_NUM_13
#define MOTOR_R_EN GPIO_NUM_18
#define MOTOR_L_EN GPIO_NUM_19
#define PWM_R_PIN GPIO_NUM_21
#define PWM_L_PIN GPIO_NUM_22

// PWM configuration (same as before)
#define PWM_FREQUENCY 1000
#define PWM_RESOLUTION LEDC_TIMER_8_BIT
#define PWM_R_CHANNEL LEDC_CHANNEL_0
#define PWM_L_CHANNEL LEDC_CHANNEL_1

static const char* TAG = "WIFI_MOTOR";

// WiFi event group
static EventGroupHandle_t s_wifi_event_group;
#define WIFI_CONNECTED_BIT BIT0
#define WIFI_FAIL_BIT BIT1

static int s_retry_num = 0;

// Motor control (same as before)
typedef enum {
    MOTOR_OFF,
    MOTOR_EXHAUST,
    MOTOR_INTAKE
} motor_mode_t;

static motor_mode_t current_mode = MOTOR_OFF;
static int current_speed = 0;

// WiFi event handler
static void event_handler(void* arg, esp_event_base_t event_base,
                         int32_t event_id, void* event_data)
{
    if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
        esp_wifi_connect();
    } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
        if (s_retry_num < WIFI_MAXIMUM_RETRY) {
            esp_wifi_connect();
            s_retry_num++;
            ESP_LOGI(TAG, "retry to connect to the AP");
        } else {
            xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT);
        }
        ESP_LOGI(TAG, "connect to the AP fail");
    } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
        ip_event_got_ip_t* event = (ip_event_got_ip_t*) event_data;
        ESP_LOGI(TAG, "got ip:" IPSTR, IP2STR(&event->ip_info.ip));
        s_retry_num = 0;
        xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
    }
}

void wifi_init_sta(void)
{
    s_wifi_event_group = xEventGroupCreate();

    ESP_ERROR_CHECK(esp_netif_init());
    ESP_ERROR_CHECK(esp_event_loop_create_default());
    esp_netif_create_default_wifi_sta();

    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));

    esp_event_handler_instance_t instance_any_id;
    esp_event_handler_instance_t instance_got_ip;
    ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT,
                                                        ESP_EVENT_ANY_ID,
                                                        &event_handler,
                                                        NULL,
                                                        &instance_any_id));
    ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT,
                                                        IP_EVENT_STA_GOT_IP,
                                                        &event_handler,
                                                        NULL,
                                                        &instance_got_ip));

    wifi_config_t wifi_config = {
        .sta = {
            .ssid = WIFI_SSID,
            .password = WIFI_PASS,
            .threshold.authmode = WIFI_AUTH_WPA2_PSK,
            .pmf_cfg = {
                .capable = true,
                .required = false
            },
        },
    };
    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
    ESP_ERROR_CHECK(esp_wifi_start());

    ESP_LOGI(TAG, "wifi_init_sta finished.");

    /* Waiting until either the connection is established (WIFI_CONNECTED_BIT) or connection failed for the maximum
     * number of re-tries (WIFI_FAIL_BIT). The bits are set by event_handler() (see above) */
    EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group,
            WIFI_CONNECTED_BIT | WIFI_FAIL_BIT,
            pdFALSE,
            pdFALSE,
            portMAX_DELAY);

    /* xEventGroupWaitBits() returns the bits before the call returned, hence we can test which event actually
     * happened. */
    if (bits & WIFI_CONNECTED_BIT) {
        ESP_LOGI(TAG, "connected to ap SSID:%s", WIFI_SSID);
    } else if (bits & WIFI_FAIL_BIT) {
        ESP_LOGI(TAG, "Failed to connect to SSID:%s", WIFI_SSID);
    } else {
        ESP_LOGE(TAG, "UNEXPECTED EVENT");
    }
}

void configure_gpio_pins(void)
{
    ESP_LOGI(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
    };
    
    gpio_config(&io_conf);
    
    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...");
    
    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);
    
    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);
    
    channel_conf.channel = PWM_L_CHANNEL;
    channel_conf.gpio_num = PWM_L_PIN;
    ledc_channel_config(&channel_conf);
    
    ESP_LOGI(TAG, "PWM configured");
}

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;
    
    uint32_t duty = (speed_percent * 255) / 100;
    
    if (mode == MOTOR_OFF || speed_percent == 0) {
        ESP_LOGI(TAG, "Motor OFF");
        gpio_set_level(LED_PIN, 0);
        gpio_set_level(MOTOR_R_EN, 0);
        gpio_set_level(MOTOR_L_EN, 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%%", speed_percent);
        gpio_set_level(LED_PIN, 1);
        gpio_set_level(MOTOR_R_EN, 1);
        gpio_set_level(MOTOR_L_EN, 0);
        vTaskDelay(pdMS_TO_TICKS(10));
        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%%", speed_percent);
        gpio_set_level(LED_PIN, 1);
        gpio_set_level(MOTOR_R_EN, 0);
        gpio_set_level(MOTOR_L_EN, 1);
        vTaskDelay(pdMS_TO_TICKS(10));
        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);
    }
}

// Simple demo task that runs once WiFi is connected
void motor_demo_task(void *pvParameters)
{
    ESP_LOGI(TAG, "Motor demo task started - waiting for WiFi...");
    
    // Wait for WiFi connection
    EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group,
            WIFI_CONNECTED_BIT,
            pdFALSE,
            pdFALSE,
            portMAX_DELAY);
    
    if (bits & WIFI_CONNECTED_BIT) {
        ESP_LOGI(TAG, "WiFi connected! Starting motor demo...");
        
        while(1) {
            ESP_LOGI(TAG, "=== WiFi Motor Demo Sequence ===");
            
            // Short demo since we have WiFi now
            set_motor_speed(MOTOR_EXHAUST, 50);
            vTaskDelay(pdMS_TO_TICKS(3000));
            
            set_motor_speed(MOTOR_INTAKE, 75);
            vTaskDelay(pdMS_TO_TICKS(3000));
            
            set_motor_speed(MOTOR_OFF, 0);
            vTaskDelay(pdMS_TO_TICKS(5000));
        }
    }
}

void app_main(void)
{
    ESP_LOGI(TAG, "Starting WiFi + Motor Control!");
    
    // Initialize NVS (needed for WiFi)
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);
    
    // Configure hardware
    configure_gpio_pins();
    configure_pwm();
    
    ESP_LOGI(TAG, "Connecting to WiFi network: %s", WIFI_SSID);
    wifi_init_sta();
    
    // Create motor demo task
    xTaskCreate(motor_demo_task, "motor_demo", 4096, NULL, 5, NULL);
    
    ESP_LOGI(TAG, "Setup complete! Motor controllable via WiFi.");
}