/* Tests for the display pipeline module (bad map, AGC, FFC scheduler, cal). */
#include <stdio.h>
#include <string.h>
#include "mag160c/mag160c.h"
#include "mag160c/mag160c_display.h"

static int failures = 0;

#define CHECK(cond, msg) do { \
    if (!(cond)) { printf("FAIL: %s\n", msg); failures++; } \
    else { printf("ok: %s\n", msg); } \
} while (0)

static void test_badmap(void) {
    /* 4x3 synthetic frame with two bad pixels and a 2-pixel cluster */
    enum { W = 4, H = 3 };
    mag160c_display_badmap_t m;
    mag160c_display_badmap_init(&m, W, H);
    m.temporal_thr = 50;

    /* pixel (1,0): bright defect (value 5000 vs peak 100) - every frame */
    /* pixels (2,1),(3,1): temporal fluctuation cluster */
    uint16_t f2[H * W];
    for (int i = 0; i < 30; ++i) {
        for (int p = 0; p < H * W; ++p) f2[p] = 100;
        f2[0 * W + 1] = 5000;
        f2[1 * W + 2] = 100 + (i % 2 ? 80 : 0);
        f2[1 * W + 3] = 100 + (i % 2 ? 80 : 0);
        mag160c_display_badmap_feed(&m, f2);
    }
    mag160c_error_t e = mag160c_display_badmap_finalize(&m);
    CHECK(e == MAG160C_OK, "badmap finalize ok");
    CHECK(m.ready == 1, "badmap ready");
    CHECK(m.bad[0 * W + 1] == 1, "bright defect detected");
    CHECK(m.bad[1 * W + 2] == 1 && m.bad[1 * W + 3] == 1, "temporal cluster detected");
    CHECK(m.bad_count == 3, "bad_count == 3");
    /* correction must fill bad pixels with neighbour means */
    for (int p = 0; p < H * W; ++p) f2[p] = 100;
    f2[0 * W + 1] = 5000;
    f2[1 * W + 2] = 180;
    f2[1 * W + 3] = 180;
    mag160c_display_badmap_correct(&m, f2);
    CHECK(f2[0 * W + 1] == 100, "bright defect corrected to neighbour mean");
    CHECK(f2[1 * W + 2] == 100, "cluster pixel corrected");
    CHECK(f2[1 * W + 3] == 100, "cluster pixel 2 corrected");
    CHECK(m.ref[1 * W + 2] == 100, "reference cluster filled");
    mag160c_display_badmap_destroy(&m);
}

static void test_agc(void) {
    /* 1000 samples: 980 background 1000..1979, 10 hot outliers 40000+,
     * 10 zeros.  Percentile [2,98] must exclude the outliers. */
    uint16_t frame[1000];
    for (int i = 0; i < 980; ++i) frame[i] = (uint16_t)(1000 + i % 980);
    for (int i = 0; i < 10; ++i) frame[980 + i] = (uint16_t)(40000 + i * 100);
    frame[990] = 0;
    frame[991] = 0;
    uint32_t lo = 0, hi = 0;
    mag160c_error_t e = mag160c_display_agc_range(frame, 1000, 2, 98, &lo, &hi);
    CHECK(e == MAG160C_OK, "agc_range ok");
    CHECK(hi < 40000, "agc excludes hot defect outliers");
    CHECK(lo >= 1000 && lo <= 1100, "agc low percentile sane");

    int32_t diff[8] = {0, 5, 10, -10, 100, -100, 1000, -1000};
    uint32_t span = 0;
    e = mag160c_display_diff_span(diff, 8, 120, 4000, &span);
    CHECK(e == MAG160C_OK, "diff_span ok");
    CHECK(span > 200 && span <= 4000, "diff span adaptive");
}

