/* simulator/test_latency.c
 *
 * Long-running BLE latency benchmark for commeownder.
 *
 * Opens 1–3 physical device serial ports, spawns the BLE simulator as a
 * background 4th peer, then measures advertisement propagation latency for
 * every directed pair of physical devices.  Reports per-pair, per-device-as-
 * source, per-device-as-receiver, and overall statistics.
 *
 * Life values used for measurement are globally unique per iteration so that
 * a PEER_RX on the destination can only have come from the source's fresh SET.
 *
 * Usage:
 *   ./test_latency --port1 /dev/ttyUSB0 --port2 /dev/ttyUSB1 --port3 /dev/ttyUSB2
 *                  [--sim PATH] [--game-id ID] [--iterations N] [--timeout S]
 */

#include <errno.h>
#include <fcntl.h>
#include <limits.h>
#include <math.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/wait.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>

#ifndef SIM_BIN
#  define SIM_BIN "./simulator"
#endif

#define MAX_DEVS          3
#define LBUF              512
#define QSIZE             256
#define DEFAULT_ITERS     30
#define DEFAULT_TIMEOUT_S 5
#define DEFAULT_GAME_ID   "4242"
#define SETTLE_S          3    /* settle time after setup before measuring */
#define LIFE_BASE         50   /* first life value used for measurement */

/* ── Line queue ─────────────────────────────────────────────────────────── */
typedef struct {
    char            data[QSIZE][LBUF];
    int             head, tail, count;
    pthread_mutex_t mtx;
    pthread_cond_t  cond;
} lq_t;

static void lq_init(lq_t *q)
{
    memset(q, 0, sizeof *q);
    pthread_mutex_init(&q->mtx, NULL);
    pthread_condattr_t attr;
    pthread_condattr_init(&attr);
    pthread_condattr_setclock(&attr, CLOCK_MONOTONIC);
    pthread_cond_init(&q->cond, &attr);
    pthread_condattr_destroy(&attr);
}

static void lq_push(lq_t *q, const char *line)
{
    pthread_mutex_lock(&q->mtx);
    if (q->count < QSIZE) {
        strncpy(q->data[q->tail], line, LBUF - 1);
        q->data[q->tail][LBUF - 1] = '\0';
        q->tail = (q->tail + 1) % QSIZE;
        q->count++;
        pthread_cond_signal(&q->cond);
    }
    pthread_mutex_unlock(&q->mtx);
}

static void lq_drain(lq_t *q)
{
    pthread_mutex_lock(&q->mtx);
    q->head = q->tail = q->count = 0;
    pthread_mutex_unlock(&q->mtx);
}

static int lq_wait(lq_t *q, const char **pats, int secs, char *out)
{
    struct timespec dl;
    clock_gettime(CLOCK_MONOTONIC, &dl);
    dl.tv_sec += secs;

    pthread_mutex_lock(&q->mtx);
    for (;;) {
        while (q->count > 0) {
            char *line = q->data[q->head];
            q->head    = (q->head + 1) % QSIZE;
            q->count--;
            int ok = 1;
            for (int i = 0; pats[i]; i++)
                if (!strstr(line, pats[i])) { ok = 0; break; }
            if (ok) {
                if (out) strncpy(out, line, LBUF - 1);
                pthread_mutex_unlock(&q->mtx);
                return 1;
            }
        }
        if (pthread_cond_timedwait(&q->cond, &q->mtx, &dl) == ETIMEDOUT) {
            pthread_mutex_unlock(&q->mtx);
            return 0;
        }
    }
}

/* ── Device ──────────────────────────────────────────────────────────────── */
typedef struct {
    const char *port;
    int         fd;
    lq_t        q;
    pthread_t   thr;
} device_t;

static device_t      g_devs[MAX_DEVS];
static int           g_ndevs = 0;
static volatile int  g_stop  = 0;

static void *serial_reader(void *arg)
{
    device_t *dev = (device_t *)arg;
    char      line[LBUF];
    int       pos = 0;

    while (!g_stop) {
        char    c;
        ssize_t n = read(dev->fd, &c, 1);
        if (n == 0) continue;
        if (n < 0) {
            if (errno == EAGAIN || errno == EWOULDBLOCK) { usleep(1000); continue; }
            break;
        }
        if (c == '\n' || c == '\r') {
            if (pos > 0) {
                line[pos] = '\0';
                if (strncmp(line, "DBG ", 4) == 0)
                    lq_push(&dev->q, line);
                pos = 0;
            }
        } else if (pos < LBUF - 1) {
            line[pos++] = c;
        }
    }
    return NULL;
}

