#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stddef.h>
#include <stdint.h>
#include <stdarg.h>
#include <unistd.h>

#include "bscan/bscan.h"
#include "svf.h"

/* ------------------------------------------------------------------ *
 *  Player state
 * ------------------------------------------------------------------ */

typedef struct {
    jtag_core   *jc;
    svf_log_fn   log;
    void        *user;
    long         line;       /* 1-based, for diagnostics */
    svf_stats    stats;

    /* Sticky TDI / MASK per scan type ([0] = DR, [1] = IR). TDO is not
     * sticky: a compare happens only when TDO is given on that scan. */
    struct {
        int      len;
        uint8_t *tdi;
        uint8_t *mask;
    } st[2];
} svf_player;

static void slog(svf_player *p, int is_error, const char *fmt, ...)
{
    char buf[256];
    va_list ap;
    if (!p->log) return;
    va_start(ap, fmt);
    vsnprintf(buf, sizeof(buf), fmt, ap);
    va_end(ap);
    p->log(p->user, is_error, buf);
}

/* ------------------------------------------------------------------ *
 *  Hex / token helpers
 * ------------------------------------------------------------------ */

static int hexval(int c)
{
    if (c >= '0' && c <= '9') return c - '0';
    if (c >= 'a' && c <= 'f') return c - 'a' + 10;
    if (c >= 'A' && c <= 'F') return c - 'A' + 10;
    return -1;
}

/* Parse `hexlen` chars of hex (whitespace allowed) representing `nbits`
 * bits, LSB shifted first, into buf (LSB-first per byte, as the bscan_*
 * shifters expect). Returns 0, or -1 on a stray character. */
static int parse_hex_bits(const char *hex, int hexlen, int nbits, uint8_t *buf)
{
    int nbytes = (nbits + 7) / 8;
    int i, nib = 0;

    memset(buf, 0, (size_t)nbytes);
    for (i = hexlen - 1; i >= 0; i--) {
        int v;
        char c = hex[i];
        if (c == ' ' || c == '\t' || c == '\r' || c == '\n')
            continue;
        v = hexval((unsigned char)c);
        if (v < 0) return -1;
        if (v) {
            int k;
            for (k = 0; k < 4; k++) {
                int bit = nib * 4 + k;
                if (bit < nbits && (v & (1 << k)))
                    buf[bit / 8] |= (uint8_t)(1u << (bit & 7));
            }
        }
        nib++;
    }
    return 0;
}

typedef struct { const char *p, *end; } svf_cur;

enum { TK_END = 0, TK_WORD, TK_HEX };

/* Next token: TK_WORD copies into word[wordsz]; TK_HEX returns a pointer
 * and length into the source (no copy — bodies can be large). */
static int svf_next(svf_cur *c, char *word, int wordsz,
                    const char **hp, int *hlen)
{
    while (c->p < c->end &&
           (*c->p == ' ' || *c->p == '\t' || *c->p == '\r' || *c->p == '\n'))
        c->p++;
    if (c->p >= c->end) return TK_END;

    if (*c->p == '(') {
        const char *start = ++c->p;
        while (c->p < c->end && *c->p != ')') c->p++;
        *hp = start;
        *hlen = (int)(c->p - start);
        if (c->p < c->end) c->p++;       /* skip ')' */
        return TK_HEX;
    }

    {
        int n = 0;
        while (c->p < c->end && *c->p != ' ' && *c->p != '\t' &&
               *c->p != '\r' && *c->p != '\n' && *c->p != '(') {
            if (n < wordsz - 1) word[n++] = *c->p;
            c->p++;
        }
        word[n] = '\0';
    }
    return TK_WORD;
}

static int ci_eq(const char *a, const char *b)
{
    while (*a && *b) {
        int ca = *a, cb = *b;
        if (ca >= 'a' && ca <= 'z') ca -= 32;
        if (cb >= 'a' && cb <= 'z') cb -= 32;
        if (ca != cb) return 0;
        a++; b++;
    }
    return *a == *b;
}

/* ------------------------------------------------------------------ *
 *  Commands
 * ------------------------------------------------------------------ */

