/*
 * MAG160C display pipeline - pure C, no OS/USB dependencies.
 *
 * Implements the pipeline validated on hardware (Windows demo v3) and
 * informed by open-source thermal SDKs (SeekThermal/OpenThermal, FLIR
 * Lepton AGC, MLX90640):
 *
 *   frame -> bad pixel correction -> diff/NUC -> AGC -> LUT -> RGB
 *
 * 1. Bad pixel detection (Seek-style histogram peak deviation + temporal
 *    min/max) with topological-order 4-neighbour fill.
 * 2. AGC: percentile stretch (FLIR LINEAR clip, 2%/98%) or adaptive diff
 *    stretch with deadband.
 * 3. Ironbow pseudo-color LUT (FLIR-style anchors).
 * 4. FFC scheduler replicating the official demo cadence recovered from
 *    analysis/captures/libusb0_trace.txt: FFC(1) after the ~10th frame
 *    (stream -> type=0), then every ffc_period frames send FFC(0) and
 *    ffc_gap frames later FFC(1).  Verified: 1400+ type=0 frames, 15 fps,
 *    zero stalls.
 * 5. Two-point linear temperature calibration (counts -> degC).
 */
#include "mag160c_internal.h"
#include "mag160c/mag160c_display.h"

#include <stdlib.h>
#include <string.h>

/* ------------------------------------------------------------------ */
/* bad pixel map                                                       */

void mag160c_display_badmap_init(mag160c_display_badmap_t *m, uint32_t w,
                                 uint32_t h) {
    if (m == NULL) {
        return;
    }
    memset(m, 0, sizeof(*m));
    m->width = w;
    m->height = h;
    m->pixels = w * h;
    m->temporal_thr = 400;      /* min/max fluctuation (counts) */
    m->hist_min_dev = 200;      /* histogram peak min deviation (counts) */
    m->bad = (uint8_t *)calloc(m->pixels, 1);
    m->order_x = (uint32_t *)malloc(m->pixels * sizeof(uint32_t));
    m->order_y = (uint32_t *)malloc(m->pixels * sizeof(uint32_t));
    m->ref_sum = (uint64_t *)calloc(m->pixels, sizeof(uint64_t));
    m->ref_min = (uint16_t *)malloc(m->pixels * sizeof(uint16_t));
    m->ref_max = (uint16_t *)malloc(m->pixels * sizeof(uint16_t));
    m->ref = (uint16_t *)calloc(m->pixels, sizeof(uint16_t));
}

void mag160c_display_badmap_destroy(mag160c_display_badmap_t *m) {
    if (m == NULL) {
        return;
    }
    free(m->bad);
    free(m->order_x);
    free(m->order_y);
    free(m->ref_sum);
    free(m->ref_min);
    free(m->ref_max);
    free(m->ref);
    memset(m, 0, sizeof(*m));
}

static int has_valid_neighbor(const uint8_t *bad, uint32_t w, uint32_t h,
                              uint32_t x, uint32_t y) {
    return (x > 0 && !bad[y * w + x - 1]) ||
           (x + 1 < w && !bad[y * w + x + 1]) ||
           (y > 0 && !bad[(y - 1) * w + x]) ||
           (y + 1 < h && !bad[(y + 1) * w + x]);
}

/* neighbour mean of frame at (x,y), skipping bad pixels; 0 when none. */
static uint32_t neighbor_mean(const uint16_t *fr, const uint8_t *bad,
                              uint32_t w, uint32_t h, uint32_t x, uint32_t y) {
    uint64_t sum = 0;
    uint32_t n = 0;
    if (x > 0 && !bad[y * w + x - 1]) { sum += fr[y * w + x - 1]; n++; }
    if (x + 1 < w && !bad[y * w + x + 1]) { sum += fr[y * w + x + 1]; n++; }
    if (y > 0 && !bad[(y - 1) * w + x]) { sum += fr[(y - 1) * w + x]; n++; }
    if (y + 1 < h && !bad[(y + 1) * w + x]) { sum += fr[(y + 1) * w + x]; n++; }
    return n ? (uint32_t)(sum / n) : 0;
}