static int open_serial(const char *port)
{
    int fd = open(port, O_RDWR | O_NOCTTY);
    if (fd < 0) { perror("open"); return -1; }
    struct termios tty;
    memset(&tty, 0, sizeof tty);
    tty.c_cflag     = CS8 | CREAD | CLOCAL;
    cfsetispeed(&tty, B115200);
    cfsetospeed(&tty, B115200);
    tty.c_oflag     = 0;
    tty.c_lflag     = 0;
    tty.c_cc[VMIN]  = 0;
    tty.c_cc[VTIME] = 1;
    if (tcsetattr(fd, TCSANOW, &tty) < 0) { perror("tcsetattr"); close(fd); return -1; }
    return fd;
}

static void dev_send(device_t *dev, const char *cmd)
{
    if (write(dev->fd, cmd, strlen(cmd)) < 0) perror("write");
}

/* Short label from port path, e.g. "/dev/ttyUSB1" → "USB1". */
static const char *port_label(const char *port)
{
    const char *p = strstr(port, "tty");
    return p ? p + 3 : port;
}

/* ── Simulator ───────────────────────────────────────────────────────────── */
static pid_t        g_sim_pid = -1;
static const char  *g_sim_bin = SIM_BIN;
static const char  *g_game_id = DEFAULT_GAME_ID;

static pid_t sim_start(void)
{
    pid_t pid = fork();
    if (pid < 0) { perror("fork"); return -1; }
    if (pid == 0) {
        for (int i = 0; i < g_ndevs; i++) close(g_devs[i].fd);
        const char *args[] = {
            g_sim_bin,
            "--name",    "LATSIM",
            "--life",    "40",
            "--poison",  "0",
            "--game-id", g_game_id,
            NULL
        };
        execv(g_sim_bin, (char *const *)args);
        _exit(1);
    }
    return pid;
}

static void sig_cleanup(int sig)
{
    (void)sig;
    if (g_sim_pid > 0) { kill(g_sim_pid, SIGTERM); waitpid(g_sim_pid, NULL, WNOHANG); }
    _exit(1);
}

/* ── Statistics ──────────────────────────────────────────────────────────── */
typedef struct {
    long   min, max;
    double sum, sum_sq;
    int    count, timeouts;
} stats_t;

static void stats_init(stats_t *s)
{
    memset(s, 0, sizeof *s);
    s->min = LONG_MAX;
    s->max = 0;
}

static void stats_record(stats_t *s, long ms)
{
    s->count++;
    s->sum    += ms;
    s->sum_sq += (double)ms * ms;
    if (ms < s->min) s->min = ms;
    if (ms > s->max) s->max = ms;
}

static void stats_merge(stats_t *dst, const stats_t *src)
{
    if (src->count == 0 && src->timeouts == 0) return;
    dst->count    += src->count;
    dst->sum      += src->sum;
    dst->sum_sq   += src->sum_sq;
    dst->timeouts += src->timeouts;
    if (src->count > 0) {
        if (src->min < dst->min) dst->min = src->min;
        if (src->max > dst->max) dst->max = src->max;
    }
}

static void stats_print(const stats_t *s, const char *label)
{
    printf("  %-28s", label);
    if (s->count == 0) {
        printf("  no samples");
    } else {
        double avg = s->sum / s->count;
        double var = s->sum_sq / s->count - avg * avg;
        double sd  = var > 0.0 ? sqrt(var) : 0.0;
        printf("  avg=%4.0fms  min=%4ldms  max=%4ldms  sd=%3.0fms  n=%d",
               avg, s->min, s->max, sd, s->count);
    }
    if (s->timeouts) printf("  timeouts=%d", s->timeouts);
    printf("\n");
}

/* ── Device setup ────────────────────────────────────────────────────────── */
static int setup_device(device_t *dev)
{
    const char *ps[] = {"DBG STATE", NULL};
    int ready = 0;
    for (int i = 0; i < 10 && !ready; i++) {
        dev_send(dev, "STATE\n");
        if (lq_wait(&dev->q, ps, 3, NULL)) ready = 1;
    }
    if (!ready) return 0;
    lq_drain(&dev->q);

    dev_send(dev, "CLEARNVS\n");
    if (!lq_wait(&dev->q, ps, 10, NULL)) return 0;
    dev_send(dev, "SET ble=1\n");
    if (!lq_wait(&dev->q, ps, 5, NULL)) return 0;
    lq_drain(&dev->q);
    return 1;
}