/* SIR / SDR: shift IR/DR with optional masked TDO compare. */
static int do_scan(svf_player *p, int is_ir, svf_cur *c)
{
    char word[64];
    const char *hex;
    int hlen, t;
    int len = -1;
    int nbytes;
    uint8_t *tdi = NULL, *tdo = NULL, *mask = NULL, *cap = NULL;
    int have_tdi = 0, have_tdo = 0, have_mask = 0;
    int idx = is_ir ? 1 : 0;
    int rc = -1, i;

    /* length */
    if (svf_next(c, word, sizeof(word), &hex, &hlen) != TK_WORD) {
        slog(p, 1, "line %ld: %s missing length", p->line, is_ir ? "SIR" : "SDR");
        return -1;
    }
    len = atoi(word);
    if (len <= 0) {
        slog(p, 1, "line %ld: %s bad length '%s'", p->line, is_ir ? "SIR" : "SDR", word);
        return -1;
    }
    nbytes = (len + 7) / 8;

    /* fields */
    while ((t = svf_next(c, word, sizeof(word), &hex, &hlen)) != TK_END) {
        const char *fld = word;
        uint8_t **dst;
        int *flag;
        if (t != TK_WORD) goto out;
        if      (ci_eq(fld, "TDI"))   { dst = &tdi;  flag = &have_tdi;  }
        else if (ci_eq(fld, "TDO"))   { dst = &tdo;  flag = &have_tdo;  }
        else if (ci_eq(fld, "MASK"))  { dst = &mask; flag = &have_mask; }
        else if (ci_eq(fld, "SMASK")) { dst = NULL;  flag = NULL;       }
        else { slog(p, 1, "line %ld: unexpected '%s' in scan", p->line, fld); goto out; }

        if (svf_next(c, word, sizeof(word), &hex, &hlen) != TK_HEX) {
            slog(p, 1, "line %ld: '%s' without (hex)", p->line, fld);
            goto out;
        }
        if (!dst) continue;                       /* SMASK: parsed, ignored */
        *dst = malloc((size_t)nbytes);
        if (!*dst) { slog(p, 1, "out of memory"); goto out; }
        if (parse_hex_bits(hex, hlen, len, *dst) < 0) {
            slog(p, 1, "line %ld: bad hex in %s", p->line, fld);
            goto out;
        }
        *flag = 1;
    }

    /* Resolve sticky TDI: reuse last value of same length when omitted. */
    if (!have_tdi) {
        if (p->st[idx].tdi && p->st[idx].len == len) {
            tdi = malloc((size_t)nbytes);
            if (!tdi) { slog(p, 1, "out of memory"); goto out; }
            memcpy(tdi, p->st[idx].tdi, (size_t)nbytes);
        } else {
            tdi = calloc(1, (size_t)nbytes);      /* default: shift zeros */
            if (!tdi) { slog(p, 1, "out of memory"); goto out; }
        }
    }
    /* Resolve sticky MASK when a compare is wanted but no MASK given. */
    if (have_tdo && !have_mask) {
        mask = malloc((size_t)nbytes);
        if (!mask) { slog(p, 1, "out of memory"); goto out; }
        if (p->st[idx].mask && p->st[idx].len == len)
            memcpy(mask, p->st[idx].mask, (size_t)nbytes);
        else
            memset(mask, 0xFF, (size_t)nbytes);   /* default: all care */
    }

    /* Shift. */
    if (have_tdo) {
        cap = calloc(1, (size_t)nbytes);
        if (!cap) { slog(p, 1, "out of memory"); goto out; }
    }
    if ((is_ir ? bscan_shift_ir(p->jc, tdi, cap, len)
               : bscan_shift_dr(p->jc, tdi, cap, len)) < 0) {
        slog(p, 1, "line %ld: shift %s failed (probe ok?)", p->line, is_ir ? "IR" : "DR");
        goto out;
    }
    p->stats.scans++;

    /* Masked compare. */
    if (have_tdo) {
        p->stats.compares++;
        for (i = 0; i < len; i++) {
            int m = (mask[i / 8] >> (i & 7)) & 1u;
            if (!m) continue;
            if (((cap[i / 8] >> (i & 7)) & 1u) != ((tdo[i / 8] >> (i & 7)) & 1u)) {
                slog(p, 1, "line %ld: %s TDO mismatch at bit %d (len %d)",
                     p->line, is_ir ? "SIR" : "SDR", i, len);
                goto out;
            }
        }
    }

    /* Update sticky state. */
    if (have_tdi || p->st[idx].len != len) {
        free(p->st[idx].tdi);
        p->st[idx].tdi = malloc((size_t)nbytes);
        if (p->st[idx].tdi) memcpy(p->st[idx].tdi, tdi, (size_t)nbytes);
    }
    if (have_mask) {
        free(p->st[idx].mask);
        p->st[idx].mask = malloc((size_t)nbytes);
        if (p->st[idx].mask) memcpy(p->st[idx].mask, mask, (size_t)nbytes);
    }
    p->st[idx].len = len;

    rc = 0;
out:
    free(tdi); free(tdo); free(mask); free(cap);
    return rc;
}