static void test_ffc(void) {
    mag160c_ffc_scheduler_t s;
    mag160c_ffc_scheduler_init(&s, 400, 9);
    int sent0 = 0, sent1 = 0;
    for (int f = 0; f < 900; ++f) {
        int32_t p = mag160c_ffc_scheduler_tick(&s);
        if (p == 0) sent0++;
        if (p == 1) sent1++;
    }
    CHECK(sent1 >= 2, "ffc scheduler sends FFC(1) (initial + pair)");
    CHECK(sent0 >= 2, "ffc scheduler sends FFC(0) periodically");
}

static void test_ffc_trigger(void) {
    /* manual trigger: FFC(0) now, FFC(1) exactly `gap` frames later */
    mag160c_ffc_scheduler_t s;
    mag160c_ffc_scheduler_init(&s, 400, 9);
    int32_t p = mag160c_ffc_scheduler_trigger(&s);
    CHECK(p == 0, "trigger returns 0 (send FFC(0))");
    int frames_to_1 = 0;
    for (int f = 0; f < 20; ++f) {
        int32_t t = mag160c_ffc_scheduler_tick(&s);
        if (t == 1) {
            frames_to_1 = f + 1;
            break;
        }
    }
    CHECK(frames_to_1 == 9, "FFC(1) comes exactly 9 frames after trigger");
}

static void test_cal(void) {
    double a = 0, b = 0;
    mag160c_error_t e = mag160c_temp_calibrate_linear(11000, 0.0, 14000, 90.0, &a, &b);
    CHECK(e == MAG160C_OK, "linear cal ok");
    CHECK(a > 0.029 && a < 0.031, "slope ~0.03 C/count");
    CHECK(mag160c_temp_apply_linear(12500, a, b) > 44.9 &&
          mag160c_temp_apply_linear(12500, a, b) < 45.1, "apply midpoint 45C");
    e = mag160c_temp_calibrate_linear(11000, 0, 11000, 90, &a, &b);
    CHECK(e != MAG160C_OK, "identical counts rejected");
}

static void test_ref_track(void) {
    /* hot object (1200 over bg) for 4500 frames must not be absorbed */
    enum { N = 8 };
    uint16_t ref[N], live[N];
    for (int i = 0; i < N; ++i) ref[i] = 11720;
    for (int i = 0; i < N; ++i) live[i] = 11720 + 1200;
    for (int f = 0; f < 4500; ++f)
        mag160c_display_ref_track(ref, live, N, 60, 32);
    int absorbed = 0;
    for (int i = 0; i < N; ++i) if (ref[i] != 11720) absorbed++;
    CHECK(absorbed == 0, "ref_track: static hot object not absorbed (no ghost)");

    /* removal: diff ~0 */
    for (int i = 0; i < N; ++i) live[i] = 11720;
    long sum = 0;
    for (int i = 0; i < N; ++i) sum += (int)live[i] - (int)ref[i];
    CHECK(sum > -10 && sum < 10, "ref_track: no ghost after object removal");

    /* slow drift still tracked */
    for (int f = 0; f < 2000; ++f) {
        for (int i = 0; i < N; ++i) live[i] = (uint16_t)(11723 + f % 10);
        mag160c_display_ref_track(ref, live, N, 60, 32);
    }
    long drift = 0;
    for (int i = 0; i < N; ++i) drift += (int)live[i] - (int)ref[i];
    CHECK(drift / N < 60, "ref_track: slow drift tracked");
}