/* ── Measurement ─────────────────────────────────────────────────────────── */
static int g_iterations = DEFAULT_ITERS;
static int g_timeout_s  = DEFAULT_TIMEOUT_S;
static int g_life_seq   = 0; /* global counter for unique life values */

/*
 * SET life=<val> on src, wait for dst to log PEER_RX carrying that value.
 * Returns elapsed milliseconds, or -1 on timeout.
 * The life value is chosen globally unique to avoid false matches from other
 * devices that may be advertising stale values.
 */
static long measure_once(device_t *src, device_t *dst)
{
    int  val = LIFE_BASE + g_life_seq++;
    char pat[32], cmd[32];
    snprintf(pat, sizeof pat, "life=%d", val);
    snprintf(cmd, sizeof cmd, "SET life=%d\n", val);

    lq_drain(&dst->q);

    struct timespec t0, t1;
    clock_gettime(CLOCK_MONOTONIC, &t0);
    dev_send(src, cmd);

    const char *pats[] = {"DBG PEER_RX", pat, NULL};
    int found = lq_wait(&dst->q, pats, g_timeout_s, NULL);
    clock_gettime(CLOCK_MONOTONIC, &t1);

    if (!found) return -1;
    return (t1.tv_sec - t0.tv_sec) * 1000L + (t1.tv_nsec - t0.tv_nsec) / 1000000L;
}

