#include <stdio.h>
#include <string.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h> // Needed for queues
#include "driver/gpio.h"
#include "esp_random.h"
#include "sdkconfig.h"
#include <math.h>

// ### Simulation Constants ###
#define SIMULATION_TICK_MS              250     // How often the main simulation loop runs
#define NORMAL_FLIGHT_ALTITUDE_M        7.5
#define NORMAL_FLIGHT_FLUCTUATION_M     5.5     // Altitude can vary by +/- this amount
#define NORMAL_FLIGHT_MIN_DURATION_S    5
#define NORMAL_FLIGHT_MAX_DURATION_S    10
#define LANDING_DESCENT_MIN_M_PER_TICK  0.05    // Min altitude lost per simulation tick
#define LANDING_DESCENT_MAX_M_PER_TICK  0.4     // Max altitude lost per simulation tick

// ### GPIO Definitions ###
// LED Bar Graph
#define NUM_LEDS 10
const int leds[NUM_LEDS] = {GPIO_NUM_23, GPIO_NUM_22, GPIO_NUM_15, GPIO_NUM_19, GPIO_NUM_18, 
                           GPIO_NUM_5, GPIO_NUM_17, GPIO_NUM_16, GPIO_NUM_4, GPIO_NUM_2};

// Landing Gear LED
#define LANDING_GEAR_LED_PIN GPIO_NUM_14

// --- NEW: Buttons and Indicator LEDs ---

// #define BUTTON_1_PIN GPIO_NUM_25  // Force Landing iki sing bener

#define BUTTON_1_PIN GPIO_NUM_29  // Force Landing
#define BUTTON_2_PIN GPIO_NUM_26  // Pause/Resume Simulation
#define BUTTON_3_PIN GPIO_NUM_27  // Manual Gear Deploy

// #define INDICATOR_LED_1_PIN GPIO_NUM_34 // Corresponds to Button 1 

#define INDICATOR_LED_1_PIN GPIO_NUM_30 // Corresponds to Button 1
#define INDICATOR_LED_2_PIN GPIO_NUM_35 // Corresponds to Button 2
#define INDICATOR_LED_3_PIN GPIO_NUM_32 // Corresponds to Button 3

// --- NEW: Interrupt and Debounce Constants ---
#define DEBOUNCE_TIME_MS 50 // 50ms debounce time
#define ESP_INTR_FLAG_DEFAULT 0


// ### Global State Variables ###
typedef enum {
    NORMAL_FLIGHT,
    LANDING
} FlightMode;

typedef enum {
    SIM_STATE_NORMAL_FLIGHT,
    SIM_STATE_LANDING,
    SIM_STATE_FINISHED
} SimulatorState;

// Volatile because they can be changed by different tasks/ISRs
volatile double current_altitude = 7.5;
volatile FlightMode current_mode = NORMAL_FLIGHT;
volatile SimulatorState state = SIM_STATE_NORMAL_FLIGHT;
volatile bool mechanism_deployed = false;
volatile bool is_simulation_paused = false; // NEW: For pause/resume functionality

// ### RTOS Handles ###
// NEW: Queue to handle events from GPIO interrupts
static QueueHandle_t gpio_evt_queue = NULL; 
// NEW: Handle to the simulation task to allow pausing/resuming it
static TaskHandle_t altitude_simulation_task_handle = NULL;


// ### Utility Functions ###
float get_random_number(float minValue, float maxValue) {
    uint32_t rand = esp_random();
    float rand_float = (float)rand / (float)UINT32_MAX;
    return minValue + rand_float * (maxValue - minValue);
}

double constrain_value(double value, double min, double max) {
    if (value < min) return min;
    if (value > max) return max;
    return value;
}

