#include <stdio.h>
#include <string.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include "driver/gpio.h"
#include "esp_random.h"
#include "sdkconfig.h"
#include <math.h>
#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
// ### Global State Variables ###

#define LED_PIN GPIO_NUM_32


typedef enum {
    NORMAL_FLIGHT,
    LANDING
} FlightMode;

volatile double current_altitude = 7.5;
volatile FlightMode current_mode = NORMAL_FLIGHT;

bool mechanism_deployed = false;
// LED GPIO definitions
int 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};
const int NUM_LEDS = 10;
const double MAX_ALTITUDE_DISPLAY = 15.0; // Max altitude for the LED bar

    typedef enum {
        SIM_STATE_NORMAL_FLIGHT,
        SIM_STATE_LANDING,
        SIM_STATE_FINISHED
    } SimulatorState;

    SimulatorState state = SIM_STATE_NORMAL_FLIGHT;
    
// ### 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);
}

// Helper to constrain a value within a range
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) {
    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
        }
    }
}


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

    while (state != SIM_STATE_FINISHED) {
        // Use a longer delay to ensure the altitude has a chance to change.
        // Let's sample slightly slower than the simulation updates.
        vTaskDelay(pdMS_TO_TICKS(300));

        // --- Measure actual time elapsed ---
        TickType_t current_ticks = xTaskGetTickCount();
        TickType_t elapsed_ticks = current_ticks - last_run_ticks;
        
        // Prevent division by zero on the first loop or if ticks wrap around
        if (elapsed_ticks == 0) {
            last_run_ticks = current_ticks; // Update and skip to next cycle
            continue;
        }
        
        // Convert elapsed ticks to seconds (as a float)
        double elapsed_seconds = (double)elapsed_ticks * portTICK_PERIOD_MS / 1000.0;

        // --- Calculate speed ---
        double altitude_change = current_altitude - last_altitude;
        double speed_mps = altitude_change / elapsed_seconds;

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

        // --- Update state for the next iteration ---
        last_altitude = current_altitude;
        last_run_ticks = current_ticks;
    }
        vTaskDelete(NULL); // The task cleans itself up

}


void altitude_simulation_task(void *pvParameters) {
    // --- Internal State for the State Machine ---
    printf("[SIMULATOR] Starting flight simulation.\n");

    // --- Time-based control variables ---
    // We will control duration using system ticks, not loop iterations.
    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)
    );
    
    // The main task loop. It runs continuously until the state is SIM_FINISHED.
    while (state != SIM_STATE_FINISHED) {

        // --- State Machine Logic ---
        switch (state) {
            case SIM_STATE_NORMAL_FLIGHT:
                // Set the global mode for other tasks to see
                current_mode = NORMAL_FLIGHT;
                
                // Update altitude with fluctuations
                current_altitude = NORMAL_FLIGHT_ALTITUDE_M + get_random_number(-NORMAL_FLIGHT_FLUCTUATION_M, NORMAL_FLIGHT_FLUCTUATION_M);

                // Check if it's time to transition to the next state
                if ((xTaskGetTickCount() - flight_phase_start_ticks) >= normal_flight_duration_ticks) {
                    printf("[SIMULATOR] >>> LANDING SEQUENCE INITIATED <<<\n");
                    current_altitude = NORMAL_FLIGHT_ALTITUDE_M; // Stabilize before landing
                    state = SIM_STATE_LANDING;                   // Transition to the next state
                }
                break;

            case SIM_STATE_LANDING:
                // Set the global mode for other tasks
                current_mode = LANDING;
                
                // Decrease altitude
                current_altitude -= get_random_number(LANDING_DESCENT_MIN_M_PER_TICK, LANDING_DESCENT_MAX_M_PER_TICK);

                // Check for landing completion
                if (current_altitude <= 0) {
                    current_altitude = 0;   // Clamp to zero to be precise
                    state = SIM_STATE_FINISHED; // Transition to the final state
                }
                break;

            case SIM_STATE_FINISHED:
                // This case is handled by the while loop condition, but it's good practice to have it.
                break;
        }

        // --- Actions that happen in every state (except finished) ---
        update_led_bar_graph(current_altitude);
        printf("[SIMULATOR] State: %s, Altitude: %.2f m\n", 
               (state == SIM_STATE_NORMAL_FLIGHT) ? "Normal" : "Landing", 
               current_altitude);

        // Delay for the next simulation tick.
        vTaskDelay(pdMS_TO_TICKS(SIMULATION_TICK_MS));
    }

    // --- Cleanup Phase ---
    // This code is only reached once the while loop exits (state == SIM_FINISHED)
    update_led_bar_graph(current_altitude); // Final update to show 0 LEDs
    
    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); // The task cleans itself up
}

#define DESCENT_CONFIRMATION_COUNT 4 // Require 5 consecutive readings of descent

void landing_mechanism_task(void *pvParameters) {
    
    // We need to store the altitude from the previous check
    double previous_altitude = current_altitude; 
    
    // This is our counter for steady descent
    int descent_count = 0;

    while (1) {
        
        // Only run the logic if the gear is not already deployed
        if (!mechanism_deployed) {

            // Step 1: Check if the drone is descending
            if (current_altitude <= previous_altitude) {
                descent_count++; // Increment the counter
                printf("descent counter %d \n",  descent_count);
            } else {
            if(descent_count > 0)descent_count -= 1; // Reset if altitude increases or is stable
            }

            // Step 2: Check for deployment condition
            // The drone must be in landing mode AND have a confirmed steady descent AND be below a safe altitude
            if (descent_count >= DESCENT_CONFIRMATION_COUNT && 
                current_altitude < 5.0) {
                
                mechanism_deployed = true;
                gpio_set_level(LED_PIN, mechanism_deployed);
                printf("[MECHANISM] Steady descent confirmed! Deploying landing gear at %.2f m.\n", current_altitude);
                vTaskDelay(1000 / portTICK_PERIOD_MS); // Simulate servo time
                printf("[MECHANISM] Landing gear deployed at %.2f m.\n", current_altitude);
                vTaskDelete(NULL);
            }
        }

        previous_altitude = current_altitude;

        vTaskDelay(200 / portTICK_PERIOD_MS); // Check 5 times per second
    }
}

// void sanity_check(void *pvParameters){
//   // char buff[2048] = {0};
//   uint32_t buf =  0;
// while(1){
//   // vTaskGetRunTimeStats(buff);
//   // vTaskGetRunTimeStats
//   buf =  getFreeHeap();
//   printf(buf);
//   printf("\n");
//   vTaskDelay(500/ portTICK_PERIOD_MS);
  
// }

// }


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

    for (int i = 0; i < NUM_LEDS; i++) {
        gpio_reset_pin(leds[i]);
        gpio_set_direction(leds[i], GPIO_MODE_OUTPUT);
    }

    gpio_reset_pin(LED_PIN);
    gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);
    gpio_set_level(LED_PIN, 0);
    

    xTaskCreate(altitude_simulation_task, "Altitude Simulator", 4096, NULL, 5, NULL);
    xTaskCreate(speed_measurement_task, "Read speed", 4096, NULL, 2, NULL);
    xTaskCreate(landing_mechanism_task, "Landing Mechanism", 2048, NULL, 5, NULL); 
    // xTaskCreate(sanity_check, "sanity check", 2048, NULL, 3, NULL);
}