/* ── Entry point ─────────────────────────────────────────────────────────── */
int main(int argc, char *argv[])
{
    const char *ports[MAX_DEVS] = {NULL};

    for (int i = 1; i < argc; i++) {
        if      (!strcmp(argv[i], "--port1") && i + 1 < argc) ports[0]       = argv[++i];
        else if (!strcmp(argv[i], "--port2") && i + 1 < argc) ports[1]       = argv[++i];
        else if (!strcmp(argv[i], "--port3") && i + 1 < argc) ports[2]       = argv[++i];
        else if (!strcmp(argv[i], "--sim")   && i + 1 < argc) g_sim_bin      = argv[++i];
        else if (!strcmp(argv[i], "--game-id") && i + 1 < argc) g_game_id    = argv[++i];
        else if (!strcmp(argv[i], "--iterations") && i + 1 < argc) g_iterations = atoi(argv[++i]);
        else if (!strcmp(argv[i], "--timeout")    && i + 1 < argc) g_timeout_s  = atoi(argv[++i]);
        else {
            fprintf(stderr,
                "usage: %s --port1 DEV [--port2 DEV] [--port3 DEV]\n"
                "           [--sim PATH] [--game-id ID]\n"
                "           [--iterations N] [--timeout S]\n", argv[0]);
            return 1;
        }
    }

    if (!ports[0]) {
        fprintf(stderr, "error: --port1 is required\n");
        return 1;
    }

    signal(SIGINT,  sig_cleanup);
    signal(SIGTERM, sig_cleanup);

    /* ── Open devices ──────────────────────────────────────────────────── */
    for (int i = 0; i < MAX_DEVS; i++) {
        if (!ports[i]) break;
        g_devs[g_ndevs].port = ports[i];
        lq_init(&g_devs[g_ndevs].q);
        g_devs[g_ndevs].fd = open_serial(ports[i]);
        if (g_devs[g_ndevs].fd < 0) {
            fprintf(stderr, "cannot open %s\n", ports[i]);
            return 1;
        }
        pthread_create(&g_devs[g_ndevs].thr, NULL, serial_reader, &g_devs[g_ndevs]);
        g_ndevs++;
    }

    if (g_ndevs < 2) {
        fprintf(stderr, "error: need at least 2 devices (--port1 and --port2)\n");
        return 1;
    }

    /* ── Print header ──────────────────────────────────────────────────── */
    printf("\ncommeownder BLE latency benchmark\n");
    printf("devices:");
    for (int i = 0; i < g_ndevs; i++) printf("  %s", g_devs[i].port);
    printf("\n");
    printf("simulator: %s  game-id: %s\n", g_sim_bin, g_game_id);
    printf("iterations: %d per pair   timeout: %ds\n\n", g_iterations, g_timeout_s);

    /* ── Set up devices ────────────────────────────────────────────────── */
    printf("Setting up devices:\n");
    for (int i = 0; i < g_ndevs; i++) {
        printf("  %s ... ", g_devs[i].port); fflush(stdout);
        if (!setup_device(&g_devs[i])) {
            printf("FAIL\n");
            return 1;
        }
        printf("OK\n");
    }

    /* ── Start simulator ───────────────────────────────────────────────── */
    printf("Starting simulator (LATSIM, life=40) ... ");
    fflush(stdout);
    g_sim_pid = sim_start();
    if (g_sim_pid < 0) { printf("FAIL\n"); return 1; }
    printf("OK  (pid %d)\n", g_sim_pid);

    /* ── Settle ────────────────────────────────────────────────────────── */
    printf("Settling (%ds) ...\n\n", SETTLE_S);
    sleep(SETTLE_S);

    /* Drain all queues after settle so old PEER_RX lines don't skew results. */
    for (int i = 0; i < g_ndevs; i++) lq_drain(&g_devs[i].q);

    /* ── Run measurements ──────────────────────────────────────────────── */
    int    npairs = g_ndevs * (g_ndevs - 1);
    stats_t pair_stats[MAX_DEVS][MAX_DEVS];
    stats_t src_stats[MAX_DEVS];
    stats_t dst_stats[MAX_DEVS];
    stats_t overall;

    for (int i = 0; i < MAX_DEVS; i++) {
        for (int j = 0; j < MAX_DEVS; j++) stats_init(&pair_stats[i][j]);
        stats_init(&src_stats[i]);
        stats_init(&dst_stats[i]);
    }
    stats_init(&overall);

    printf("Running measurements (%d pairs × %d iterations):\n", npairs, g_iterations);

    for (int si = 0; si < g_ndevs; si++) {
        for (int di = 0; di < g_ndevs; di++) {
            if (si == di) continue;

            printf("  %s → %s  [", port_label(g_devs[si].port), port_label(g_devs[di].port));
            fflush(stdout);

            for (int iter = 0; iter < g_iterations; iter++) {
                long ms = measure_once(&g_devs[si], &g_devs[di]);
                if (ms < 0) {
                    pair_stats[si][di].timeouts++;
                    printf("T"); fflush(stdout);
                } else {
                    stats_record(&pair_stats[si][di], ms);
                    printf("."); fflush(stdout);
                }
            }

            printf("]\n");

            stats_merge(&src_stats[si], &pair_stats[si][di]);
            stats_merge(&dst_stats[di], &pair_stats[si][di]);
            stats_merge(&overall,       &pair_stats[si][di]);
        }
    }

    /* ── Stop simulator ────────────────────────────────────────────────── */
    if (g_sim_pid > 0) {
        kill(g_sim_pid, SIGTERM);
        waitpid(g_sim_pid, NULL, 0);
        g_sim_pid = -1;
    }

    /* ── Report ────────────────────────────────────────────────────────── */
    printf("\n────────────────────────────────────────────────────────────────\n");
    printf("Pair statistics:\n");
    for (int si = 0; si < g_ndevs; si++) {
        for (int di = 0; di < g_ndevs; di++) {
            if (si == di) continue;
            char label[64];
            snprintf(label, sizeof label, "%s → %s",
                     port_label(g_devs[si].port), port_label(g_devs[di].port));
            stats_print(&pair_stats[si][di], label);
        }
    }

    printf("\nPer-device as source:\n");
    for (int i = 0; i < g_ndevs; i++) {
        char label[64];
        snprintf(label, sizeof label, "%s", port_label(g_devs[i].port));
        stats_print(&src_stats[i], label);
    }

    printf("\nPer-device as receiver:\n");
    for (int i = 0; i < g_ndevs; i++) {
        char label[64];
        snprintf(label, sizeof label, "%s", port_label(g_devs[i].port));
        stats_print(&dst_stats[i], label);
    }

    printf("\nOverall (%d samples):\n", overall.count);
    stats_print(&overall, "all pairs");
    printf("────────────────────────────────────────────────────────────────\n\n");

    g_stop = 1;
    for (int i = 0; i < g_ndevs; i++) {
        pthread_join(g_devs[i].thr, NULL);
        close(g_devs[i].fd);
    }
    return (overall.count == 0) ? 1 : 0;
}