/* --------------------------------------------------------------
   Application: Satellite Control System for Kerbodyne CubeSats
   Class: Real Time Systems - Su 2025
   Updated: Thermal model stabilization, alert handling, logic analyzer
   AI-Assisted: Physics model, non-blocking alerts, state management
---------------------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "driver/gpio.h"
#include "esp_adc/adc_oneshot.h"
#include "esp_random.h"
#include "esp_log.h"
#include "esp_system.h"  // For reboot functionality

// System Components
#define LED_GREEN GPIO_NUM_5
#define LED_RED   GPIO_NUM_4
#define LED_BLUE  GPIO_NUM_15
#define BUTTON_PIN GPIO_NUM_18
#define POT_ADC_CHANNEL ADC_CHANNEL_6

// Logic Analyzer Pins (AI: GPIO selection for timing proof)
#define LOGIC_RAD   12  // Radiation task timing
#define LOGIC_TEMP  13  // Temperature task timing
#define LOGIC_ALERT 14  // Alert handler activation

// Timing constraints (ms)
#define RAD_TASK_PERIOD 100
#define TEMP_TASK_PERIOD 200
#define COMMS_TASK_PERIOD 500
#define HEARTBEAT_PERIOD 1000
#define STATUS_LOG_INTERVAL 2000

// Thresholds
#define HEAT_WARNING_THRESHOLD 2500.0f
#define HEAT_ALERT_THRESHOLD 3000.0f
#define TEMP_ALERT_THRESHOLD 80.0f
#define CRITICAL_TEMP_THRESHOLD 90.0f
#define MAX_HEAT_ENERGY 5000.0f
#define HYSTERESIS_FACTOR 0.95f

// Physics Constants (AI: Thermal model parameters)
#define THERMAL_MASS 40.0f        // J/°C
#define RADIATION_POWER_FACTOR 0.01f  // W per ADC unit
#define COOLING_RATE 0.1f         // W/°C
#define AMBIENT_TEMP 3.0f         // K (deep space)

// Alert event structure
typedef struct {
    char source[16];
    float value;
} AlertEvent;

// FreeRTOS objects
SemaphoreHandle_t sem_emergency;
SemaphoreHandle_t print_mutex;
SemaphoreHandle_t state_mutex;
QueueHandle_t alert_queue;
TaskHandle_t comms_task_handle;

// ADC Unit Handle
adc_oneshot_unit_handle_t adc1_handle;

// System state
typedef enum {
    NOMINAL,
    HEAT_ALERT,
    TEMP_ALERT,
    EMERGENCY
} SystemState;

// Thermal state
typedef struct {
    SystemState sys_state;
    float current_temp;    // °C
    float heat_accum;      // J
    int rad_level;         // ADC reading
} SystemState_t;

SystemState_t system_state = {
    .sys_state = NOMINAL,
    .current_temp = 30.0f,
    .heat_accum = (30.0f - AMBIENT_TEMP) * THERMAL_MASS,
    .rad_level = 0
};

// Global shutdown flag
bool critical_shutdown = false;

// IRQ for emergency button
void IRAM_ATTR emergency_isr(void* arg) {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xSemaphoreGiveFromISR(sem_emergency, &xHigherPriorityTaskWoken);
    if (xHigherPriorityTaskWoken == pdTRUE) {
        portYIELD_FROM_ISR();
    }
}

// Radiation monitoring (AI: Thermal model implementation)
void radiation_task(void *pvParameters) {
    const TickType_t xPeriod = pdMS_TO_TICKS(RAD_TASK_PERIOD);
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const float fixed_delta_t = RAD_TASK_PERIOD / 1000.0f;  // Fixed 100ms step
    TickType_t last_warning_time = 0;
    
    while (1) {
        if (critical_shutdown) {
            vTaskDelay(portMAX_DELAY);
        }
        
        // Toggle for logic analyzer
        gpio_set_level(LOGIC_RAD, 1);
        
        int rad_level = 0;
        adc_oneshot_read(adc1_handle, POT_ADC_CHANNEL, &rad_level);
        system_state.rad_level = rad_level;
        
        xSemaphoreTake(state_mutex, portMAX_DELAY);
        
        /* AI-Assisted Physics Model:
           - Energy input from radiation
           - Newton's law of cooling
           - Thermal mass relationship
        */
        float rad_power = rad_level * RADIATION_POWER_FACTOR; // Watts
        float temp_diff = system_state.current_temp - AMBIENT_TEMP;
        float cooling = COOLING_RATE * temp_diff * fixed_delta_t;
        float energy_input = rad_power * fixed_delta_t;
        
        system_state.heat_accum = fmaxf(0, system_state.heat_accum + energy_input - cooling);
        system_state.current_temp = AMBIENT_TEMP + system_state.heat_accum / THERMAL_MASS;
        
        if (system_state.heat_accum > MAX_HEAT_ENERGY) {
            system_state.heat_accum = MAX_HEAT_ENERGY;
            system_state.current_temp = AMBIENT_TEMP + MAX_HEAT_ENERGY / THERMAL_MASS;
        }
        
        // Clear heat alert with hysteresis
        if (system_state.sys_state == HEAT_ALERT && 
            system_state.heat_accum < HEAT_ALERT_THRESHOLD * HYSTERESIS_FACTOR) {
            system_state.sys_state = NOMINAL;
        }
        
        // Trigger new heat alert
        bool trigger_alert = false;
        if (system_state.heat_accum > HEAT_ALERT_THRESHOLD && 
            system_state.sys_state == NOMINAL) {
            system_state.sys_state = HEAT_ALERT;
            trigger_alert = true;
        }
        
        xSemaphoreGive(state_mutex);
        
        // Periodic heat warnings (AI: Timing implementation)
        TickType_t now = xTaskGetTickCount();
        if (system_state.heat_accum > HEAT_WARNING_THRESHOLD) {
            if (now - last_warning_time > pdMS_TO_TICKS(1000)) {
                xSemaphoreTake(print_mutex, portMAX_DELAY);
                printf("[WARNING] Heat energy critical: %.1fJ\n", system_state.heat_accum);
                xSemaphoreGive(print_mutex);
                last_warning_time = now;
            }
        }
        
        if (trigger_alert) {
            AlertEvent heat_alert = {.source = "HEAT ENERGY", .value = system_state.heat_accum};
            xQueueSend(alert_queue, &heat_alert, 0);
        }
        
        gpio_set_level(LOGIC_RAD, 0);  // End of task
        vTaskDelayUntil(&xLastWakeTime, xPeriod);
    }
}

