#include "ohm_logic.h"
#include <stdio.h>

// ------------------------------------------------------------ ADC to ohms

float ratioToOhms(const RawReading& r, float rRef) {
    int32_t ref = r.ref - r.refOff;
    // Scale the reference to 0.256 V-range counts so MIN_REF_COUNTS means the
    // same current whichever range it was read on.
    float ref256 = ref * (r.refFsr / 0.256f);
    if (ref256 < MIN_REF_COUNTS) return R_OPEN;
    float vXY  = (r.xy - r.xyOff) * (r.xyFsr / 32768.0f);
    float vRef = ref * (r.refFsr / 32768.0f);
    float ohms = rRef * vXY / vRef;
    return ohms < 0 ? 0 : ohms;   // a few counts of noise on a dead short
}

// ------------------------------------------------------------- verdicts

Verdict conductorVerdict(float ohms, float limit) {
    if (ohms <= limit)      return V_PASS;
    if (ohms <= CLOSED_MAX) return V_HIGH;
    return V_OPEN;
}

Verdict insulationVerdict(float ohms) {
    if (ohms < SHORT_MAX) return V_SHORT;
    if (ohms < LEAK_MAX)  return V_LEAKY;
    return V_PASS;
}

const char* verdictWord(Verdict v) {
    switch (v) {
        case V_PASS:  return "ok";
        case V_HIGH:  return "HIGH";
        case V_OPEN:  return "OPEN";
        case V_SHORT: return "SHORT";
        case V_LEAKY: return "LEAKY";
        default:      return "...";
    }
}

// Integer formatting: the Pico's printf may lack %f, and the PC tests must
// print exactly what the screen shows.
void formatOhms(float ohms, char* out, int n) {
    if (ohms >= LEAK_MAX) { snprintf(out, n, "OPEN"); return; }
    if (ohms >= 1000.0f)  { snprintf(out, n, "%ldk", (long)(ohms / 1000.0f + 0.5f)); return; }
    if (ohms >= 100.0f)   { snprintf(out, n, "%ld", (long)(ohms + 0.5f)); return; }
    if (ohms >= 10.0f) {
        long t = (long)(ohms * 10.0f + 0.5f);
        snprintf(out, n, "%ld.%ld", t / 10, t % 10);
        return;
    }
    long m = (long)(ohms * 1000.0f + 0.5f);
    if (m >= 10000) { snprintf(out, n, "10.0"); return; }   // 9.9996 rounds up
    snprintf(out, n, "%ld.%03ld", m / 1000, m % 1000);
}

// ----------------------------------------------------------- cord test

static const Term J1[3] = { T_J1A, T_J1B, T_J1C };
static const Term J2[3] = { T_J2A, T_J2B, T_J2C };

CordReport testCord(Meter& m) {
    CordReport r;
    r.type = CORD_NONE;
    bool anyClosed = false;

    for (int ln = 0; ln < 3; ++ln) {
        WireResult& w = r.wire[ln];
        w.crossed = false;
        w.farEnd  = J2[ln];
        w.ohms    = m.ohms(J1[ln], J2[ln]);
        if (ln == 0) {
            // Line A ends at J2-A on a 3-prong cord, at the clip on a foil or
            // sabre cord. Whichever conducts better is where it went.
            float clip = m.ohms(T_J1A, T_CLIP);
            if (clip < w.ohms) { w.ohms = clip; w.farEnd = T_CLIP; }
        }
        if (w.ohms > CLOSED_MAX) {
            // Not on its own pin: is it wired to another one?
            const Term others[4] = { T_J2A, T_J2B, T_J2C, T_CLIP };
            for (Term t : others) {
                if (t == J2[ln] || (ln == 0 && t == T_CLIP)) continue;
                float o = m.ohms(J1[ln], t);
                if (o <= CLOSED_MAX) { w.ohms = o; w.farEnd = t; w.crossed = true; break; }
            }
        }
        w.v = w.crossed ? V_OPEN : conductorVerdict(w.ohms, LIM_CORD);
        if (w.ohms <= CLOSED_MAX) anyClosed = true;
    }

    if (anyClosed) {
        bool aClip = r.wire[0].farEnd == T_CLIP && !r.wire[0].crossed;
        r.type = aClip ? CORD_CLIP : CORD_3PIN;
    }

    // With both ends plugged the J2 pins are separate terminals, so the only
    // path between two J1 pins is a fault inside the cord.
    const Term pa[3] = { T_J1A, T_J1B, T_J1A };
    const Term pb[3] = { T_J1B, T_J1C, T_J1C };
    for (int i = 0; i < 3; ++i) {
        r.shortOhms[i] = m.ohms(pa[i], pb[i]);
        r.shortV[i]    = insulationVerdict(r.shortOhms[i]);
    }

    r.pass = r.type != CORD_NONE;
    for (int i = 0; i < 3; ++i)
        r.pass = r.pass && r.wire[i].v == V_PASS && r.shortV[i] == V_PASS;
    return r;
}

