// =================================================================
// ==     TESTE REAL DA LÓGICA DA MÁQUINA DE ESTADOS (FSM)        ==
// =================================================================
#include <stdio.h>
#include "driver/gpio.h"
#include "freertos/FreeRTOS.h"
#include <Arduino.h>
#include <math.h> // Incluído para a função fabsf (valor absoluto de float)
#include <Preferences.h>
#include "nvs_flash.h"
#include "nvs.h"

// =================================================================
// == 1. Globals.hpp: Definições Globais e Dados Compartilhados    ==
// =================================================================

enum FlightState_e {
    PAD_IDLE,
    POWERED_FLIGHT,
    COASTING,
    DESCENT,
    LANDED
};

struct AvionicsData {
    uint32_t timestamp_ms;
    float altitude;
    float accel_z;
    float angle_x;
    float angle_y;
};

enum Command_e {
    CMD_NO_COMMAND,
    CMD_ARM_SYSTEM,
    CMD_FORCE_EJECT,
    CMD_RESET_MEMORY,
    CMD_START_CALIBRATION,
    FORCE_BUTTON,
    CMD_BUTTON
};

AvionicsData g_avionicsData;
Preferences preferences;
SemaphoreHandle_t g_dataMutex = NULL;
EventGroupHandle_t g_dataEventGroup = NULL;
FlightState_e g_flightState = PAD_IDLE;
QueueHandle_t commandQueue = NULL;
const EventBits_t BIT_NEW_DATA_AVAILABLE = (1 << 0);
const uint8_t InterruptPin = 13;
bool flagSave;
volatile uint32_t last_interrupt_time = 0;
bool pressedButton = false;

// =================================================================
// == 2. Módulos com Funções Mockadas                             ==
// =================================================================

namespace Ejection {
    void trigger() { Serial.println("\n****************\n*** EJEÇÃO ACIONADA! ***\n****************\n"); }
}

namespace Telemetry {
    void sendData(const AvionicsData& data, FlightState_e state) {
        const char* stateNames[] = {"PAD_IDLE", "POWERED_FLIGHT", "COASTING", "DESCENT", "LANDED"};
        Serial.printf("[TELEMETRY] Estado: %s, Alt: %.2f m, Accel: %.2f G\n, AngleX: %.2f º ", stateNames[state], data.altitude, data.accel_z, data.angle_x);
    }
}

namespace Storage {
    void saveData(const AvionicsData& data) {
        // Se quiserem colocar uma mensagem, tirei para não poluir o log
    }
}

namespace Acquisiton {
    bool setup() { 
      return true; 
    }
      void readAllSensors(AvionicsData& data) {
    static unsigned long simStartTime = millis();
    unsigned long simTime = millis() - simStartTime;

    data.timestamp_ms = simTime;

    if (simTime < 3000) { // 0-3s: Na rampa
        data.accel_z = 1.0;
        data.altitude = 10.0;
        data.angle_x = 3.6f;   // -1.2 rad/s * 3 s
        data.angle_y = 2.4f;    // 0.8 rad/s * 3 s
    } else if (simTime < 3200) { // 3-3.2s: Pico de lançamento (200 ms depois)
        data.accel_z = 15.0;
        data.altitude = 15.0;
        data.angle_x = 4.04f;  // -3.6 + (-4.6 * 0.2)
        data.angle_y = 2.86f;   // 2.4 + (2.3 * 0.2)
    } else if (simTime < 7000) { // 3.2-7s: Voo propulsado (3.8 s)
        data.accel_z = 8.0;
        data.altitude = 10.0 + (simTime - 3000) * 0.2;
        data.angle_x = 19.42f; // -4.04 + (-8.2 * 3.8)
        data.angle_y = 8.06f;   // 2.86 + (4.0 * 3.8)
    } else if (simTime < 7500) { // 7-7.5s: Fim da queima (0.5 s)
        data.accel_z = 0.5;
        data.altitude = 810.0;
        data.angle_x = -21.97f; // -19.42 + (-5.1 * 0.5)
        data.angle_y = 9.86f;   // 8.06 + (3.6 * 0.5)
    } else if (simTime < 15000) { // 7.5-15s: Subida inercial (7.5 s)
        data.accel_z = -0.98;
        float t = (simTime - 7500) / 1000.0;
        data.altitude = 810 + 120 * t - 16 * t * t;
        data.angle_x = -40.72f; // -21.97 + (-2.5 * 7.5)
        data.angle_y = 21.11f;  // 9.86 + (1.5 * 7.5)
    } else if (simTime < 30000) { // 15-30s: Descida (15 s)
        data.accel_z = 0.2;
        float descent_start_alt = -1;
        if (descent_start_alt < 0) descent_start_alt = data.altitude;
        float descending_alt = descent_start_alt - (simTime - 15000) * 0.075;
        data.altitude = max(0.0f, descending_alt);
        data.angle_x = -55.72f; // -40.72 + (-1.0 * 15)
        data.angle_y = 31.61f;  // 21.11 + (0.7 * 15)
    } else { // 30s+: Pousado
        data.accel_z = 1.0;
        data.altitude = 10.0;
        data.angle_x = -55.72f; // Mantém último ângulo
        data.angle_y = 31.61f;
    }
}



     }