/* topological fill order so clusters are filled edge-first */
static void build_fill_order(mag160c_display_badmap_t *m) {
    const uint32_t w = m->width, h = m->height, n = m->pixels;
    uint8_t *work = (uint8_t *)malloc(n);
    if (work == NULL) {
        return;
    }
    memcpy(work, m->bad, n);
    uint32_t remain = 0;
    for (uint32_t i = 0; i < n; ++i) {
        if (work[i]) remain++;
    }
    uint32_t len = 0;
    while (remain > 0) {
        uint32_t progress = 0;
        for (uint32_t y = 0; y < h; ++y) {
            for (uint32_t x = 0; x < w; ++x) {
                if (!work[y * w + x]) continue;
                if (!has_valid_neighbor(work, w, h, x, y)) continue;
                m->order_x[len] = x;
                m->order_y[len] = y;
                len++;
                work[y * w + x] = 0;
                remain--;
                progress++;
            }
        }
        if (progress == 0) { /* fully isolated: force-fill in order */
            for (uint32_t y = 0; y < h && progress == 0; ++y) {
                for (uint32_t x = 0; x < w && progress == 0; ++x) {
                    if (!work[y * w + x]) continue;
                    m->order_x[len] = x;
                    m->order_y[len] = y;
                    len++;
                    work[y * w + x] = 0;
                    remain--;
                    progress++;
                }
            }
        }
    }
    m->order_len = len;
    free(work);
}

/* threshold = histPeak - (frameMax - histPeak)  (Seek method).
 * Guarded: a pixel is only flagged when it is also at least hist_min_dev
 * counts above the scene peak, so a lone extreme outlier cannot push the
 * threshold below the whole background. */
static uint32_t histogram_peak_threshold(const uint16_t *fr, uint32_t n,
                                         uint32_t min_dev) {
    uint32_t *hist = (uint32_t *)calloc(65536, sizeof(uint32_t));
    if (hist == NULL) {
        return 0xffffu;
    }
    uint32_t peakv = 0, peakc = 0, maxv = 0;
    for (uint32_t i = 0; i < n; ++i) {
        uint32_t v = fr[i];
        if (++hist[v] > peakc) {
            peakc = hist[v];
            peakv = v;
        }
        if (v > maxv) maxv = v;
    }
    free(hist);
    if (maxv <= peakv) {
        return 0xffffu;
    }
    int64_t t = (int64_t)peakv - ((int64_t)maxv - (int64_t)peakv);
    uint64_t guard = (uint64_t)peakv + min_dev;
    return t > (int64_t)guard ? (uint32_t)t : (uint32_t)guard;
}

/* Feed frames for reference statistics (call ~30 times at idle scene). */
mag160c_error_t mag160c_display_badmap_feed(mag160c_display_badmap_t *m,
                                            const uint16_t *frame) {
    if (m == NULL || frame == NULL || m->bad == NULL) {
        mag160c_set_error("mag160c_display_badmap_feed: null argument");
        return MAG160C_ERR_INVALID_ARGUMENT;
    }
    const uint32_t n = m->pixels;
    for (uint32_t i = 0; i < n; ++i) {
        uint16_t v = frame[i];
        m->ref_sum[i] += v;
        if (m->feed_count == 0) {
            m->ref_min[i] = m->ref_max[i] = v;
        } else {
            if (v < m->ref_min[i]) m->ref_min[i] = v;
            if (v > m->ref_max[i]) m->ref_max[i] = v;
        }
    }
    m->feed_count++;
    return MAG160C_OK;
}