void CordMemory::remember(const CordReport& r) {
    // Only a cord whose three wires all conduct on their own pins is a usable
    // reference; a high wire is still subtracted (it is the cord in use).
    for (int i = 0; i < 3; ++i)
        if (r.wire[i].crossed || r.wire[i].ohms > CLOSED_MAX) return;
    valid = true;
    a = r.wire[0].ohms; b = r.wire[1].ohms; c = r.wire[2].ohms;
}

static float minus(float raw, float cord) {
    if (raw >= R_OPEN) return raw;
    float w = raw - cord;
    return w < 0 ? 0 : w;
}

// -------------------------------------------------------- weapon tests
// Contacts bounce as a tip is pressed or released, so a resistance only
// counts toward a "worst" once two passes running agree (the lower of the
// pair): a bounce can never fail a good weapon.

void FoilSabreTest::update(float rBC, float rCprobe, const CordMemory& cord) {
    float alone = minus(rBC, cord.valid ? cord.b + cord.c : 0);
    if (rBC <= CLOSED_MAX) {
        if (closedNow) {                                  // second closed pass in a row
            float steady = alone < restOhms ? alone : restOhms;
            if (!seenRest || steady > worstRest) worstRest = steady;
            seenRest = true;
        }
        restOhms = alone;
        closedNow = true;
        partialRun = 0;
    } else {
        if (rBC >= LEAK_MAX) {
            if (closedNow) { seenOpen = true; ++presses; }   // closed last pass, open now: a press
            partialRun = 0;
        } else if (++partialRun >= 2) {
            partial = true;
        }
        closedNow = false;
    }
    if (rCprobe <= CLOSED_MAX) {
        float p = minus(rCprobe, cord.valid ? cord.c : 0);
        if (probeNow) {
            float steady = p < probeOhms ? p : probeOhms;
            if (!seenProbe || steady > worstProbe) worstProbe = steady;
            seenProbe = true;
        }
        probeOhms = p;
        probeNow = true;
    } else {
        probeNow = false;
    }
}

void EpeeTest::update(float rAB, float rAC, float rBC, const CordMemory& cord) {
    if (rAC < minGuard) minGuard = rAC;
    if (rBC < minGuard) minGuard = rBC;
    if (rAB >= LEAK_MAX) {
        seenOpenRest = true;
        closedNow = false;
    } else if (rAB <= CLOSED_MAX) {
        float alone = minus(rAB, cord.valid ? cord.a + cord.b : 0);
        if (!closedNow) ++presses;
        else {
            float steady = alone < pressedOhms ? alone : pressedOhms;
            if (!seenPressed || steady > worstPressed) worstPressed = steady;
            seenPressed = true;
        }
        pressedOhms = alone;
        closedNow = true;
    }
}

Verdict EpeeTest::verdict() const {
    if (guardVerdict() != V_PASS) return guardVerdict();
    if (!seenOpenRest || !seenPressed) return V_WAIT;
    return conductorVerdict(worstPressed, LIM_EPEE);
}

void LameTest::update(float rProbeClip, float rProbeJ1A, const CordMemory& cord) {
    // Through the cord at J1 the cord's A wire is in the path; subtract it.
    float viaCord = minus(rProbeJ1A, cord.valid ? cord.a : 0);
    liveOhms = rProbeClip < viaCord ? rProbeClip : viaCord;
    // Anything under 1 k is the probe on the fabric (a dead patch reads tens
    // or hundreds of ohms); above that the probe is lifted.
    // A spot must read the same twice running (about 60 ms) to count, so the
    // probe bouncing as it lands does not log a fake bad spot.
    if (liveOhms < SHORT_MAX && prevOhms < SHORT_MAX) {
        float steady = liveOhms < prevOhms ? liveOhms : prevOhms;
        if (!seenContact || steady > worst) worst = steady;
        seenContact = true;
    }
    prevOhms = liveOhms;
}

Verdict LameTest::verdict() const {
    if (!seenContact) return V_WAIT;
    return conductorVerdict(worst, limit);
}

// ------------------------------------------------------------ flex test