// Temperature monitoring
void temperature_task(void *pvParameters) {
    const TickType_t xPeriod = pdMS_TO_TICKS(TEMP_TASK_PERIOD);
    TickType_t xLastWakeTime = xTaskGetTickCount();
    TickType_t last_log_time = 0;
    TickType_t last_temp_warning = 0;
    
    while (1) {
        if (critical_shutdown) {
            vTaskDelay(portMAX_DELAY);
        }
        
        gpio_set_level(LOGIC_TEMP, 1);  // Task start
        
        xSemaphoreTake(state_mutex, portMAX_DELAY);
        float current_temp = system_state.current_temp;
        float heat_accum = system_state.heat_accum;
        int rad_level = system_state.rad_level;
        SystemState current_state = system_state.sys_state;
        
        // Clear temp alert with hysteresis
        if (system_state.sys_state == TEMP_ALERT && 
            system_state.current_temp < TEMP_ALERT_THRESHOLD * HYSTERESIS_FACTOR) {
            system_state.sys_state = NOMINAL;
        }
        
        // Critical shutdown
        if (current_temp > CRITICAL_TEMP_THRESHOLD && !critical_shutdown) {
            critical_shutdown = true;
            printf("\n!!! CRITICAL SHUTDOWN INITIATED !!!\n");
            printf("Temperature exceeded safety limit: %.1f°C\n", current_temp);
            printf("Press EMERGENCY button to reboot system\n");
            
            gpio_set_level(LED_GREEN, 0);
            gpio_set_level(LED_BLUE, 0);
            gpio_set_level(LED_RED, 1);
        }
        xSemaphoreGive(state_mutex);
        
        // Temp alert detection
        bool trigger_alert = false;
        if (current_temp > TEMP_ALERT_THRESHOLD && 
            current_state == NOMINAL) {
            xSemaphoreTake(state_mutex, portMAX_DELAY);
            if (system_state.sys_state == NOMINAL) {
                system_state.sys_state = TEMP_ALERT;
                trigger_alert = true;
            }
            xSemaphoreGive(state_mutex);
        }
        
        if (trigger_alert) {
            AlertEvent temp_alert = {.source = "TEMPERATURE", .value = current_temp};
            xQueueSend(alert_queue, &temp_alert, 0);
        }
        
        // Periodic temp warnings (AI: Timing implementation)
        TickType_t now = xTaskGetTickCount();
        if (current_temp > TEMP_ALERT_THRESHOLD) {
            if (now - last_temp_warning > pdMS_TO_TICKS(1000)) {
                xSemaphoreTake(print_mutex, portMAX_DELAY);
                printf("[CRITICAL] Temperature critical: %.1f°C\n", current_temp);
                xSemaphoreGive(print_mutex);
                last_temp_warning = now;
            }
        }
        
        // Status logging
        if (now - last_log_time >= pdMS_TO_TICKS(STATUS_LOG_INTERVAL) && !critical_shutdown) {
            xSemaphoreTake(print_mutex, portMAX_DELAY);
            printf("[STATUS] Temp: %.1f°C | Heat: %.1fJ | Rad: %d | State: %d\n", 
                   current_temp, heat_accum, rad_level, system_state.sys_state);
            xSemaphoreGive(print_mutex);
            last_log_time = now;
        }
        
        // Shutdown handling
        if (critical_shutdown) {
            gpio_set_level(LED_RED, 1);
            vTaskDelay(pdMS_TO_TICKS(500));
            gpio_set_level(LED_RED, 0);
            vTaskDelay(pdMS_TO_TICKS(500));
        } else {
            gpio_set_level(LOGIC_TEMP, 0);  // Task end
            vTaskDelayUntil(&xLastWakeTime, xPeriod);
        }
    }
}