/* Finalize: averages, temporal + histogram bad pixel detection, fill. */
mag160c_error_t mag160c_display_badmap_finalize(mag160c_display_badmap_t *m) {
    if (m == NULL || m->bad == NULL) {
        mag160c_set_error("mag160c_display_badmap_finalize: null argument");
        return MAG160C_ERR_INVALID_ARGUMENT;
    }
    if (m->feed_count == 0) {
        mag160c_set_error("mag160c_display_badmap_finalize: no frames fed");
        return MAG160C_ERR_NOT_READY;
    }
    const uint32_t n = m->pixels;
    for (uint32_t i = 0; i < n; ++i) {
        m->ref[i] = (uint16_t)(m->ref_sum[i] / m->feed_count);
        m->bad[i] = 0;
    }
    uint32_t nb = 0;
    /* temporal detection */
    for (uint32_t i = 0; i < n; ++i) {
        if ((uint32_t)m->ref_max[i] - (uint32_t)m->ref_min[i] > m->temporal_thr) {
            m->bad[i] = 1;
            nb++;
        }
    }
    /* histogram peak deviation (bright defects) */
    uint32_t thr = histogram_peak_threshold(m->ref, n, m->hist_min_dev);
    for (uint32_t i = 0; i < n; ++i) {
        if (!m->bad[i] && (uint32_t)m->ref[i] > thr) {
            m->bad[i] = 1;
            nb++;
        }
    }
    m->bad_count = nb;
    build_fill_order(m);
    /* fill the reference image itself */
    uint8_t *work = (uint8_t *)malloc(n);
    if (work == NULL) {
        return MAG160C_ERR_NO_MEMORY;
    }
    memcpy(work, m->bad, n);
    for (uint32_t i = 0; i < m->order_len; ++i) {
        uint32_t x = m->order_x[i], y = m->order_y[i];
        uint32_t v = neighbor_mean(m->ref, work, m->width, m->height, x, y);
        if (v > 0) {
            m->ref[y * m->width + x] = (uint16_t)v;
            work[y * m->width + x] = 0;
        }
    }
    free(work);
    m->ready = 1;
    mag160c_clear_error();
    return MAG160C_OK;
}

/* Correct one frame in place using the stored topological order. */
mag160c_error_t mag160c_display_badmap_correct(const mag160c_display_badmap_t *m,
                                               uint16_t *frame) {
    if (m == NULL || frame == NULL || !m->ready) {
        mag160c_set_error("mag160c_display_badmap_correct: not ready");
        return MAG160C_ERR_NOT_READY;
    }
    for (uint32_t i = 0; i < m->order_len; ++i) {
        uint32_t x = m->order_x[i], y = m->order_y[i];
        uint32_t v = neighbor_mean(frame, m->bad, m->width, m->height, x, y);
        if (v > 0) frame[y * m->width + x] = (uint16_t)v;
    }
    mag160c_clear_error();
    return MAG160C_OK;
}

/* MOG-style per-pixel reference tracking (anti-ghost):
 *  - |d| < thr  : background pixel, slowly track drift (ref += d/alpha)
 *  - |d| >= thr : foreground (object), reference FROZEN - a static object
 *                 is never absorbed, so moving it away leaves no ghost.
 * Pass ref=null to just classify (not needed by callers today). */
void mag160c_display_ref_track(uint16_t *ref, const uint16_t *live,
                               uint32_t n, int32_t thr, int32_t alpha) {
    if (ref == NULL || live == NULL || thr <= 0 || alpha <= 0) {
        return;
    }
    for (uint32_t i = 0; i < n; ++i) {
        int32_t d = (int32_t)live[i] - (int32_t)ref[i];
        if (d > -thr && d < thr) {
            ref[i] = (uint16_t)((int32_t)ref[i] + d / alpha);
        }
    }
}

/* MOG tracking with self-healing (anti-ghost + anti-startup-noise):
 * fg_count[] counts consecutive foreground frames per pixel.  A pixel stuck
 * as *isolated* foreground (fewer than min_nbr foreground 8-neighbors) for
 * more than heal_frames is a reference error (startup noise baked into the
 * reference, bad pixel) rather than a real object (objects are contiguous)
 * - it is reset to the live value so noise cannot persist forever. */