void FlexTracker::sample(int ln, bool conducting, uint32_t tUs) {
    LineFlex& L = line[ln];
    if (conducting) {
        if (L.deadRun >= 2 && L.everClosed) {
            ++L.breaks;
            uint32_t d = tUs - L.deadSinceUs;
            if (d > L.longestUs) L.longestUs = d;
        }
        L.deadRun = 0;
        L.openNow = false;
        L.everClosed = true;
        return;
    }
    if (L.deadRun == 0) L.deadSinceUs = tUs;
    if (L.deadRun < 255) ++L.deadRun;
    if (L.deadRun >= 2) L.openNow = true;
}


// ------------------------------------------------------------ auto mode

Scan scanAll(Meter& m) {
    Scan s;
    s.j1j2[0]   = m.ohms(T_J1A, T_J2A);
    s.j1j2[1]   = m.ohms(T_J1B, T_J2B);
    s.j1j2[2]   = m.ohms(T_J1C, T_J2C);
    s.aClip     = m.ohms(T_J1A, T_CLIP);
    s.ab        = m.ohms(T_J1A, T_J1B);
    s.bc        = m.ohms(T_J1B, T_J1C);
    s.ac        = m.ohms(T_J1A, T_J1C);
    s.probeClip = m.ohms(T_CLIP, T_PROBE);
    s.probeA    = m.ohms(T_J1A, T_PROBE);
    s.probeC    = m.ohms(T_J1C, T_PROBE);
    return s;
}

void AutoTester::reset() { *this = AutoTester(); }

void AutoTester::setLight(Light l, bool blinkIt) {
    if (l != light) {
        if (l == L_GREEN) sound = SND_PASS;
        else if (l == L_RED) sound = SND_FAIL;
    }
    light = l;
    blink = blinkIt;
}

void AutoTester::enter(Situation s, uint32_t nowMs) {
    (void)nowMs;
    sit = s;
    shown = s;
    switch (s) {
        case S_CORD:
            cord = CordReport();
            cordFull = false;
            for (int i = 0; i < 3; ++i) { worstWire[i] = 0; minShort[i] = R_OPEN; openRun[i] = 0; }
            noneRun = 0;
            breaks = 0;
            break;
        case S_FOILSABRE: fs.reset(); break;
        case S_EPEE:
            ep.reset();
            ep.seenOpenRest = lastAbOpen;   // A-B was open on the pass before the press
            break;
        case S_PROBE: probe.reset(); break;
        default: break;
    }
    light = L_OFF;
    blink = false;
}

void AutoTester::update(const Scan& s, const CordMemory& mem, uint32_t nowMs) {
    sound = SND_NONE;
    bool cordHere = s.aClip <= CLOSED_MAX;
    for (int i = 0; i < 3; ++i) if (s.j1j2[i] <= CLOSED_MAX) cordHere = true;
    bool probeHere = s.probeClip < SHORT_MAX || s.probeA < SHORT_MAX;
    bool abC = s.ab <= CLOSED_MAX, bcC = s.bc <= CLOSED_MAX, acC = s.ac <= CLOSED_MAX;

    // A long enough gap with nothing connected ends the session: whatever
    // comes next is new (an epee swapped for a foil is not a guard short).
    uint32_t gap = sit == S_PROBE ? FRESH_PROBE_MS : FRESH_MS;
    if (sit != S_IDLE && nowMs - lastSeenMs > gap) sit = S_IDLE;

    Situation want = S_IDLE;
    if (cordHere)       want = S_CORD;
    else if (probeHere) want = S_PROBE;
    else if (abC)       want = S_EPEE;
    else if (bcC)       want = sit == S_EPEE ? S_EPEE : S_FOILSABRE;   // in an epee: a guard short
    else if (acC)       want = sit == S_EPEE ? S_EPEE : S_ODD;
    else if (sit == S_FOILSABRE && s.bc < LEAK_MAX) want = S_FOILSABRE;  // dirty, part-open point

    // While probing a lame, a foil still on the cord must not take over when
    // the probe lifts between strokes: only a cord in both sockets can, until
    // the probe has been off for FRESH_PROBE_MS.
    if (sit == S_PROBE && want != S_CORD && want != S_PROBE) want = S_IDLE;

    if (want != S_IDLE) {
        lastSeenMs = nowMs;
        if (want != sit) enter(want, nowMs);
    }
    lastAbOpen = s.ab >= LEAK_MAX;

    switch (sit) {
        case S_FOILSABRE: {
            uint16_t before = fs.presses;
            fs.update(s.bc, s.probeC, mem);
            if (!fs.seenRest && !fs.closedNow)            setLight(L_OFF);
            else if (fs.seenRest && fs.worstRest > LIM_FOIL) setLight(L_RED);
            else if (fs.seenProbe && !fs.seenOpen && fs.worstProbe > LIM_SABRE) setLight(L_RED);
            else if (fs.partial)                          setLight(L_YELLOW);
            else if (fs.seenOpen)                         setLight(L_GREEN);
            else if (!fs.seenRest)                        setLight(L_OFF);
            else if (fs.worstRest <= LIM_SABRE)           setLight(L_GREEN);
            else                                          setLight(L_YELLOW, true);   // foil only: press the tip
            if (fs.presses != before && sound == SND_NONE) sound = SND_TICK;
            break;
        }
        case S_EPEE: {
            uint16_t before = ep.presses;
            ep.update(s.ab, s.ac, s.bc, mem);
            Verdict g = ep.guardVerdict();
            if (g == V_SHORT)                                    setLight(L_RED);
            else if (ep.seenPressed && ep.worstPressed > LIM_EPEE) setLight(L_RED);
            else if (g == V_LEAKY)                               setLight(L_YELLOW);
            else if (ep.seenPressed && ep.seenOpenRest)          setLight(L_GREEN);
            else if (ep.closedNow || ep.seenPressed)             setLight(L_YELLOW, true);   // let go of the tip
            else                                                 setLight(L_OFF);
            if (ep.presses != before && sound == SND_NONE) sound = SND_TICK;
            break;
        }
        case S_PROBE: {
            float worstBefore = probe.worst;
            probe.update(s.probeClip, s.probeA, mem);
            float live = probe.liveOhms;
            // The light follows the probe live: green fine for anything, yellow
            // fine for a lame or mask but too high for a wire, red too high for both.
            light = live >= SHORT_MAX ? L_OFF : live <= LIM_WIRE ? L_GREEN : live <= LIM_LAME ? L_YELLOW : L_RED;
            blink = false;
            // Buzz when a steady reading finds a new worst spot over the lame
            // limit; no chirps while sweeping good fabric.
            if (probe.worst > LIM_LAME && probe.worst > worstBefore) sound = SND_FAIL;
            break;
        }
        case S_ODD:
            setLight(L_RED);
            break;
        case S_IDLE:
            // Keep showing the last answer for a while, then go dark.
            if (nowMs - lastSeenMs > 30000) { light = L_OFF; blink = false; shown = S_IDLE; }
            break;
        default:
            break;   // S_CORD runs through updateCord
    }
}

