/**
 * SN-L00 Latency Measurement
 *
 * High-resolution latency measurement using RP2040 hardware timer
 */

#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/timer.h"

#include "config.h"
#include "latency.h"

// Statistics storage
static float samples[STATS_SAMPLES];
static uint32_t sample_index = 0;
static uint32_t sample_count = 0;
static float min_latency = 0;
static float max_latency = 0;

void latency_init(void) {
    // Configure trigger output pin
    gpio_init(PIN_TRIG_OUT);
    gpio_set_dir(PIN_TRIG_OUT, GPIO_OUT);
    gpio_put(PIN_TRIG_OUT, 0);

    // Configure return input pin
    gpio_init(PIN_RETURN_IN);
    gpio_set_dir(PIN_RETURN_IN, GPIO_IN);
    gpio_pull_down(PIN_RETURN_IN);  // Default low

    latency_reset_stats();
}

measurement_t latency_measure(void) {
    measurement_t result = {
        .valid = false,
        .latency_ms = 0,
        .latency_us = 0
    };

    // Ensure output is low before starting
    gpio_put(PIN_TRIG_OUT, 0);
    sleep_us(100);

    // Record start time and send trigger
    uint64_t start_us = time_us_64();
    gpio_put(PIN_TRIG_OUT, 1);

    // Wait for trigger pulse duration
    sleep_ms(TRIGGER_PULSE_MS);
    gpio_put(PIN_TRIG_OUT, 0);

    // Wait for return signal with timeout
    uint64_t timeout_us = start_us + (MEASURE_TIMEOUT_MS * 1000);

    while (time_us_64() < timeout_us) {
        if (gpio_get(PIN_RETURN_IN)) {
            // Got return signal!
            uint64_t end_us = time_us_64();
            result.latency_us = end_us - start_us;
            result.latency_ms = (float)result.latency_us / 1000.0f;
            result.valid = true;
            break;
        }
        // Tight polling loop - don't sleep to maintain resolution
    }

    return result;
}

void latency_reset_stats(void) {
    for (int i = 0; i < STATS_SAMPLES; i++) {
        samples[i] = 0;
    }
    sample_index = 0;
    sample_count = 0;
    min_latency = 0;
    max_latency = 0;
}

void latency_add_to_stats(float latency_ms) {
    // Add to circular buffer
    samples[sample_index] = latency_ms;
    sample_index = (sample_index + 1) % STATS_SAMPLES;

    if (sample_count < STATS_SAMPLES) {
        sample_count++;
    }

    // Update min/max
    if (sample_count == 1) {
        min_latency = latency_ms;
        max_latency = latency_ms;
    } else {
        if (latency_ms < min_latency) min_latency = latency_ms;
        if (latency_ms > max_latency) max_latency = latency_ms;
    }
}

stats_t latency_get_stats(void) {
    stats_t s = {
        .min_ms = min_latency,
        .max_ms = max_latency,
        .avg_ms = 0,
        .count = sample_count
    };

    if (sample_count > 0) {
        float sum = 0;
        for (uint32_t i = 0; i < sample_count; i++) {
            sum += samples[i];
        }
        s.avg_ms = sum / sample_count;
    }

    return s;
}