void mag160c_display_ref_track_heal(uint16_t *ref, const uint16_t *live,
                                    uint32_t w, uint32_t h,
                                    int32_t thr, int32_t alpha,
                                    uint8_t *fg_count, uint32_t heal_frames,
                                    uint32_t min_nbr) {
    if (ref == NULL || live == NULL || w == 0 || h == 0 ||
        thr <= 0 || alpha <= 0 || fg_count == NULL) {
        return;
    }
    if (heal_frames == 0) heal_frames = 1;
    const uint32_t n = w * h;
    for (uint32_t i = 0; i < n; ++i) {
        int32_t d = (int32_t)live[i] - (int32_t)ref[i];
        if (d > -thr && d < thr) {
            ref[i] = (uint16_t)((int32_t)ref[i] + d / alpha);
            fg_count[i] = 0;
        } else {
            if (fg_count[i] < 255) fg_count[i]++;
            if (fg_count[i] >= heal_frames) {
                uint32_t x = i % w, y = i / w;
                uint32_t nbr_fg = 0;
                for (int32_t dy = -1; dy <= 1; ++dy) {
                    for (int32_t dx = -1; dx <= 1; ++dx) {
                        if (dx == 0 && dy == 0) continue;
                        int32_t xx = (int32_t)x + dx, yy = (int32_t)y + dy;
                        if (xx < 0 || (uint32_t)xx >= w ||
                            yy < 0 || (uint32_t)yy >= h) {
                            continue;
                        }
                        uint32_t nbr = (uint32_t)yy * w + (uint32_t)xx;
                        int32_t dn = (int32_t)live[nbr] - (int32_t)ref[nbr];
                        if (dn > thr || dn < -thr) nbr_fg++;
                    }
                }
                if (nbr_fg < min_nbr) {
                    ref[i] = live[i];   /* heal reference error */
                    fg_count[i] = 0;
                }
            }
        }
    }
}

/* Flat-field (NUC) correction: out[i] = live[i] - (ref[i] - mean(ref)).
 * Removes the fixed sensor mura (measured row span 15704 -> 401 counts on
 * the MAG160C), so the absolute temperature image is smooth.  The reference
 * is the fixed pattern only - it must NOT track the scene (see
 * mag160c_display_ref_track_heal for the freeze/self-heal model and the
 * object-free guard at capture time), otherwise objects ghost. */
mag160c_error_t mag160c_display_nuc(const uint16_t *live, const uint16_t *ref,
                                    uint32_t n, uint16_t *out) {
    if (live == NULL || ref == NULL || out == NULL || n == 0) {
        mag160c_set_error("mag160c_display_nuc: null argument");
        return MAG160C_ERR_INVALID_ARGUMENT;
    }
    uint64_t s = 0;
    for (uint32_t i = 0; i < n; ++i) s += ref[i];
    const int32_t mean = (int32_t)(s / n);
    for (uint32_t i = 0; i < n; ++i) {
        int64_t v = (int64_t)live[i] - ((int64_t)ref[i] - mean);
        if (v < 0) v = 0;
        if (v > 65535) v = 65535;
        out[i] = (uint16_t)v;
    }
    mag160c_clear_error();
    return MAG160C_OK;
}

/* Re-align the reference after FFC(1): the type=0 baseline shifts (measured
 * up to +1000 counts).  Applies the median of (live-ref) clamped to
 * max_shift as a global offset, robust to a small object in the scene. */