/* HIR/HDR/TIR/TDR: header/trailer bits. Single-device only -> must be 0. */
static int do_header(svf_player *p, const char *kw, svf_cur *c)
{
    char word[64];
    const char *hex; int hlen, t, len;

    if (svf_next(c, word, sizeof(word), &hex, &hlen) != TK_WORD) {
        slog(p, 1, "line %ld: %s missing length", p->line, kw);
        return -1;
    }
    len = atoi(word);
    if (len != 0) {
        slog(p, 1, "line %ld: %s %d — multi-device chains not supported",
             p->line, kw, len);
        return -1;
    }
    /* drain any TDI()/TDO()/... that may accompany a 0-length header */
    while ((t = svf_next(c, word, sizeof(word), &hex, &hlen)) != TK_END)
        ;
    return 0;
}

static int do_endstate(svf_player *p, const char *kw, svf_cur *c)
{
    char word[64]; const char *hex; int hlen;
    if (svf_next(c, word, sizeof(word), &hex, &hlen) != TK_WORD) {
        slog(p, 1, "line %ld: %s missing state", p->line, kw);
        return -1;
    }
    if (!ci_eq(word, "IDLE")) {
        slog(p, 1, "line %ld: %s %s — only IDLE end state supported",
             p->line, kw, word);
        return -1;
    }
    return 0;       /* scans already finish in Run-Test/Idle */
}

static int do_state(svf_player *p, svf_cur *c)
{
    char word[64]; const char *hex; int hlen, t;
    int want_reset = 0, only_idle = 1;

    while ((t = svf_next(c, word, sizeof(word), &hex, &hlen)) != TK_END) {
        if (t != TK_WORD) continue;
        if (ci_eq(word, "RESET"))      want_reset = 1;
        else if (ci_eq(word, "IDLE"))  /* ok */;
        else                           only_idle = 0;
    }
    if (want_reset)
        return bscan_tap_reset(p->jc);
    if (only_idle)
        return 0;       /* already in Idle between commands */
    slog(p, 1, "line %ld: STATE — only RESET / IDLE supported", p->line);
    return -1;
}

static int do_runtest(svf_player *p, svf_cur *c)
{
    char word[64]; const char *hex; int hlen, t;
    long cycles = 0;
    double secs = 0.0;
    char pending[64];
    int have_pending = 0;

    while ((t = svf_next(c, word, sizeof(word), &hex, &hlen)) != TK_END) {
        if (t != TK_WORD) continue;
        if (have_pending && (ci_eq(word, "TCK") || ci_eq(word, "SCK"))) {
            cycles = atol(pending);
            have_pending = 0;
        } else if (have_pending && ci_eq(word, "SEC")) {
            secs = atof(pending);
            have_pending = 0;
        } else if (word[0] == '.' || (word[0] >= '0' && word[0] <= '9')) {
            strncpy(pending, word, sizeof(pending) - 1);
            pending[sizeof(pending) - 1] = '\0';
            have_pending = 1;
        } else {
            have_pending = 0;   /* MAXIMUM / ENDSTATE / state names, etc. */
        }
    }

    while (cycles > 0) {
        int chunk = (cycles > 1000000) ? 1000000 : (int)cycles;
        if (bscan_idle_cycles(p->jc, chunk) < 0) {
            slog(p, 1, "line %ld: RUNTEST idle failed", p->line);
            return -1;
        }
        cycles -= chunk;
    }
    if (secs > 0.0) {
        if (secs > 60.0) secs = 60.0;        /* sanity cap */
        usleep((useconds_t)(secs * 1e6));
    }
    return 0;
}

