#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "esp_timer.h"
#include "esp_random.h"
#include "rom/ets_sys.h" // For busy-waiting

// --- CONFIGURATION ---
// GPIO 0 is the BOOT button (Active Low)
#define ESTOP_BUTTON_PIN GPIO_NUM_0 

// Window sizes
#define WIN_10    10
#define WIN_100   100
#define WIN_1000  1000
#define WIN_10000 10000

// Global buffers
static int data_history[WIN_10000];
static int data_index = 0; 

// --- 1. SENSOR SIMULATION (Blocking) ---
int collect_sensor_data() {
    // Busy wait for 50ms to simulate hardware read time
    ets_delay_us(50000); 
    return (int)(esp_random() % 100);
}

// --- 2. HEAVY MATH (CPU Hog) ---
void compute_statistics(float *mean_out, float *std_out, int window_size) {
    double sum = 0.0;
    
    // Pass 1: Mean
    for(int i = 0; i < window_size; i++) {
        int idx = (data_index - i + WIN_10000) % WIN_10000;
        sum += data_history[idx];
    }
    double mean = sum / window_size;

    // Pass 2: Std Dev
    double sum_sq_diff = 0.0;
    for(int i = 0; i < window_size; i++) {
        int idx = (data_index - i + WIN_10000) % WIN_10000;
        sum_sq_diff += pow((data_history[idx] - mean), 2);
    }
    
    *mean_out = (float)mean;
    *std_out = (float)sqrt(sum_sq_diff / window_size);
}

// --- 3. NETWORK SIMULATION (Jittery) ---
void send_telemetry(int payload_size) {
    int delay_ms = payload_size / 20; 
    
    // 10% chance of "network timeout"
    if ((esp_random() % 10) == 0) {
        printf(">> [NETWORK] Retry... blocking...\n");
        delay_ms += 500; 
    }

    // Blocking delay
    vTaskDelay(pdMS_TO_TICKS(delay_ms));
}

// --- MAIN ---
void app_main(void)
{
    // CRITICAL: Disable stdout buffering so prints appear immediately
    setvbuf(stdout, NULL, _IONBF, 0);

    // Setup GPIO
    gpio_reset_pin(ESTOP_BUTTON_PIN);
    gpio_set_direction(ESTOP_BUTTON_PIN, GPIO_MODE_INPUT);
    gpio_set_pull_mode(ESTOP_BUTTON_PIN, GPIO_PULLUP_ONLY); 

    memset(data_history, 0, sizeof(data_history));

    printf("\n\n");
    printf("======================================\n");
    printf("      TYPICAL SUPER LOOP DEMO         \n");
    printf("======================================\n");
    printf("Press BOOT button to test latency.\n\n");

    while (1) {
        int64_t start_time = esp_timer_get_time();

        // 1. Collect
        int raw_data = collect_sensor_data();
        data_index = (data_index + 1) % WIN_10000;
        data_history[data_index] = raw_data;

        // 2. Compute
        float m10, s10, m100, s100, m1k, s1k, m10k, s10k;
        compute_statistics(&m10, &s10, WIN_10);
        compute_statistics(&m100, &s100, WIN_100);
        compute_statistics(&m1k, &s1k, WIN_1000);
        compute_statistics(&m10k, &s10k, WIN_10000);

        // 3. Send
        int payload_size = (esp_random() % 2 == 0) ? 100 : 5000;
        send_telemetry(payload_size);

        // 4. Input Check (The Trap)
        if (gpio_get_level(ESTOP_BUTTON_PIN) == 0) {
            int64_t current_time = esp_timer_get_time();
            int64_t latency = (current_time - start_time) / 1000;

            printf("\n\n****************************************\n");
            printf("!!! E-STOP BUTTON DETECTED !!!\n");
            printf("Input Latency: %lld ms\n", latency);
            printf("****************************************\n\n");
        }

        int64_t loop_duration = (esp_timer_get_time() - start_time) / 1000;

        printf("Loop: %4lld ms | Val: %3d | 10k Mean: %5.2f\n", 
               loop_duration, raw_data, m10k);
        
        // Safety yield
        vTaskDelay(1); 
    }
}