mag160c_error_t mag160c_display_ref_rebase(uint16_t *ref, const uint16_t *live,
                                           uint32_t n, int32_t max_shift) {
    if (ref == NULL || live == NULL || n == 0) {
        mag160c_set_error("mag160c_display_ref_rebase: null argument");
        return MAG160C_ERR_INVALID_ARGUMENT;
    }
    int32_t *d = (int32_t *)malloc(n * sizeof(int32_t));
    if (d == NULL) {
        return MAG160C_ERR_NO_MEMORY;
    }
    for (uint32_t i = 0; i < n; ++i) {
        int32_t v = (int32_t)live[i] - (int32_t)ref[i];
        if (v > 4000) v = 4000;
        if (v < -4000) v = -4000;
        d[i] = v;
    }
    /* median */
    for (uint32_t a = 1; a < n; ++a) {
        int32_t key = d[a];
        uint32_t b = a;
        while (b > 0 && d[b - 1] > key) {
            d[b] = d[b - 1];
            b--;
        }
        d[b] = key;
    }
    int32_t off = d[n / 2];
    if (off > max_shift) off = max_shift;
    if (off < -max_shift) off = -max_shift;
    free(d);
    if (off != 0) {
        for (uint32_t i = 0; i < n; ++i) {
            int64_t v = (int64_t)ref[i] + off;
            if (v < 0) v = 0;
            if (v > 65535) v = 65535;
            ref[i] = (uint16_t)v;
        }
    }
    mag160c_clear_error();
    return MAG160C_OK;
}

/* ------------------------------------------------------------------ */
/* AGC                                                                 */

/* Percentile stretch: find [lo_pct, hi_pct] value range of frame. */
mag160c_error_t mag160c_display_agc_range(const uint16_t *frame, uint32_t n,
                                          uint32_t lo_pct, uint32_t hi_pct,
                                          uint32_t *out_lo, uint32_t *out_hi) {
    if (frame == NULL || out_lo == NULL || out_hi == NULL) {
        mag160c_set_error("mag160c_display_agc_range: null argument");
        return MAG160C_ERR_INVALID_ARGUMENT;
    }
    if (lo_pct >= hi_pct || hi_pct > 100) {
        mag160c_set_error("mag160c_display_agc_range: bad percentiles");
        return MAG160C_ERR_INVALID_ARGUMENT;
    }
    uint32_t hist[65536];
    memset(hist, 0, sizeof(hist));
    uint32_t nz = 0;
    for (uint32_t i = 0; i < n; ++i) {
        if (frame[i] == 0) continue;
        hist[frame[i]]++;
        nz++;
    }
    if (nz == 0) {
        *out_lo = 0;
        *out_hi = 65535;
        return MAG160C_OK;
    }
    uint64_t acc = 0, lo = (uint64_t)nz * lo_pct / 100, hi = (uint64_t)nz * hi_pct / 100;
    uint32_t mn = 0, mx = 0;
    for (uint32_t k = 0; k < 65536; ++k) {
        acc += hist[k];
        if (acc >= lo && mn == 0) mn = k;
        if (acc >= hi) {
            mx = k;
            break;
        }
    }
    if (mx <= mn + 50) {
        mn = 0;
        mx = 65535;
    }
    *out_lo = mn;
    *out_hi = mx;
    mag160c_clear_error();
    return MAG160C_OK;
}

/* Adaptive diff AGC: span = clamp(2*mean|d|, min_span, max_span). */
mag160c_error_t mag160c_display_diff_span(const int32_t *diff, uint32_t n,
                                          uint32_t min_span, uint32_t max_span,
                                          uint32_t *out_span) {
    if (diff == NULL || out_span == NULL) {
        mag160c_set_error("mag160c_display_diff_span: null argument");
        return MAG160C_ERR_INVALID_ARGUMENT;
    }
    uint64_t sum = 0;
    uint32_t cnt = 0;
    for (uint32_t i = 0; i < n; ++i) {
        int32_t d = diff[i];
        if (d < 0) d = -d;
        if (d > 2) {
            sum += (uint64_t)d;
            cnt++;
        }
    }
    uint64_t span = cnt ? 2 * sum / cnt : 0;
    if (span < min_span) span = min_span;
    if (span > max_span) span = max_span;
    *out_span = (uint32_t)span;
    mag160c_clear_error();
    return MAG160C_OK;
}