// Alert handler (AI: Non-blocking state machine)
void alert_handler_task(void *pvParameters) {
    AlertEvent alert;
    const TickType_t timeout = pdMS_TO_TICKS(10);
    TickType_t alert_end_time = 0;
    bool alert_active = false;

    while (1) {
        if (critical_shutdown) {
            vTaskDelay(portMAX_DELAY);
        }
        
        if (!alert_active) {
            if (xQueueReceive(alert_queue, &alert, timeout)) {
                gpio_set_level(LOGIC_ALERT, 1);  // Alert start
                gpio_set_level(LED_RED, 1);
                
                xSemaphoreTake(print_mutex, portMAX_DELAY);
                printf("[ALERT!] %s CRITICAL: %.1f\n", alert.source, alert.value);
                xSemaphoreGive(print_mutex);
                
                alert_end_time = xTaskGetTickCount() + pdMS_TO_TICKS(500);
                alert_active = true;
            }
        } else {
            if (xTaskGetTickCount() >= alert_end_time) {
                gpio_set_level(LED_RED, 0);
                gpio_set_level(LOGIC_ALERT, 0);  // Alert end
                alert_active = false;
            }
            vTaskDelay(pdMS_TO_TICKS(10));
        }
    }
}

// Data transmission
void comms_task(void *pvParameters) {
    const TickType_t xPeriod = pdMS_TO_TICKS(COMMS_TASK_PERIOD);
    TickType_t xLastWakeTime = xTaskGetTickCount();
    
    while (1) {
        if (critical_shutdown) {
            vTaskDelay(portMAX_DELAY);
        }
        
        int transmit_time = 100 + (esp_random() % 300);
        gpio_set_level(LED_BLUE, 1);
        vTaskDelay(pdMS_TO_TICKS(transmit_time));
        gpio_set_level(LED_BLUE, 0);
        
        xSemaphoreTake(print_mutex, portMAX_DELAY);
        printf("[COMMS] Data packet transmitted (%dms)\n", transmit_time);
        xSemaphoreGive(print_mutex);
        
        vTaskDelayUntil(&xLastWakeTime, xPeriod);
    }
}