Verdict AutoTester::wireVerdict(int i) const {
    const WireResult& w = cord.wire[i];
    bool open = w.crossed || w.ohms > CLOSED_MAX;
    if (open && openRun[i] >= 2) return V_OPEN;
    float r = open ? worstWire[i] : (w.ohms > worstWire[i] ? w.ohms : worstWire[i]);
    return conductorVerdict(r, LIM_CORD);
}

void AutoTester::updateCord(const CordReport& r, CordMemory& mem, uint32_t nowMs) {
    sound = SND_NONE;
    if (r.type == CORD_NONE) {
        // Two empty passes in a row: the cord is out. Keep the answer showing.
        if (++noneRun >= 2) sit = S_IDLE;
        return;
    }
    noneRun = 0;
    lastSeenMs = nowMs;
    cord = r;

    bool allClosed = true;
    for (int i = 0; i < 3; ++i) {
        const WireResult& w = r.wire[i];
        bool closed = !w.crossed && w.ohms <= CLOSED_MAX;
        if (!closed) {
            allClosed = false;
            if (openRun[i] < 255) ++openRun[i];
        } else {
            openRun[i] = 0;
            if (cordFull && w.ohms > worstWire[i]) worstWire[i] = w.ohms;
        }
    }
    if (allClosed) {
        if (!cordFull) { cordFull = true; cordFullSinceMs = nowMs; }
        mem.remember(r);
    }
    for (int i = 0; i < 3; ++i) {
        if (cordFull) { if (r.shortOhms[i] < minShort[i]) minShort[i] = r.shortOhms[i]; }
        else minShort[i] = r.shortOhms[i];
    }

    bool red = breaks > 0, yellow = false, pending = !cordFull;
    for (int i = 0; i < 3; ++i) {
        Verdict v = wireVerdict(i);
        if (v == V_OPEN || v == V_HIGH) red = true;
        if (r.wire[i].ohms > CLOSED_MAX && openRun[i] < 2) pending = true;
        Verdict sv = shortVerdict(i);
        if (sv == V_SHORT) red = true;
        if (sv == V_LEAKY) yellow = true;
    }
    if (red)          setLight(L_RED);
    else if (pending) {}                     // going in or coming out: keep the last answer
    else if (yellow)  setLight(L_YELLOW);
    else              setLight(L_GREEN);
}

void AutoTester::noteBreaks(uint32_t total) {
    sound = SND_NONE;
    if (total > breaks) {
        breaks = total;
        light = L_RED;
        blink = false;
        sound = SND_BREAK;
    }
}