/*
 * MAGICRIDE CONTROL SYSTEM - Real-Time Prototype
 * Company: MagicRide Technologies (Orlando, FL)
 * Product: SafeCoaster™ Ride Control System
 * 
 * This prototype demonstrates real-time control for a theme park ride
 * with critical safety requirements and guest experience features.
 * 
 * AI Usage: Code structure inspired by FreeRTOS examples from
 * https://www.freertos.org/tutorial
 */

#include <Arduino.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>
#include <freertos/semphr.h>

// Hardware Pin Definitions
#define EMERGENCY_STOP_BTN 4    // Red button - Hard real-time
#define RIDE_START_BTN 5        // Green button - Soft real-time
#define HEARTBEAT_LED 2         // System health indicator
#define BRAKE_LED 15            // Brake status (RED)
#define MOTION_LED 13           // Motion status (GREEN)
#define TEMP_SENSOR_PIN 34      // Temperature sensor (analog)
#define SPEED_SENSOR_PIN 35     // Speed sensor simulation (analog)

// Task Periods and Deadlines (in ms)
#define SAFETY_MONITOR_PERIOD 10      // HARD: 10ms period/deadline
#define SENSOR_SCAN_PERIOD 50         // HARD: 50ms period/deadline  
#define MOTION_CONTROL_PERIOD 100     // SOFT: 100ms period/deadline
#define GUEST_DISPLAY_PERIOD 500      // SOFT: 500ms period/deadline

// System Constants
#define MAX_SAFE_TEMP 80              // Celsius
#define MAX_SAFE_SPEED 100            // Arbitrary units
#define QUEUE_SIZE 10
#define SENSOR_QUEUE_SIZE 20
#define TASK_STACK_SIZE 4096

// Global Synchronization Objects
SemaphoreHandle_t brakeMutex;        // Protects brake state
SemaphoreHandle_t emergencyFlag;     // Binary semaphore for e-stop
QueueHandle_t sensorDataQueue;       // Sensor readings queue
QueueHandle_t displayQueue;          // Guest display messages

// Shared State (protected by mutexes)
volatile bool brakeEngaged = true;   // Start with brakes on
volatile bool rideActive = false;    // Ride state
volatile uint32_t lastEmergencyTime = 0;

// Performance Monitoring
typedef struct {
    uint32_t taskStart;
    uint32_t taskEnd;
    uint32_t worstCase;
    uint32_t deadline;
    const char* taskName;
} TaskTiming_t;

TaskTiming_t timingData[4];

// Sensor Data Structure
typedef struct {
    float temperature;
    float speed;
    uint32_t timestamp;
} SensorData_t;

// Display Message Structure
typedef struct {
    char message[64];
    uint8_t priority;
} DisplayMsg_t;

/*
 * EMERGENCY STOP ISR (HARD REAL-TIME)
 * Deadline: < 1ms response time
 * Consequence of miss: Rider safety compromised
 */
void IRAM_ATTR emergencyStopISR() {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    lastEmergencyTime = millis();
    
    // Immediately engage brakes (critical section)
    brakeEngaged = true;
    digitalWrite(BRAKE_LED, HIGH);
    digitalWrite(MOTION_LED, LOW);
    
    // Signal emergency to safety monitor task
    xSemaphoreGiveFromISR(emergencyFlag, &xHigherPriorityTaskWoken);
    
    if (xHigherPriorityTaskWoken) {
        portYIELD_FROM_ISR();
    }
}

/*
 * TASK 1: SAFETY MONITOR (HARD REAL-TIME)
 * Period: 10ms, Deadline: 10ms
 * Purpose: Monitor emergency conditions and enforce safety
 * Consequence of deadline miss: Safety system failure
 */
void safetyMonitorTask(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xPeriod = pdMS_TO_TICKS(SAFETY_MONITOR_PERIOD);
    
    while (1) {
        uint32_t startTime = millis();
        
        // Check for emergency stop signal
        if (xSemaphoreTake(emergencyFlag, 0) == pdTRUE) {
            Serial.println("[SAFETY] EMERGENCY STOP ACTIVATED!");
            
            // Ensure brakes are engaged (double-check)
            if (xSemaphoreTake(brakeMutex, portMAX_DELAY) == pdTRUE) {
                brakeEngaged = true;
                rideActive = false;
                xSemaphoreGive(brakeMutex);
            }
            
            // Log to display
            DisplayMsg_t msg;
            snprintf(msg.message, sizeof(msg.message), "EMERGENCY STOP - Time: %lu", lastEmergencyTime);
            msg.priority = 1;
            xQueueSend(displayQueue, &msg, 0);
        }
        
        // Periodic safety checks
        SensorData_t sensorData;
        if (xQueuePeek(sensorDataQueue, &sensorData, 0) == pdTRUE) {
            bool safetyViolation = false;
            
            if (sensorData.temperature > MAX_SAFE_TEMP) {
                Serial.printf("[SAFETY] TEMP VIOLATION: %.1f°C\n", sensorData.temperature);
                safetyViolation = true;
            }
            
            if (sensorData.speed > MAX_SAFE_SPEED && !brakeEngaged) {
                Serial.printf("[SAFETY] SPEED VIOLATION: %.1f\n", sensorData.speed);
                safetyViolation = true;
            }
            
            if (safetyViolation) {
                if (xSemaphoreTake(brakeMutex, portMAX_DELAY) == pdTRUE) {
                    brakeEngaged = true;
                    rideActive = false;
                    digitalWrite(BRAKE_LED, HIGH);
                    digitalWrite(MOTION_LED, LOW);
                    xSemaphoreGive(brakeMutex);
                }
            }
        }
        
        // Timing tracking
        uint32_t endTime = millis();
        uint32_t execTime = endTime - startTime;
        if (execTime > timingData[0].worstCase) {
            timingData[0].worstCase = execTime;
        }
        timingData[0].taskEnd = endTime;
        
        // Wait for next period
        vTaskDelayUntil(&xLastWakeTime, xPeriod);
    }
}