/* ------------------------------------------------------------------ *
 *  Top-level
 * ------------------------------------------------------------------ */

/* Blank out '!' and '//' comments to end-of-line (keep newlines). */
static void strip_comments(char *buf, long len)
{
    long i;
    for (i = 0; i < len; i++) {
        if (buf[i] == '!' || (buf[i] == '/' && i + 1 < len && buf[i + 1] == '/')) {
            while (i < len && buf[i] != '\n') buf[i++] = ' ';
        }
    }
}

static int dispatch(svf_player *p, char *cmd, long cmdlen)
{
    svf_cur c;
    char kw[64];
    const char *hex; int hlen;

    c.p = cmd; c.end = cmd + cmdlen;
    if (svf_next(&c, kw, sizeof(kw), &hex, &hlen) != TK_WORD)
        return 0;       /* blank */

    p->stats.commands++;

    if      (ci_eq(kw, "SDR"))  return do_scan(p, 0, &c);
    else if (ci_eq(kw, "SIR"))  return do_scan(p, 1, &c);
    else if (ci_eq(kw, "RUNTEST")) return do_runtest(p, &c);
    else if (ci_eq(kw, "STATE"))   return do_state(p, &c);
    else if (ci_eq(kw, "ENDIR") || ci_eq(kw, "ENDDR")) return do_endstate(p, kw, &c);
    else if (ci_eq(kw, "HIR") || ci_eq(kw, "HDR") ||
             ci_eq(kw, "TIR") || ci_eq(kw, "TDR")) return do_header(p, kw, &c);
    else if (ci_eq(kw, "TRST") || ci_eq(kw, "FREQUENCY"))
        return 0;       /* accepted; not acted on (single-device, clock set at open) */

    slog(p, 1, "line %ld: unsupported command '%s'", p->line, kw);
    return -1;
}

int svf_play_file(jtag_core *jc, const char *path,
                  svf_log_fn log, void *user, svf_stats *stats)
{
    FILE *f;
    long size, i, cmd_start;
    char *buf;
    svf_player p;
    int rc = 0;

    memset(&p, 0, sizeof(p));
    p.jc = jc; p.log = log; p.user = user; p.line = 1;

    f = fopen(path, "rb");
    if (!f) { slog(&p, 1, "cannot open %s", path); return -1; }
    if (fseek(f, 0, SEEK_END) != 0) { fclose(f); return -1; }
    size = ftell(f);
    if (size <= 0) { fclose(f); slog(&p, 1, "empty file"); return -1; }
    rewind(f);
    buf = malloc((size_t)size + 1);
    if (!buf) { fclose(f); slog(&p, 1, "out of memory"); return -1; }
    if (fread(buf, 1, (size_t)size, f) != (size_t)size) {
        free(buf); fclose(f); slog(&p, 1, "read error"); return -1;
    }
    buf[size] = '\0';
    fclose(f);

    strip_comments(buf, size);

    /* Warm up the link before the first real scan: the FTDI MPSSE's first
     * data read after a fresh open returns stale FIFO content. The normal
     * Viveris flow hides this (jtag_scan/autoinit runs first); do a
     * throwaway reset + DR read so a standalone svf_play is reliable. */
    {
        uint8_t junk[4];
        bscan_tap_reset(jc);
        bscan_shift_dr(jc, NULL, junk, 32);
    }

    /* Split on ';' and dispatch each command, tracking the line number. */
    cmd_start = 0;
    for (i = 0; i < size; i++) {
        if (buf[i] == '\n') p.line++;
        if (buf[i] == ';') {
            rc = dispatch(&p, buf + cmd_start, i - cmd_start);
            if (rc < 0) break;
            cmd_start = i + 1;
        }
    }

    free(buf);
    free(p.st[0].tdi); free(p.st[0].mask);
    free(p.st[1].tdi); free(p.st[1].mask);

    if (stats) *stats = p.stats;
    if (rc == 0)
        slog(&p, 0, "SVF done: %ld commands, %ld scans, %ld compares",
             p.stats.commands, p.stats.scans, p.stats.compares);
    return rc;
}