// =================================================================
// == 3. Implementação das Tarefas RTOS                           ==
// =================================================================

void task_data_acquisition(void* pvParameters);
void task_flight_state_manager(void* pvParameters);
void task_telemetry(void* pvParameters);
void task_storage(void* pvParameters);
void saveFlagToNVS(bool pressedButton);
void readFlagFromNVS();


void setup() {
    Serial.begin(115200);
    delay(1000);
    Serial.println("Iniciando Simulação da FSM Real da Aviônica PIG IV...");

    g_dataMutex = xSemaphoreCreateMutex();
    g_dataEventGroup = xEventGroupCreate();

    xTaskCreatePinnedToCore(task_data_acquisition, "Data Acq Task", 4096, NULL, 3, NULL, 1);
    xTaskCreatePinnedToCore(task_flight_state_manager, "FSM Task", 4096, NULL, 3, NULL, 1);
    xTaskCreatePinnedToCore(task_telemetry, "Telemetry Task", 4096, NULL, 2, NULL, 0);
    xTaskCreatePinnedToCore(task_storage, "Storage Task", 4096, NULL, 2, NULL, 0);


}

void IRAM_ATTR isr() {
  uint32_t current_time = xTaskGetTickCountFromISR(); // Tempo atual (ticks)
    // Debounce simples: ignora interrupções muito próximas (<200ms)
    if (current_time - last_interrupt_time > pdMS_TO_TICKS(150)) {
      Serial.println("Interrupt pressedButton Received!");
      pressedButton = true;

      last_interrupt_time = current_time;
    }
  }


void saveFlagToNVS(bool stateButton){
  flagSave = stateButton;
  nvs_handle_t nvs_handle;
  nvs_open("FlagNVS", NVS_READWRITE, &nvs_handle);
  nvs_set_u8(nvs_handle, "flagSave", (uint8_t)pressedButton);
  nvs_commit(nvs_handle);
  nvs_close(nvs_handle);
        //---------------------//
}

void readFlagFromNVS(){
  nvs_handle_t nvs_handle;
  uint8_t fastFlag = 0;
  nvs_open("FlagNVS", NVS_READONLY, &nvs_handle);
  nvs_get_u8(nvs_handle, "flagSave", &fastFlag);
  nvs_close(nvs_handle);
  flagSave = (fastFlag);
}

void processCommand(Command_e cmd) {
  switch(cmd) {
    case CMD_BUTTON:
        {
          Serial.println("[CMD] Comando de RESET DE CICLO recebido.");
          if (pressedButton){
            pressedButton = false;
            saveFlagToNVS(pressedButton);
          }  
        }
        
        break;
    case CMD_FORCE_EJECT:
            //sem uso
        break;
    case CMD_RESET_MEMORY:
            //sem uso
        break;
    case CMD_START_CALIBRATION:
            //sem uso
        break;
    case CMD_NO_COMMAND:
        // Nenhum comando a processar
        break;
    default:
        break;
}
}

void loop() {
    vTaskDelete(NULL);
}

