#include <Arduino.h>

/* =================================================================================
 * 1. DETERMINISTIC FINITE STATE MACHINE (FSM) DEFINITIONS
 * =================================================================================
 * An enumerated type (enum) is used to strictly enforce system states. This guarantees
 * the architecture is completely deterministic; the system can only exist in one 
 * defined state at a time, eliminating race conditions and erratic overlapping behaviors.
 */
typedef enum {
    STATE_SYSTEM_IDLE,        // HMI: Blue LED ON. System waiting for operator.
    STATE_WAIT_RELEASE,       // Input validation: Debounce and 3-second fault timeout check.
    STATE_FOREPERIOD,         // Anticipation phase: Randomized warning delay.
    STATE_MEASURE_RESPONSE,   // Data acquisition: Active tracking of operator RT.
    STATE_DATA_LOG,           // Post-processing: Array storage and visual HMI scoreboard.
    STATE_FAULT_SEQUENCE,     // Error handling: Dedicated Fault LED ON (3-second hold).
    STATE_SAFE_MODE_LOCK      // Safety protocol: Hard lockout until operator clears fault.
} SystemState;

SystemState currentState = STATE_SYSTEM_IDLE;

// ENTRY ACTION FLAG: Ensures hardware configuration (like toggling an LED) executes 
// exactly once upon entering a new state. This prevents the microcontroller from 
// spamming hardware registers during millions of high-speed loop iterations.
bool state_just_changed = true; 

/* =================================================================================
 * 2. GLOBAL SYSTEM TIMERS & VARIABLES
 * =================================================================================
 */
// Non-blocking delta timers using the millis() hardware clock.
uint32_t timestamp = 0;         
uint32_t release_timestamp = 0; 
uint32_t foreperiod_delay = 0;

// System tracking and Hardware Timer (PWM) mapping
uint32_t trial_number = 0;
uint8_t stimulus_brightness = 255; // PWM Duty Cycle (0-255) mapped from 10-bit ADC

/* ---------------------------------------------------------------------------------
 * CIRCULAR BUFFER (MEMORY MANAGEMENT)
 * ---------------------------------------------------------------------------------
 * A circular buffer provides continuous data logging with a fixed, predictable O(1) 
 * memory footprint. The modulo operator (%) forces the index to wrap back to 0,
 * safely overwriting the oldest data point without risking dynamic memory leaks.
 */
uint32_t last_reaction_time = 0;
uint32_t result_history[3] = {0, 0, 0}; 
uint8_t history_index = 0;              
uint8_t valid_trials_count = 0;         

/* =================================================================================
 * 3. HELPER FUNCTIONS
 * =================================================================================
 */
// Encapsulates state transitions to enforce the resetting of the Entry Action flag.
void changeState(SystemState newState) {
    currentState = newState;
    state_just_changed = true;
}

/* =================================================================================
 * 4. HARDWARE INITIALIZATION
 * =================================================================================
 */
void setup() {
    Serial.begin(115200);
    
    // BARE-METAL CLOCK ENABLE: Directly accessing the CMSIS register to power Port A
    RCC->IOPENR |= RCC_IOPENR_GPIOAEN;
    
    // HIGH-LEVEL ABSTRACTION: Initialize D3 (PA2) for Hardware Timer PWM via framework
    pinMode(D3, OUTPUT);

    // -----------------------------------------------------------------------------
    // REGISTER-LEVEL PIN CONFIGURATION (Maximized efficiency bypassing framework overhead)
    // -----------------------------------------------------------------------------
    // Configure PA4 (Red Status), PA5 (Blue Idle), and PA6 (Fault) as Output Mode ('01')
    GPIOA->MODER &= ~(3 << (4 * 2)); // Clear bits 8 & 9
    GPIOA->MODER |= (1 << (4 * 2));  // Set bit 8 to 1 (PA4 Output)
    
    GPIOA->MODER &= ~(3 << (5 * 2)); // Clear bits 10 & 11
    GPIOA->MODER |= (1 << (5 * 2));  // Set bit 10 to 1 (PA5 Output)
    
    GPIOA->MODER &= ~(3 << (6 * 2)); // Clear bits 12 & 13
    GPIOA->MODER |= (1 << (6 * 2));  // Set bit 12 to 1 (PA6 Output)

    // Configure PA3 (User Button) as Input ('00') with an Internal Pull-Up ('01')
    GPIOA->MODER &= ~(3 << (3 * 2)); // Clear bits 6 & 7 for Input Mode
    GPIOA->PUPDR &= ~(3 << (3 * 2)); // Clear Pull-Up/Pull-Down bits
    GPIOA->PUPDR |= (1 << (3 * 2));  // Activate Pull-Up resistor (Defaults to 3.3V HIGH)

    // System Header & Tab-Separated Data Table Configuration
    Serial.println("HUMAN RESPONSE SYSTEM: FULL INSTRUMENT");
    Serial.println("System Booted. Waiting for operator...");
    Serial.println("----------------------------------------------------------------");
    Serial.println("Trial\tDelay(ms)\tPWM Brightness\tRT(ms)\t\tStatus");
    Serial.println("----------------------------------------------------------------");
}