void update_led_bar_graph(double altitude) {
    const double MAX_ALTITUDE_DISPLAY = 15.0;
    double constrained_alt = constrain_value(altitude, 0.0, MAX_ALTITUDE_DISPLAY);
    int leds_to_light = (int)(constrained_alt / MAX_ALTITUDE_DISPLAY * NUM_LEDS);

    for (int i = 0; i < NUM_LEDS; i++) {
        if (i < leds_to_light) {
            gpio_set_level(leds[i], 1); // Turn ON
        } else {
            gpio_set_level(leds[i], 0); // Turn OFF
        }
    }
}


// ### NEW: Interrupt Service Routine (ISR) ###
// This function is called every time a button press triggers an interrupt.
// It must be fast and should not block.
static void IRAM_ATTR gpio_isr_handler(void* arg) {
    // Basic debouncing
    static uint32_t last_interrupt_time = 0;
    uint32_t current_time = xTaskGetTickCountFromISR() * portTICK_PERIOD_MS;
    vTaskDelay(pdMS_TO_TICKS(900));
    // if (current_time - last_interrupt_time > DEBOUNCE_TIME_MS) {
    if(true){
        uint32_t gpio_num = (uint32_t) arg;
        // Send the GPIO number to the queue to be processed by a task
        // xQueueSendFromISR(gpio_evt_queue, &gpio_num, NULL);
        xQueueSend(gpio_evt_queue, &gpio_num, NULL);
        last_interrupt_time = current_time;
    }
}


// ### NEW: Button Handler Task ###
// This task waits for events from the ISR queue and handles the button logic.
void button_handler_task(void* arg) {
    uint32_t io_num;
    char buffer[2000];
    while (1) {
        // Wait forever for a message to appear in the queue
        // if (xQueueReceive(gpio_evt_queue, &io_num, portMAX_DELAY)) {
        if (xQueueReceive(gpio_evt_queue, *io_num, portMAX_DELAY)) {
            printf("[BUTTON] GPIO[%lu] intr, val: %d\n", io_num, gpio_get_level(io_num));

            switch (io_num) {
                case BUTTON_1_PIN:
                    printf("[BUTTON] Forcing LANDING state.\n");
                    // Action: Force the simulation into landing mode
                    if (state == SIM_STATE_NORMAL_FLIGHT) {
                        state = SIM_STATE_LANDING;
                    }
                    // Visual Feedback: Blink the indicator LED
                    gpio_set_level(INDICATOR_LED_1_PIN, 1);
                    vTaskDelay(pdMS_TO_TICKS(200));
                    gpio_set_level(INDICATOR_LED_1_PIN, 0);
                    break;

                case BUTTON_2_PIN:
                    is_simulation_paused = !is_simulation_paused;
                    if (is_simulation_paused) {
                        printf("[BUTTON] Pausing simulation.\n");
                        // Action: Suspend the main simulation task
                        if (altitude_simulation_task_handle != NULL) {
                           vTaskSuspend(altitude_simulation_task_handle);
                        }
                        // Visual Feedback: Turn LED on while paused
                        gpio_set_level(INDICATOR_LED_2_PIN, 1); 
                    } else {
                        printf("[BUTTON] Resuming simulation.\n");
                        // Action: Resume the main simulation task
                        if (altitude_simulation_task_handle != NULL) {
                           vTaskResume(altitude_simulation_task_handle);
                        }
                        // Visual Feedback: Turn LED off when running
                        gpio_set_level(INDICATOR_LED_2_PIN, 0);
                    }
                    break;

                case BUTTON_3_PIN:
                     printf("[BUTTON] Manual landing gear deploy.\n");
                    // Action: Manually deploy the landing gear
                    if (!mechanism_deployed) {
                        mechanism_deployed = true;
                        gpio_set_level(LANDING_GEAR_LED_PIN, 1);
                        printf("[MECHANISM] Manual override! Landing gear deployed.\n");
                    }
                    // Visual Feedback: Blink the indicator LED
                    gpio_set_level(INDICATOR_LED_3_PIN, 1);
                    vTaskDelay(pdMS_TO_TICKS(200));
                    gpio_set_level(INDICATOR_LED_3_PIN, 0);
                    break;
            }
        }
    }
}