void task_data_acquisition(void* pvParameters) {
    for (;;) {
        AvionicsData localData;
        Acquisiton::readAllSensors(localData);
        
        xSemaphoreTake(g_dataMutex, portMAX_DELAY);
        g_avionicsData = localData;
        xSemaphoreGive(g_dataMutex);

        xEventGroupSetBits(g_dataEventGroup, BIT_NEW_DATA_AVAILABLE);
        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

void task_flight_state_manager(void* pvParameters) {

  void processCommand(Command_e cmd);
    pinMode(InterruptPin, INPUT_PULLUP);
    attachInterrupt(InterruptPin, isr, FALLING );
    static float launchAltitude = 0.0f;
    static float previousAltitude = 0.0f;
    static uint32_t previousTimestamp = 0;
    static int apogeeCounter = 0;
    static bool isFirstRunInCoasting = true;
    static bool launchAltitudeSet = false;

    for (;;) {
        xEventGroupWaitBits(g_dataEventGroup, BIT_NEW_DATA_AVAILABLE, pdTRUE, pdFALSE, portMAX_DELAY);

        Command_e received_cmd;
        if (xQueueReceive(commandQueue, &received_cmd, 0) == pdPASS) {
            processCommand(received_cmd);
        }
        AvionicsData localData;
        xSemaphoreTake(g_dataMutex, portMAX_DELAY);
        localData = g_avionicsData;
        xSemaphoreGive(g_dataMutex);

        switch (g_flightState) {

            case PAD_IDLE:
                if (!launchAltitudeSet && localData.altitude != 0.0f) {
                    launchAltitude = localData.altitude;
                    launchAltitudeSet = true;
                }
                if (localData.accel_z > 5.0f) {
                    Serial.println("\n[FSM] Lançamento detectado! Mudando para POWERED_FLIGHT\n");
                    g_flightState = POWERED_FLIGHT;
                }
                break;

            case POWERED_FLIGHT:
                if (localData.accel_z < 1.5f) {
                    Serial.println("\n[FSM] Fim da queima detectado! Mudando para COASTING\n");
                    g_flightState = COASTING;
                    isFirstRunInCoasting = true;
                    apogeeCounter = 0;
                }
                break;

            case COASTING:
                if (isFirstRunInCoasting) {
                    previousAltitude = localData.altitude;
                    isFirstRunInCoasting = false;
                } else {
                    // só contamos uma queda se ela for maior que 0.7m,
                    // tornando a detecção imune a ruídos e variações mínimas.
                    if (localData.altitude < (previousAltitude - 0.7f)) {
                      if(localData.angle_x || localData.angle_y < -15.0f){
                    // =======================================================
                        apogeeCounter++;
                        Serial.printf("[FSM] Queda de altitude SIGNIFICATIVA detectada! Contador: %d\n", apogeeCounter);
                      }
                    } else {
                        // Se a altitude sobe ou cai muito pouco, resetamos o contador.
                        apogeeCounter = 0;
                    }
                }

                previousAltitude = localData.altitude;

                if (apogeeCounter >= 3) {
                    Ejection::trigger();
                    g_flightState = DESCENT;
                }
                break;

            case DESCENT:
            {
                float dt = (localData.timestamp_ms - previousTimestamp) / 1000.0f;
                float vertical_velocity = 0.0;
                if (dt > 0) {
                   vertical_velocity = (localData.altitude - previousAltitude) / dt;
                }
                
                if (launchAltitudeSet && localData.altitude < launchAltitude + 30.0f && fabsf(vertical_velocity) < 1.0f) {
                    Serial.println("\n[FSM] Pouso detectado! Mudando para LANDED\n");
                    g_flightState = LANDED;
                }
                
                previousAltitude = localData.altitude;
                previousTimestamp = localData.timestamp_ms;
            }
            break;

            case LANDED:
                Serial.println("[FSM] Missão concluída. Sistema em modo de baixo consumo.");
                vTaskSuspend(NULL);
                break;
        }
    }
}

void task_telemetry(void* pvParameters) {
    for (;;) {
        xEventGroupWaitBits(g_dataEventGroup, BIT_NEW_DATA_AVAILABLE, pdTRUE, pdFALSE, portMAX_DELAY);
        
        AvionicsData localData;
        FlightState_e currentState;
        xSemaphoreTake(g_dataMutex, portMAX_DELAY);
        localData = g_avionicsData;
        currentState = g_flightState;
        xSemaphoreGive(g_dataMutex);
        
        Telemetry::sendData(localData, currentState);
    }
}

void task_storage(void* pvParameters) {
     for (;;) {
        xEventGroupWaitBits(g_dataEventGroup, BIT_NEW_DATA_AVAILABLE, pdTRUE, pdFALSE, portMAX_DELAY);
        
        AvionicsData localData;
        FlightState_e currentState;
        xSemaphoreTake(g_dataMutex, portMAX_DELAY);
        localData = g_avionicsData;
        currentState = g_flightState;
        xSemaphoreGive(g_dataMutex);
        
        if (currentState == POWERED_FLIGHT || currentState == COASTING || currentState == DESCENT) {
            // Storage::saveData(localData);
        }
    }
}