/*
 * TASK 2: SENSOR SCANNER (HARD REAL-TIME)
 * Period: 50ms, Deadline: 50ms
 * Purpose: Read critical sensors for safety monitoring
 * Consequence of deadline miss: Stale sensor data, safety risk
 */
void sensorScanTask(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xPeriod = pdMS_TO_TICKS(SENSOR_SCAN_PERIOD);
    
    while (1) {
        uint32_t startTime = millis();
        
        // Read sensors (simulate variable processing time)
        SensorData_t data;
        data.timestamp = millis();
        
        // Temperature reading with noise
        int tempRaw = analogRead(TEMP_SENSOR_PIN);
        data.temperature = (tempRaw / 4095.0) * 100.0;  // 0-100°C range
        
        // Speed reading (varies with ride state)
        int speedRaw = analogRead(SPEED_SENSOR_PIN);
        data.speed = (speedRaw / 4095.0) * 150.0;  // 0-150 units
        
        // Simulate variable processing time (1-5ms)
        int processingTime = random(1, 6);
        delay(processingTime);
        
        // Send to safety monitor
        if (xQueueSend(sensorDataQueue, &data, pdMS_TO_TICKS(5)) != pdTRUE) {
            Serial.println("[SENSOR] WARNING: Queue full!");
        }
        
        // Timing tracking
        uint32_t endTime = millis();
        uint32_t execTime = endTime - startTime;
        if (execTime > timingData[1].worstCase) {
            timingData[1].worstCase = execTime;
        }
        timingData[1].taskEnd = endTime;
        
        vTaskDelayUntil(&xLastWakeTime, xPeriod);
    }
}

/*
 * TASK 3: MOTION CONTROL (SOFT REAL-TIME)
 * Period: 100ms, Deadline: 100ms
 * Purpose: Control ride motion and respond to start/stop
 * Consequence of deadline miss: Jerky ride experience
 */
void motionControlTask(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xPeriod = pdMS_TO_TICKS(MOTION_CONTROL_PERIOD);
    bool lastStartBtn = HIGH;
    
    while (1) {
        uint32_t startTime = millis();
        
        // Check start button (with debouncing)
        bool startBtn = digitalRead(RIDE_START_BTN);
        if (startBtn == LOW && lastStartBtn == HIGH) {
            // Button pressed - attempt to start ride
            if (xSemaphoreTake(brakeMutex, portMAX_DELAY) == pdTRUE) {
                if (brakeEngaged && !rideActive) {
                    // Check safety conditions
                    SensorData_t sensorData;
                    bool safeToStart = true;
                    
                    if (xQueuePeek(sensorDataQueue, &sensorData, 0) == pdTRUE) {
                        if (sensorData.temperature > MAX_SAFE_TEMP * 0.9) {
                            safeToStart = false;
                            Serial.println("[MOTION] Start denied - temp too high");
                        }
                    }
                    
                    if (safeToStart) {
                        brakeEngaged = false;
                        rideActive = true;
                        digitalWrite(BRAKE_LED, LOW);
                        digitalWrite(MOTION_LED, HIGH);
                        Serial.println("[MOTION] Ride started!");
                        
                        DisplayMsg_t msg;
                        snprintf(msg.message, sizeof(msg.message), "Ride Active - Enjoy!");
                        msg.priority = 2;
                        xQueueSend(displayQueue, &msg, 0);
                    }
                } else if (rideActive) {
                    // Stop ride normally
                    brakeEngaged = true;
                    rideActive = false;
                    digitalWrite(BRAKE_LED, HIGH);
                    digitalWrite(MOTION_LED, LOW);
                    Serial.println("[MOTION] Ride stopped normally");
                }
                xSemaphoreGive(brakeMutex);
            }
        }
        lastStartBtn = startBtn;
        
        // Simulate ride motion control (variable time)
        if (rideActive) {
            int motionCalcTime = random(10, 30);
            delay(motionCalcTime);
        }
        
        // Timing tracking
        uint32_t endTime = millis();
        uint32_t execTime = endTime - startTime;
        if (execTime > timingData[2].worstCase) {
            timingData[2].worstCase = execTime;
        }
        timingData[2].taskEnd = endTime;
        
        vTaskDelayUntil(&xLastWakeTime, xPeriod);
    }
}