void speed_measurement_task(void *pvParameters) {
    double last_altitude = current_altitude;
    TickType_t last_run_ticks = xTaskGetTickCount();

    while (state != SIM_STATE_FINISHED) {
        vTaskDelay(pdMS_TO_TICKS(300));

        // If paused, just loop without doing calculations
        if (is_simulation_paused) continue;

        TickType_t current_ticks = xTaskGetTickCount();
        TickType_t elapsed_ticks = current_ticks - last_run_ticks;
        
        if (elapsed_ticks == 0) {
            last_run_ticks = current_ticks;
            continue;
        }
        
        double elapsed_seconds = (double)elapsed_ticks * portTICK_PERIOD_MS / 1000.0;
        double altitude_change = current_altitude - last_altitude;
        double speed_mps = altitude_change / elapsed_seconds;

        printf("[SPEED] Vertical Speed: %.2f m/s\n", speed_mps);

        last_altitude = current_altitude;
        last_run_ticks = current_ticks;
    }
    vTaskDelete(NULL);
}


void altitude_simulation_task(void *pvParameters) {
    printf("[SIMULATOR] Starting flight simulation.\n");

    TickType_t flight_phase_start_ticks = xTaskGetTickCount();
    TickType_t normal_flight_duration_ticks = pdMS_TO_TICKS(
        get_random_number(NORMAL_FLIGHT_MIN_DURATION_S * 1000, NORMAL_FLIGHT_MAX_DURATION_S * 1000)
    );
    
    while (state != SIM_STATE_FINISHED) {
        switch (state) {
            case SIM_STATE_NORMAL_FLIGHT:
                current_mode = NORMAL_FLIGHT;
                current_altitude = NORMAL_FLIGHT_ALTITUDE_M + get_random_number(-NORMAL_FLIGHT_FLUCTUATION_M, NORMAL_FLIGHT_FLUCTUATION_M);

                if ((xTaskGetTickCount() - flight_phase_start_ticks) >= normal_flight_duration_ticks) {
                    printf("[SIMULATOR] >>> LANDING SEQUENCE INITIATED <<<\n");
                    current_altitude = NORMAL_FLIGHT_ALTITUDE_M;
                    state = SIM_STATE_LANDING;
                }
                break;

            case SIM_STATE_LANDING:
                current_mode = LANDING;
                current_altitude -= get_random_number(LANDING_DESCENT_MIN_M_PER_TICK, LANDING_DESCENT_MAX_M_PER_TICK);

                if (current_altitude <= 0) {
                    current_altitude = 0;
                    state = SIM_STATE_FINISHED;
                }
                break;

            case SIM_STATE_FINISHED:
                break;
        }

        update_led_bar_graph(current_altitude);
        printf("[SIMULATOR] State: %s, Altitude: %.2f m\n", 
               (state == SIM_STATE_NORMAL_FLIGHT) ? "Normal" : "Landing", 
               current_altitude);

        vTaskDelay(pdMS_TO_TICKS(SIMULATION_TICK_MS));
    }

    update_led_bar_graph(current_altitude);
    
    if (mechanism_deployed) {
        printf("[SIMULATOR] Landed safely. Deleting simulation task.\n");
    } else {
        printf("[SIMULATOR] CRASHED! Mechanism did not deploy. Deleting simulation task.\n");
    }

    vTaskDelete(NULL);
}

#define DESCENT_CONFIRMATION_COUNT 4