// Emergency override (AI: Reboot feature)
void emergency_task(void *pvParameters) {
    while (1) {
        if (xSemaphoreTake(sem_emergency, portMAX_DELAY)) {
            if (critical_shutdown) {
                printf("\n!!! REBOOTING SYSTEM !!!\n");
                vTaskDelay(pdMS_TO_TICKS(100));
                esp_restart();
            }
            
            xSemaphoreTake(state_mutex, portMAX_DELAY);
            system_state.sys_state = EMERGENCY;
            xSemaphoreGive(state_mutex);
            
            gpio_set_level(LED_RED, 1);
            xSemaphoreTake(print_mutex, portMAX_DELAY);
            printf("[EMERGENCY] SAFETY OVERRIDE ACTIVATED!\n");
            xSemaphoreGive(print_mutex);
            
            vTaskSuspend(comms_task_handle);
            vTaskDelay(pdMS_TO_TICKS(5000));
            gpio_set_level(LED_RED, 0);
            
            xSemaphoreTake(state_mutex, portMAX_DELAY);
            system_state.sys_state = NOMINAL;
            xSemaphoreGive(state_mutex);
            
            vTaskResume(comms_task_handle);
        }
    }
}

// System heartbeat
void heartbeat_task(void *pvParameters) {
    while (1) {
        if (critical_shutdown) {
            gpio_set_level(LED_GREEN, 0);
            vTaskDelay(portMAX_DELAY);
        }
        
        gpio_set_level(LED_GREEN, 1);
        vTaskDelay(pdMS_TO_TICKS(HEARTBEAT_PERIOD / 2));
        gpio_set_level(LED_GREEN, 0);
        vTaskDelay(pdMS_TO_TICKS(HEARTBEAT_PERIOD / 2));
    }
}

void app_main() {
    // Initialize hardware
    gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << LED_GREEN) | (1ULL << LED_RED) | (1ULL << LED_BLUE) |
                        (1ULL << LOGIC_RAD) | (1ULL << LOGIC_TEMP) | (1ULL << LOGIC_ALERT),
        .mode = GPIO_MODE_OUTPUT,
    };
    gpio_config(&io_conf);
    
    gpio_config_t btn_conf = {
        .pin_bit_mask = (1ULL << BUTTON_PIN),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_ENABLE,
        .intr_type = GPIO_INTR_NEGEDGE
    };
    gpio_config(&btn_conf);
    
    gpio_install_isr_service(0);
    gpio_isr_handler_add(BUTTON_PIN, emergency_isr, NULL);
    
    // Initialize ADC
    adc_oneshot_unit_init_cfg_t adc_config = { .unit_id = ADC_UNIT_1 };
    adc_oneshot_new_unit(&adc_config, &adc1_handle);
    
    adc_oneshot_chan_cfg_t channel_config = {
        .bitwidth = ADC_BITWIDTH_12,
        .atten = ADC_ATTEN_DB_12,
    };
    adc_oneshot_config_channel(adc1_handle, POT_ADC_CHANNEL, &channel_config);
    
    // Create RTOS primitives
    sem_emergency = xSemaphoreCreateBinary();
    print_mutex = xSemaphoreCreateMutex();
    state_mutex = xSemaphoreCreateMutex();
    alert_queue = xQueueCreate(10, sizeof(AlertEvent));
    
    // Create tasks
    xTaskCreate(radiation_task, "RAD Monitor", 2048, NULL, 4, NULL);
    xTaskCreate(temperature_task, "TEMP Monitor", 2048, NULL, 4, NULL);
    xTaskCreate(comms_task, "Data Comms", 2048, NULL, 2, &comms_task_handle);
    xTaskCreate(alert_handler_task, "Alert Handler", 2048, NULL, 5, NULL);
    xTaskCreate(emergency_task, "Emergency", 2048, NULL, 6, NULL);
    xTaskCreate(heartbeat_task, "Heartbeat", 1024, NULL, 1, NULL);
    
    // Initial status
    printf("Satellite Control System INITIALIZED\n");
    printf("Thermal Mass: %.1f J/K\n", THERMAL_MASS);
    printf("Radiation Power Factor: %.4f W/unit\n", RADIATION_POWER_FACTOR);
    printf("Cooling Rate: %.3f W/K\n", COOLING_RATE);
    printf("Heat Warning Threshold: %.1fJ\n", HEAT_WARNING_THRESHOLD);
    printf("Heat Alert Threshold: %.1fJ\n", HEAT_ALERT_THRESHOLD);
    printf("Temp Alert Threshold: %.1f°C\n", TEMP_ALERT_THRESHOLD);
    printf("Critical Shutdown Temperature: %.1f°C\n", CRITICAL_TEMP_THRESHOLD);
}