/* =================================================================================
 * 5. MAIN SYSTEM LOOP (NON-BLOCKING ARCHITECTURE)
 * =================================================================================
 */
void loop() {
    // HARDWARE POLLING: Read PA3 via the Input Data Register (IDR). 
    // Bitwise mask isolates bit 3. Since we use a Pull-Up, logic '0' = pressed.
    uint8_t button_pressed = ((GPIOA->IDR & (1 << 3)) == 0);
    
    // Capture system uptime once per cycle for synchronized delta-time math
    uint32_t currentTicks = millis();

    switch (currentState) {
        
        // -------------------------------------------------------------------------
        // IDLE: System is ready. HMI visual indicator is active.
        // -------------------------------------------------------------------------
        case STATE_SYSTEM_IDLE:
            if (state_just_changed) {
                analogWrite(D3, 0);      // Ensure PWM Stimulus (Green) is off
                GPIOA->ODR &= ~(1 << 4); // Ensure Red Status is off
                GPIOA->ODR &= ~(1 << 6); // Ensure Dedicated Fault is off
                GPIOA->ODR |= (1 << 5);  // HMI: Turn ON Blue LED (System Ready)
                state_just_changed = false;
            }

            if (button_pressed) {
                GPIOA->ODR &= ~(1 << 5); // Instantly extinguish Blue LED to prevent distraction
                timestamp = currentTicks;          // Initialize fault timer
                release_timestamp = currentTicks;  // Initialize debounce timer
                changeState(STATE_WAIT_RELEASE);
            }
            break;

        // -------------------------------------------------------------------------
        // WAIT RELEASE: Mechanical debounce and implausible input detection.
        // -------------------------------------------------------------------------
        case STATE_WAIT_RELEASE:
            if (button_pressed) {
                release_timestamp = currentTicks; // Constantly reset debounce timer
                
                // FAULT DETECT: User held the button for > 3 seconds (Hardware stuck or misuse)
                if (currentTicks - timestamp > 3000) {
                    Serial.println("\n>> SYSTEM FAULT: Implausible Input (Button Held).");
                    timestamp = currentTicks;
                    changeState(STATE_FAULT_SEQUENCE);
                }
            }
            // Proceed only if the button is physically released for 50 continuous ms
            else if (currentTicks - release_timestamp >= 50) {
                
                // Read 10-bit ADC (0 - 1023) to define system difficulty parameters
                uint16_t pot_value = analogRead(A0);
                
                // Map ADC to a BASE delay (1000ms up to 5000ms) + 1000ms randomized variance
                uint32_t base_delay = map(pot_value, 0, 1023, 1000, 5000);
                foreperiod_delay = base_delay + (rand() % 1000); 
                
                // Map ADC to PWM Duty Cycle (Inverted: Hard/Long Delay = Dim LED)
                stimulus_brightness = map(pot_value, 0, 1023, 255, 5);
                
                trial_number++;
                timestamp = currentTicks; 
                changeState(STATE_FOREPERIOD);
            }
            break;

        // -------------------------------------------------------------------------
        // FOREPERIOD: The randomized warning delay (Anticipation phase).
        // -------------------------------------------------------------------------
        case STATE_FOREPERIOD:
            if (state_just_changed) {
                state_just_changed = false; 
            }

            if (button_pressed) {
                // FAULT DETECT: Anticipatory Error (Operator pressed before stimulus fired)
                Serial.print(trial_number);
                Serial.print("\t");
                Serial.print(foreperiod_delay);
                Serial.print("\t\t");
                Serial.print(stimulus_brightness);
                Serial.println("\t\tN/A\t\tFAULT (Early)");
                
                timestamp = currentTicks;
                changeState(STATE_FAULT_SEQUENCE);
            }
            // Non-blocking delta check: Proceed to measurement when time elapses
            else if (currentTicks - timestamp >= foreperiod_delay) {
                changeState(STATE_MEASURE_RESPONSE);
            }
            break;

        // -------------------------------------------------------------------------
        // MEASURE RESPONSE: Triggers the PWM LED and calculates reaction delta.
        // -------------------------------------------------------------------------
        case STATE_MEASURE_RESPONSE:
            if (state_just_changed) {
                analogWrite(D3, stimulus_brightness); // Fire stimulus via Hardware PWM
                timestamp = currentTicks; // Start the reaction stopwatch
                state_just_changed = false;
            }

            if (button_pressed) {
                last_reaction_time = currentTicks - timestamp; 
                
                // Array Management: Save result and advance circular buffer index
                result_history[history_index] = last_reaction_time;
                history_index = (history_index + 1) % 3; 
                if (valid_trials_count < 3) valid_trials_count++;
                
                // Serial Data Logging Output
                Serial.print(trial_number);
                Serial.print("\t");
                Serial.print(foreperiod_delay);
                Serial.print("\t\t");
                Serial.print(stimulus_brightness); 
                Serial.print("\t\t");
                Serial.print(last_reaction_time);
                Serial.println("\t\tVALID");

                // Output Circular Buffer History (Chronological Order: Oldest to Newest)
                Serial.print("\t-> Last 3 Valid RTs: [ ");
                for (int i = 0; i < valid_trials_count; i++) {
                    int read_idx = (history_index - valid_trials_count + i + 3) % 3;
                    Serial.print(result_history[read_idx]);
                    Serial.print("ms ");
                }
                Serial.println("]\n");
                
                timestamp = currentTicks; 
                changeState(STATE_DATA_LOG);
            }
            break;

        // -------------------------------------------------------------------------
        // DATA LOG: Post-trial visual HMI feedback (Scoreboard).
        // -------------------------------------------------------------------------
        case STATE_DATA_LOG:
            if (state_just_changed) {
                // Bounds checking prior to map() to prevent data rollover anomalies
                uint32_t bounded_rt = last_reaction_time;
                if (bounded_rt < 200) bounded_rt = 200; 
                if (bounded_rt > 600) bounded_rt = 600; 
                
                // Inversely map reaction time to visual brightness (Fast RT = Bright LED)
                uint8_t visual_score = map(bounded_rt, 200, 600, 255, 5);
                
                analogWrite(D3, visual_score); // Display score on Green LED
                GPIOA->ODR |= (1 << 4);        // Turn on Red LED (Valid trial indicator)
                state_just_changed = false;
            }

            // Non-blocking hold: Display the visual score for 1.5 seconds
            if (currentTicks - timestamp >= 1500) {
                if (!button_pressed) {
                    changeState(STATE_SYSTEM_IDLE); 
                }
            }
            break;

        // -------------------------------------------------------------------------
        // FAULT SEQUENCE: Extinguish all operational LEDs, ignite Dedicated Fault LED.
        // -------------------------------------------------------------------------
        case STATE_FAULT_SEQUENCE:
            if (state_just_changed) {
                analogWrite(D3, 0);      // Green OFF
                GPIOA->ODR &= ~(1 << 4); // Red OFF
                GPIOA->ODR &= ~(1 << 5); // Blue OFF
                
                // HMI: Activate dedicated Fault Indicator on PA6
                GPIOA->ODR |= (1 << 6);  
                
                state_just_changed = false;
            }

            // Non-blocking timer: Hold the warning state for 3000ms
            if (currentTicks - timestamp >= 3000) { 
                Serial.println(">> SAFE MODE ACTIVE. Release button to clear.\n");
                changeState(STATE_SAFE_MODE_LOCK);
            }
            break;

        // -------------------------------------------------------------------------
        // SAFE MODE: Hard lockout requiring operator intervention to clear fault.
        // -------------------------------------------------------------------------
        case STATE_SAFE_MODE_LOCK:
            if (state_just_changed) {
                // Keep the Fault LED (PA6) ON during the lockout to alert operator
                state_just_changed = false;
            }

            if (button_pressed) {
                timestamp = currentTicks; // Reset the lockout timer if operator touches switch
            }
            // Require 1 full second of absolute zero physical interaction to clear fault
            else if (currentTicks - timestamp >= 1000) {
                GPIOA->ODR &= ~(1 << 6); // Extinguish Fault LED upon successful clear
                Serial.println("Safe Mode cleared. Returning to Idle.\n");
                changeState(STATE_SYSTEM_IDLE);
            }
            break;
    }
}