/* ------------------------------------------------------------------ */
/* pseudo color                                                         */

/* FLIR-style ironbow anchors, v in [0,255] */
void mag160c_display_ironbow(uint32_t v, uint8_t *r, uint8_t *g, uint8_t *b) {
    static const uint8_t anchors[7][3] = {
        {0, 0, 0}, {23, 0, 60}, {93, 0, 128}, {170, 31, 83},
        {226, 117, 29}, {249, 196, 66}, {255, 255, 220}};
    double f = (v & 0xff) / 255.0 * 6.0;
    uint32_t i = (uint32_t)f;
    if (i > 5) i = 5;
    double t = f - i;
    *r = (uint8_t)(anchors[i][0] + t * (anchors[i + 1][0] - anchors[i][0]));
    *g = (uint8_t)(anchors[i][1] + t * (anchors[i + 1][1] - anchors[i][1]));
    *b = (uint8_t)(anchors[i][2] + t * (anchors[i + 1][2] - anchors[i][2]));
}

/* ------------------------------------------------------------------ */
/* FFC scheduler (official demo cadence)                               */

void mag160c_ffc_scheduler_init(mag160c_ffc_scheduler_t *s, uint32_t period,
                                uint32_t gap) {
    if (s == NULL) {
        return;
    }
    memset(s, 0, sizeof(*s));
    s->period = period ? period : 400;
    s->gap = gap ? gap : 9;
}

/* Call once per complete frame.  Returns:
 *   -1 : nothing to send
 *   >=0: FFC param to send (0 or 1), then call again after the response. */
int32_t mag160c_ffc_scheduler_tick(mag160c_ffc_scheduler_t *s) {
    if (s == NULL) {
        return -1;
    }
    s->frames++;
    if (!s->started) {
        /* official: FFC(1) after ~10 complete frames switches to type=0 */
        if (s->frames >= 10) {
            s->started = 1;
            s->frames = 0;
            return 1;
        }
        return -1;
    }
    if (s->wait1 && s->frames >= s->gap) {
        s->wait1 = 0;
        s->frames = 0;
        return 1;
    }
    if (!s->wait1 && s->frames >= s->period) {
        s->wait1 = 1;
        s->frames = 0;
        return 0;
    }
    return -1;
}

/* Force an immediate FFC pair: returns 0 (send FFC(0) now); the following
 * tick() calls will then return 1 (send FFC(1)) after `gap` frames.  This
 * is the official manual-FFC behavior (FFC(0) then 9 frames later FFC(1)). */
int32_t mag160c_ffc_scheduler_trigger(mag160c_ffc_scheduler_t *s) {
    if (s == NULL) {
        return -1;
    }
    s->started = 1;
    s->wait1 = 1;
    s->frames = 0;
    return 0;
}

/* ------------------------------------------------------------------ */
/* two-point temperature calibration                                   */

/* counts = a * temp + b ;  solve from two known points */
mag160c_error_t mag160c_temp_calibrate_linear(double counts0, double temp0,
                                              double counts1, double temp1,
                                              double *out_a, double *out_b) {
    if (out_a == NULL || out_b == NULL) {
        mag160c_set_error("mag160c_temp_calibrate_linear: null argument");
        return MAG160C_ERR_INVALID_ARGUMENT;
    }
    if (counts1 == counts0) {
        mag160c_set_error("mag160c_temp_calibrate_linear: identical counts");
        return MAG160C_ERR_INVALID_ARGUMENT;
    }
    *out_a = (temp1 - temp0) / (counts1 - counts0);
    *out_b = temp0 - *out_a * counts0;
    mag160c_clear_error();
    return MAG160C_OK;
}

double mag160c_temp_apply_linear(double counts, double a, double b) {
    return a * counts + b;
}