/**
 * SN-L00 Latency Tester
 * SubModular / Sub-Net e.U.
 *
 * Main application entry point and state machine
 */

#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"

#include "config.h"
#include "display.h"
#include "latency.h"
#include "button.h"

typedef enum {
    MODE_SINGLE,        // Single shot measurement
    MODE_CONTINUOUS,    // Continuous measurement
    MODE_STATS          // Show statistics
} app_mode_t;

static app_mode_t current_mode = MODE_SINGLE;
static uint32_t last_trigger_time = 0;
static bool measurement_pending = false;

void show_startup(void) {
    display_clear();
    display_mode("SN-L00");
    sleep_ms(1000);
    display_clear();
    display_mode("SINGLE");
}

void handle_single_mode(button_event_t event) {
    if (event == BTN_SHORT_PRESS) {
        display_waiting();

        measurement_t result = latency_measure();

        if (result.valid) {
            display_latency(result.latency_ms);
            latency_add_to_stats(result.latency_ms);
            printf("Latency: %.3f ms\n", result.latency_ms);
        } else {
            display_timeout();
            printf("Timeout - no return signal\n");
        }
    } else if (event == BTN_LONG_PRESS) {
        // Switch to continuous mode
        current_mode = MODE_CONTINUOUS;
        display_mode("CONT");
        latency_reset_stats();
        sleep_ms(500);
    }
}

void handle_continuous_mode(button_event_t event) {
    uint32_t now = to_ms_since_boot(get_absolute_time());

    // Auto-trigger at interval
    if (now - last_trigger_time >= CONTINUOUS_INTERVAL_MS) {
        last_trigger_time = now;

        measurement_t result = latency_measure();

        if (result.valid) {
            display_latency(result.latency_ms);
            latency_add_to_stats(result.latency_ms);
        } else {
            display_timeout();
        }
    }

    // Short press exits to stats mode
    if (event == BTN_SHORT_PRESS) {
        current_mode = MODE_STATS;
        stats_t s = latency_get_stats();
        display_stats(s.min_ms, s.max_ms, s.avg_ms);
    }

    // Long press goes back to single mode
    if (event == BTN_LONG_PRESS) {
        current_mode = MODE_SINGLE;
        display_mode("SINGLE");
        sleep_ms(500);
    }
}

void handle_stats_mode(button_event_t event) {
    // Any press returns to single mode
    if (event == BTN_SHORT_PRESS || event == BTN_LONG_PRESS) {
        current_mode = MODE_SINGLE;
        display_mode("SINGLE");
        latency_reset_stats();
        sleep_ms(500);
    }
}

int main(void) {
    // Initialize stdio for USB serial debugging
    stdio_init_all();

    // Wait for USB connection (optional, for debugging)
    sleep_ms(2000);

    printf("\n=== SN-L00 Latency Tester ===\n");
    printf("SubModular / Sub-Net e.U.\n\n");

    // Initialize subsystems
    display_init();
    latency_init();
    button_init();

    show_startup();
    display_mode("READY");

    printf("Ready. Press button to measure.\n");
    printf("Short press: Measure\n");
    printf("Long press: Toggle continuous mode\n\n");

    // Main loop
    while (true) {
        button_event_t event = button_poll();

        switch (current_mode) {
            case MODE_SINGLE:
                handle_single_mode(event);
                break;
            case MODE_CONTINUOUS:
                handle_continuous_mode(event);
                break;
            case MODE_STATS:
                handle_stats_mode(event);
                break;
        }

        display_update();
        sleep_ms(10);
    }

    return 0;
}