static void test_ref_heal(void) {
    enum { W2 = 160, H2 = 120 };
    enum { N = W2 * H2 };
    static uint16_t ref[N], live[N];
    static uint8_t fg[N];
    for (int i = 0; i < N; ++i) { ref[i] = 11720; live[i] = 11720; }

    /* 40 isolated startup-noise pixels wrongly in the reference (+800) */
    int noise[40];
    for (int n = 0; n < 40; ++n) {
        int x = 10 + (n % 8) * 5, y = 10 + (n / 8) * 5;
        noise[n] = y * W2 + x;
        ref[noise[n]] = 11720 + 800;
    }
    for (int f = 0; f < 50; ++f)
        mag160c_display_ref_track_heal(ref, live, W2, H2, 60, 32, fg, 30, 2);
    int healed = 0;
    for (int n = 0; n < 40; ++n) if (ref[noise[n]] == 11720) healed++;
    CHECK(healed == 40, "heal: isolated startup noise pixels healed");

    /* contiguous 60x40 hot object must stay frozen (not healed/absorbed) */
    for (int i = 0; i < N; ++i) { ref[i] = 11720; live[i] = 11720; fg[i] = 0; }
    for (int y = 40; y < 80; ++y)
        for (int x = 50; x < 110; ++x) live[y * W2 + x] = 11720 + 1200;
    for (int f = 0; f < 200; ++f)
        mag160c_display_ref_track_heal(ref, live, W2, H2, 60, 32, fg, 30, 2);
    int absorbed = 0;
    for (int y = 40; y < 80; ++y)
        for (int x = 50; x < 110; ++x)
            if (ref[y * W2 + x] > 11720 + 100) absorbed++;
    CHECK(absorbed == 0, "heal: contiguous object never absorbed/healed");
}

static void test_nuc(void) {
    /* synthetic mura: background 11720 with a +5000 row-1 band and a
     * -3000 col-17 band; a hand (+1200 over a region) must survive NUC */
    enum { W2 = 160, H2 = 120 };
    enum { N = W2 * H2 };
    static uint16_t live[N], ref[N], out[N];
    for (int i = 0; i < N; ++i) {
        int x = i % W2, y = i / W2;
        uint16_t v = 11720;
        if (y == 1) v += 5000;        /* mura row band */
        if (x == 17) v -= 3000;       /* mura col band */
        ref[i] = v;
        live[i] = v;
        if (x >= 60 && x < 100 && y >= 40 && y < 80) live[i] += 1200; /* hand */
    }
    mag160c_error_t e = mag160c_display_nuc(live, ref, N, out);
    CHECK(e == MAG160C_OK, "nuc ok");
    /* mura removed: row1 and col17 pixels back near background */
    int r1 = out[1 * W2 + 50];
    int c17 = out[50 * W2 + 17];
    CHECK(r1 >= 11690 && r1 <= 11770, "nuc removes row mura band");
    CHECK(c17 >= 11690 && c17 <= 11770, "nuc removes col mura band");
    /* hand region still visible */
    int hand = out[50 * W2 + 80];
    CHECK(hand >= 11700 + 1100 && hand <= 11700 + 1300, "nuc keeps object");
}

static void test_ref_rebase(void) {
    enum { N = 8 };
    uint16_t ref[N], live[N];
    for (int i = 0; i < N; ++i) { ref[i] = 11720; live[i] = 11720 + 800; }
    mag160c_error_t e = mag160c_display_ref_rebase(ref, live, N, 2000);
    CHECK(e == MAG160C_OK, "ref_rebase ok");
    long sum = 0;
    for (int i = 0; i < N; ++i) sum += (int)live[i] - (int)ref[i];
    CHECK(sum == 0, "ref_rebase removes global offset");
    /* single hot pixel (outlier) does not drag the median */
    live[0] = 11720 + 6000;
    mag160c_display_ref_rebase(ref, live, N, 2000);
    sum = 0;
    for (int i = 0; i < N; ++i) sum += (int)live[i] - (int)ref[i];
    CHECK(sum >= 0 && sum < 6000, "ref_rebase robust to single outlier");
}

int main(void) {
    test_badmap();
    test_agc();
    test_ffc();
    test_ffc_trigger();
    test_ref_track();
    test_ref_heal();
    test_nuc();
    test_ref_rebase();
    test_cal();
    printf(failures ? "\nTEST_DISPLAY_FAILED (%d)\n" : "\nTEST_DISPLAY_PASSED\n",
           failures);
    return failures ? 1 : 0;
}