void landing_mechanism_task(void *pvParameters) {
    double previous_altitude = current_altitude; 
    int descent_count = 0;

    while (state != SIM_STATE_FINISHED) {
        if (is_simulation_paused) {
             vTaskDelay(pdMS_TO_TICKS(200)); // Still delay when paused to prevent busy-waiting
             continue;
        }
        
        if (!mechanism_deployed) {
            if (current_altitude <= previous_altitude) {
                descent_count++;
            } else {
                if (descent_count > 0) descent_count--;
            }

            if (descent_count >= DESCENT_CONFIRMATION_COUNT && 
                current_altitude < 5.0) {
                
                mechanism_deployed = true;
                gpio_set_level(LANDING_GEAR_LED_PIN, mechanism_deployed);
                printf("[MECHANISM] Steady descent confirmed! Deploying landing gear at %.2f m.\n", current_altitude);
                vTaskDelay(pdMS_TO_TICKS(1000));
                printf("[MECHANISM] Landing gear deployed at %.2f m.\n", current_altitude);
                
                // This task has done its job, it can now exit.
                vTaskDelete(NULL); 
            }
        } else {
            // Mechanism is already deployed, no need for this task anymore.
             vTaskDelete(NULL);
        }

        previous_altitude = current_altitude;
        vTaskDelay(pdMS_TO_TICKS(200));
    }
     vTaskDelete(NULL); // Clean up if simulation finishes before deployment
}


void app_main() {
    printf("[MAIN] Starting Drone Landing System Test.\n");

    // Configure LED Bar Graph GPIOs
    for (int i = 0; i < NUM_LEDS; i++) {
        gpio_reset_pin(leds[i]);
        gpio_set_direction(leds[i], GPIO_MODE_OUTPUT);
    }

    // Configure Landing Gear LED GPIO
    gpio_reset_pin(LANDING_GEAR_LED_PIN);
    gpio_set_direction(LANDING_GEAR_LED_PIN, GPIO_MODE_OUTPUT);
    gpio_set_level(LANDING_GEAR_LED_PIN, 0);
    
    // --- NEW: Configure Buttons and Indicator LEDs ---
    gpio_config_t io_conf = {};
    // Button Configuration
    io_conf.intr_type = GPIO_INTR_NEGEDGE;      // Interrupt on falling edge (press)
    io_conf.pin_bit_mask = (1ULL << BUTTON_1_PIN) | (1ULL << BUTTON_2_PIN) | (1ULL << BUTTON_3_PIN);
    io_conf.mode = GPIO_MODE_INPUT;
    // io_conf.pull_up_en = 1;                     // Enable internal pull-up resistors
    io_conf.pull_up_en = false;                     // Enable internal pull-up resistors
    gpio_config(&io_conf);

    // Indicator LED Configuration
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.pin_bit_mask = (1ULL << INDICATOR_LED_1_PIN) | (1ULL << INDICATOR_LED_2_PIN) | (1ULL << INDICATOR_LED_3_PIN);
    io_conf.mode = GPIO_MODE_OUTPUT;
    // io_conf.pull_up_en = 0;
    io_conf.pull_up_en = 1;
    io_conf.pull_down_en = 0;
    gpio_config(&io_conf);
    
    // --- NEW: Setup Interrupt Handling ---
    // Create a queue to handle gpio events from isr
    gpio_evt_queue = xQueueCreate(10, sizeof(uint32_t));
    
    // Install gpio isr service
    // gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT);
 
    // Hook isr handler for specific gpio pins
    gpio_isr_handler_add(BUTTON_1_PIN, gpio_isr_handler, (void*) BUTTON_1_PIN);
    gpio_isr_handler_add(BUTTON_2_PIN, gpio_isr_handler, (void*) BUTTON_2_PIN);
    gpio_isr_handler_add(BUTTON_3_PIN, gpio_isr_handler, (void*) BUTTON_3_PIN);

    // --- Create all the tasks ---
    // xTaskCreate(button_handler_task, "Button Handler", 2048, NULL, 10, NULL);
    xTaskCreate(button_handler_task, "Button Handler", 1024, NULL, 5, NULL);
    xTaskCreate(altitude_simulation_task, "Altitude Simulator", 4096, NULL, 5, &altitude_simulation_task_handle);
    // xTaskCreate(speed_measurement_task, "Read speed", 4096, NULL, 2, NULL);
    xTaskCreate(speed_measurement_task, "Read speed", 2048, NULL, 2, NULL);
    xTaskCreate(landing_mechanism_task, "Landing Mechanism", 2048, NULL, 5, NULL); 
}