/*
 * TASK 4: GUEST DISPLAY (SOFT REAL-TIME)
 * Period: 500ms, Deadline: 500ms
 * Purpose: Update guest information display
 * Consequence of deadline miss: Poor guest experience
 */
void guestDisplayTask(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xPeriod = pdMS_TO_TICKS(GUEST_DISPLAY_PERIOD);
    
    while (1) {
        uint32_t startTime = millis();
        
        // Process display messages
        DisplayMsg_t msg;
        if (xQueueReceive(displayQueue, &msg, 0) == pdTRUE) {
            Serial.printf("[DISPLAY] %s\n", msg.message);
        }
        
        // Regular status update
        SensorData_t sensorData;
        if (xQueuePeek(sensorDataQueue, &sensorData, 0) == pdTRUE) {
            Serial.printf("[DISPLAY] Status - Temp: %.1f°C, Speed: %.1f, Brake: %s, Active: %s\n",
                sensorData.temperature, sensorData.speed,
                brakeEngaged ? "ON" : "OFF",
                rideActive ? "YES" : "NO");
        }
        
        // Heartbeat LED toggle
        static bool heartbeat = false;
        digitalWrite(HEARTBEAT_LED, heartbeat ? HIGH : LOW);
        heartbeat = !heartbeat;
        
        // Print timing statistics
        Serial.println("\n=== TIMING REPORT ===");
        for (int i = 0; i < 4; i++) {
            Serial.printf("%s - Worst: %lums, Deadline: %lums, Margin: %ldms\n",
                timingData[i].taskName,
                timingData[i].worstCase,
                timingData[i].deadline,
                timingData[i].deadline - timingData[i].worstCase);
        }
        Serial.println("====================\n");
        
        // Timing tracking
        uint32_t endTime = millis();
        uint32_t execTime = endTime - startTime;
        if (execTime > timingData[3].worstCase) {
            timingData[3].worstCase = execTime;
        }
        timingData[3].taskEnd = endTime;
        
        vTaskDelayUntil(&xLastWakeTime, xPeriod);
    }
}

void setup() {
    Serial.begin(115200);
    delay(1000);
    
    Serial.println("\n=== MAGICRIDE CONTROL SYSTEM ===");
    Serial.println("SafeCoaster™ Real-Time Prototype");
    Serial.println("Company: MagicRide Technologies");
    Serial.println("Location: Orlando, FL\n");
    
    // Hardware initialization
    pinMode(EMERGENCY_STOP_BTN, INPUT_PULLUP);
    pinMode(RIDE_START_BTN, INPUT_PULLUP);
    pinMode(HEARTBEAT_LED, OUTPUT);
    pinMode(BRAKE_LED, OUTPUT);
    pinMode(MOTION_LED, OUTPUT);
    
    // Initial safe state
    digitalWrite(BRAKE_LED, HIGH);
    digitalWrite(MOTION_LED, LOW);
    digitalWrite(HEARTBEAT_LED, LOW);
    
    // Create synchronization objects
    brakeMutex = xSemaphoreCreateMutex();
    emergencyFlag = xSemaphoreCreateBinary();
    sensorDataQueue = xQueueCreate(SENSOR_QUEUE_SIZE, sizeof(SensorData_t));
    displayQueue = xQueueCreate(QUEUE_SIZE, sizeof(DisplayMsg_t));
    
    // Initialize timing data
    timingData[0] = {0, 0, 0, SAFETY_MONITOR_PERIOD, "Safety Monitor (H)"};
    timingData[1] = {0, 0, 0, SENSOR_SCAN_PERIOD, "Sensor Scanner (H)"};
    timingData[2] = {0, 0, 0, MOTION_CONTROL_PERIOD, "Motion Control (S)"};
    timingData[3] = {0, 0, 0, GUEST_DISPLAY_PERIOD, "Guest Display (S)"};
    
    // Attach emergency stop interrupt
    attachInterrupt(digitalPinToInterrupt(EMERGENCY_STOP_BTN), emergencyStopISR, FALLING);
    
    // Create FreeRTOS tasks with appropriate priorities
    xTaskCreate(safetyMonitorTask, "SafetyMonitor", TASK_STACK_SIZE, NULL, 4, NULL);  // Highest
    xTaskCreate(sensorScanTask, "SensorScan", TASK_STACK_SIZE, NULL, 3, NULL);       // High
    xTaskCreate(motionControlTask, "MotionControl", TASK_STACK_SIZE, NULL, 2, NULL); // Medium
    xTaskCreate(guestDisplayTask, "GuestDisplay", TASK_STACK_SIZE, NULL, 1, NULL);   // Low
    
    Serial.println("System initialized - All tasks running");
    Serial.println("Press GREEN button to start ride");
    Serial.println("Press RED button for EMERGENCY STOP\n");
}

void loop() {
    // Empty - all work done in tasks
    vTaskDelay(portMAX_DELAY);
}