#!/usr/bin/env python3
"""Measure CaribouLite transmit quadrature and DC-offset coefficients.

Runs on the HOST with a HackRF watching the CaribouLite.

Model. The impairment is y = alpha*x + beta*conj(x) + D, so with the
pre-distortion x' = x + k*conj(x) + d the residual image amplitude is
proportional to |k + m| and the residual carrier to |d + v|, where
m = beta/alpha and v = D/alpha are the two unknowns. Both are affine in
the coefficient, so probing each with a few values and fitting

    y_i = |c_i + u|^2 = |c_i|^2 + 2 Re(c_i) Re(u) + 2 Im(c_i) Im(u) + |u|^2

as a linear least-squares problem in (Re u, Im u, |u|^2) recovers u --
with |u|^2 estimated *independently* of Re u and Im u, which is the
check that matters: if the fitted |u|^2 does not match Re(u)^2+Im(u)^2,
the model does not describe what the hardware is doing and the answer
should be thrown away.

Two measurement rules, both learned the hard way on this bench:

 1. Magnitudes only. Averaging the complex image/tone ratio across
    blocks looks more direct and is wrong: the component frequencies are
    known only to a few Hz, so each block's phase is rotated further
    than the last and a coherent average over 100 blocks cancels itself.
    It under-read the image by 15 dB and sent the correction the wrong
    way.

 2. Trim the analysis window hard, and put every probe inside ONE
    transmission. The radio leaks its carrier for as long as it is in
    transmit, which begins before the samples do and ends after they
    stop. Blocks that hold leakage but no tone therefore sit just inside
    an envelope-detected window; each one drags the tone average down
    and the carrier ratio up by an amount that depends on where the
    capture happened to start. Measured that way, three identical
    transmissions reported carrier leakage of -19.8, -25.0 and
    -13.5 dBc, which looked like unstable hardware and is not: trimming
    0.25 s inside each edge of the same three captures gives -26.0,
    -26.0 and -28.6 dBc. gen_probe_sweep.py puts the probes in a single
    self-timed waveform so the segments can be trimmed this way, and so
    that all of them share one tuning.

 3. Probe k and d in separate segments. Probing both at once is a
    tempting economy and it corrupts the image measurement: a large DC
    offset lands energy on the image frequency by itself -- a product of
    the offset with the signal falls at 2*f_carrier - f_tone, which is
    the image -- and that energy does not
    respond to k. With d = +-0.06 in the same segments as the k probes,
    the fit's own consistency check sat 2.8 dB out and the apparent
    image floor rose by 2.4e-4 in power -- as much as the impairment
    being measured.

Frequency plan. The receiver is tuned deliberately OFF the transmit
carrier, because the receiver mirrors every component it hears about its
own centre. With the carrier at 445.680 MHz, a +250 kHz tone and the
receiver at 445.755 MHz, the three things being measured land at +175,
-75 and -325 kHz in receive baseband, while the receiver's own mirrors
of them land at -175, +75 and +325 kHz -- so nothing measured sits on a
receiver artifact, nor on the receiver's DC spur, nor on this host's
500 kHz comb.

  calibrate.py [--runs N] [--verify]

--verify sends k=0 against the saved k in one waveform, which is the
only honest way to state an improvement: both in the same transmission.
"""
import json
import os
import subprocess
import sys
import time

import numpy as np

PI = "172.19.100.242"
REMOTE_DIR = "~/cariboulite-txfix/release"

TX_CARRIER = 445.680e6
OFFSET = 250e3
TX_RATE = 2e6
TX_GAIN = 24
AMP = 0.5
SEG_S = 1.0
GAP_S = 0.25
TRIM_S = 0.15                       # dropped inside each segment edge

RX_TUNE = 445.755e6
RX_RATE = 2e6
RX_ARGS = ["-a", "0", "-l", "20", "-g", "16"]
if "--rx" in sys.argv:                 # e.g. --rx "-a 0 -l 12 -g 12"
    RX_ARGS = sys.argv[sys.argv.index("--rx") + 1].split()

# Probe step sizes. These are not free parameters: the affine fit
# estimates a residual by perturbing it, so a step much larger than the
# residual buries it and a step much smaller leaves it in the noise.
# DD = 0.03 suits the ~0.025 baseband offset behind an untrimmed -26 dBc
# carrier. Hold a good TXCI/TXCQ pair instead and the residual drops to
# ~0.0012, 26x smaller, at which point this step returns a d eight times
# too big and the model check says so (+4.6 dB). Override with --dd/--dk.
DK = 0.02
DD = 0.03
if "--dk" in sys.argv:
    DK = float(sys.argv[sys.argv.index("--dk") + 1])
if "--dd" in sys.argv:
    DD = float(sys.argv[sys.argv.index("--dd") + 1])

# Optional AT86RF215 TXCI/TXCQ LO-leakage trim to hold during the
# measurement, as "--txcal I:Q". sweep_txcal.py finds the good pair; the
# modem's own choice is about 19 dB off on this board, and pre-distortion
# measured against the untrimmed leakage does not transfer to the
# trimmed one -- the two cancel the same carrier, so they are
# alternatives, not cumulative.
TXCAL = None
if "--txcal" in sys.argv:
    TXCAL = tuple(int(v)
                  for v in sys.argv[sys.argv.index("--txcal") + 1].split(":"))

TMP = "/home/dmcgrath/.claude/jobs/b1296029/tmp"
OUT = os.path.join(TMP, "iqcal.json")
NBLK = 1 << 16
EBLK = 1 << 12                      # envelope resolution, ~2 ms


def probe_set():
    """Baseline, then four k probes, then four d probes -- never both.

    Returns (probes, k_idx, d_idx): which segments feed which fit. The
    baseline is in both, as the c = 0 point each fit needs.
    """
    probes = [(0, 0)]
    probes += [(DK, 0), (DK * 1j, 0), (-DK, 0), (-DK * 1j, 0)]
    probes += [(0, DD), (0, DD * 1j), (0, -DD), (0, -DD * 1j)]
    return probes, [0, 1, 2, 3, 4], [0, 5, 6, 7, 8]


def transmit_and_capture(probes, tag):
    """Generate the probe waveform on the Pi, send it, capture it here."""
    spec = " ".join("%.9g,%.9g,%.9g,%.9g"
                    % (k.real if isinstance(k, complex) else k,
                       k.imag if isinstance(k, complex) else 0.0,
                       d.real if isinstance(d, complex) else d,
                       d.imag if isinstance(d, complex) else 0.0)
                    for k, d in probes)
    g = subprocess.run(
        ["ssh", "-o", "ConnectTimeout=10", PI,
         "cd %s && python3 -I ~/gen_probe_sweep.py %g %g ~/probe.bin %g %g %g %s"
         % (REMOTE_DIR, TX_RATE, OFFSET, SEG_S, GAP_S, AMP, spec)],
        capture_output=True, text=True, timeout=600)
    if g.returncode != 0:
        sys.exit("generation failed: %s" % (g.stderr or g.stdout))
    print(g.stdout.strip())

    span = GAP_S + len(probes) * (SEG_S + GAP_S)
    cap_s = span + 4.0
    iq = os.path.join(TMP, "probe_%s.iq" % tag)
    cap = subprocess.Popen(
        ["hackrf_transfer", "-r", iq, "-f", str(int(RX_TUNE)),
         "-s", str(int(RX_RATE)), "-n", str(int(RX_RATE * cap_s))] + RX_ARGS,
        stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
    time.sleep(1.5)
    trim = "" if TXCAL is None else " -I %d -Q %d" % TXCAL
    subprocess.run(
        ["ssh", "-o", "ConnectTimeout=10", PI,
         "cd %s && ./tx_test -c 1 -f %d -r %d -g %d%s ~/probe.bin "
         ">/dev/null 2>&1; echo ok"
         % (REMOTE_DIR, int(TX_CARRIER), int(TX_RATE), TX_GAIN, trim)],
        capture_output=True, text=True, timeout=600)
    cap.wait()

    want = int(RX_RATE * cap_s) * 2
    got = os.path.getsize(iq)
    if got < want:
        sys.exit("capture short: %d of %d bytes -- rerun" % (got, want))

    raw = np.fromfile(iq, dtype=np.int8)
    occ = int(max(np.max(np.abs(raw[0::2])), np.max(np.abs(raw[1::2]))))
    x = (raw[0::2].astype(np.float64) + 1j * raw[1::2].astype(np.float64)) / 127.0
    return x, occ


def find_segments(x, n_expected):
    """Locate the probe segments from the envelope, in transmitted order."""
    nb = len(x) // EBLK
    p = np.asarray([np.mean(np.abs(x[i*EBLK:(i+1)*EBLK]) ** 2)
                    for i in range(nb)])
    pdb = 10 * np.log10(p + 1e-20)
    # Anchor the threshold to the segment level, not to the midpoint
    # between floor and peak: the gaps are NOT silent. The radio stays in
    # transmit, so its carrier leakage keeps radiating through them, some
    # 20 dB under the segments but well above the off-air noise floor --
    # which is enough to put a floor-to-peak midpoint below the gaps and
    # merge all five segments into one.
    thr = np.percentile(pdb, 95) - 10.0
    on = pdb > thr

    runs, start = [], None
    for i, v in enumerate(on):
        if v and start is None:
            start = i
        elif not v and start is not None:
            runs.append((start, i))
            start = None
    if start is not None:
        runs.append((start, len(on)))

    need = int(0.6 * SEG_S * RX_RATE / EBLK)
    runs = [r for r in runs if r[1] - r[0] >= need]
    if len(runs) != n_expected:
        sys.exit("found %d segments, expected %d (threshold %.1f dB, "
                 "lengths %s s)"
                 % (len(runs), n_expected, thr,
                    [round((b - a) * EBLK / RX_RATE, 2) for a, b in runs]))

    # Trim each segment hard: drop the ramps, and keep a whole number of
    # analysis blocks well inside the steady part.
    out = []
    for a, b in runs:
        s0 = a * EBLK + int(TRIM_S * RX_RATE)
        s1 = b * EBLK - int(TRIM_S * RX_RATE)
        nblk = (s1 - s0) // NBLK
        if nblk < 8:
            sys.exit("segment too short to measure (%d blocks)" % nblk)
        out.append((s0, nblk))
    return out


def power_at(x, s0, nblk, freqs):
    w = np.hanning(NBLK)
    nrm = np.sum(w)
    n = np.arange(NBLK)
    kern = [w * np.exp(-2j * np.pi * f * n / RX_RATE) for f in freqs]
    acc = np.zeros(len(freqs))
    for i in range(nblk):
        seg = x[s0 + i*NBLK: s0 + (i+1)*NBLK]
        for c, kc in enumerate(kern):
            acc[c] += abs(np.dot(seg, kc) / nrm) ** 2
    return acc / nblk


def refine(x, s0, nblk, f0, span=5e3, step=25.0):
    best, bf = -1.0, f0
    for s in (step, step / 10.0):
        for f in np.arange(bf - span, bf + span + s, s):
            v = power_at(x, s0, min(3, nblk), [f])[0]
            if v > best:
                best, bf = v, f
        span = s
    return bf


def solve(coefs, ys):
    """Least-squares fit of y_i = |c_i + u|^2 for complex u.

    Returns (u, consistency_db): the second value compares the
    independently fitted |u|^2 with |Re u + j Im u|^2. A few tenths of a
    dB means the affine model holds; several dB means it does not.
    """
    A = np.array([[2 * c.real, 2 * c.imag, 1.0] for c in coefs])
    b = np.array([y - abs(c) ** 2 for c, y in zip(coefs, ys)])
    (a, bb, cc), *_ = np.linalg.lstsq(A, b, rcond=None)
    u = complex(a, bb)
    direct = max(cc, 1e-30)
    implied = max(abs(u) ** 2, 1e-30)
    return u, 10 * np.log10(direct / implied)


def dbc(num, den):
    return 10 * np.log10(max(num, 1e-30) / max(den, 1e-30))


def measure(probes, tag):
    x, occ = transmit_and_capture(probes, tag)
    segs = find_segments(x, len(probes))
    ft = refine(x, segs[0][0], segs[0][1], (TX_CARRIER + OFFSET) - RX_TUNE)
    fc = refine(x, segs[0][0], segs[0][1], TX_CARRIER - RX_TUNE)
    delta = ft - fc
    freqs = [ft, ft - delta, ft - 2 * delta]
    print("  ADC %d/127, tone %+.1f kHz, spacing %.1f Hz "
          "(carrier %+.1f, image %+.1f kHz)"
          % (occ, ft / 1e3, delta, (ft - delta) / 1e3, (ft - 2 * delta) / 1e3))
    rows = []
    for (k, d), (s0, nblk) in zip(probes, segs):
        pt, pc, pi_ = power_at(x, s0, nblk, freqs)
        rows.append((complex(k), complex(d), pt, pc, pi_))
    return rows


def report(rows):
    print("  %-26s %-9s %-12s %-12s" % ("probe", "tone", "carrier", "image"))
    for k, d, pt, pc, pi_ in rows:
        print("  k=%+.3f%+.3fj d=%+.3f%+.3fj  %6.2f dBfs %7.2f dBc  %7.2f dBc"
              % (k.real, k.imag, d.real, d.imag,
                 10 * np.log10(pt), dbc(pc, pt), dbc(pi_, pt)))


print("transmit  carrier %.4f MHz, tone %+.1f kHz, -g %d, amp %.2f"
      % (TX_CARRIER / 1e6, OFFSET / 1e3, TX_GAIN, AMP))
print("receive   %.4f MHz, %s" % (RX_TUNE / 1e6, " ".join(RX_ARGS)))
print("txcal     %s" % ("left to the modem" if TXCAL is None else "I=%d Q=%d held" % TXCAL))

if "--verify" in sys.argv:
    with open(OUT) as fh:
        saved = json.load(fh)
    k = complex(saved["kr"], saved["ki"])
    d = complex(saved["dr"], saved["di"])
    print("\nverifying the saved coefficients against zero, both in one")
    print("transmission -- the only honest way to state an improvement.")
    print("k = %+.5f%+.5fj  d = %+.5f%+.5fj"
          % (k.real, k.imag, d.real, d.imag))
    rows = measure([(0, 0), (k, d), (k, 0), (0, d)], "ver")
    report(rows)
    base_c = dbc(rows[0][3], rows[0][2])
    base_i = dbc(rows[0][4], rows[0][2])
    print("\n%-16s %-14s %-14s" % ("segment", "carrier", "image"))
    for lbl, row in zip(("both off", "both on", "k only", "d only"), rows):
        print("%-16s %7.2f dBc   %7.2f dBc  (%+.1f / %+.1f dB)"
              % (lbl, dbc(row[3], row[2]), dbc(row[4], row[2]),
                 dbc(row[3], row[2]) - base_c, dbc(row[4], row[2]) - base_i))
    sys.exit(0)

nruns = 1
if "--runs" in sys.argv:
    nruns = int(sys.argv[sys.argv.index("--runs") + 1])

sols = []
for r in range(nruns):
    print("\n--- run %d of %d" % (r + 1, nruns))
    probes, kidx, didx = probe_set()
    rows = measure(probes, "r%d" % r)
    report(rows)

    ks = [rows[i][0] for i in kidx]
    yi = [rows[i][4] / rows[i][2] for i in kidx]
    ds = [rows[i][1] for i in didx]
    yc = [AMP * AMP * rows[i][3] / rows[i][2] for i in didx]

    m, mchk = solve(ks, yi)
    v, vchk = solve(ds, yc)
    print("  m = %+.5f%+.5fj  |m| = %.5f (%.2f dBc)  model check %+.2f dB"
          % (m.real, m.imag, abs(m), 20 * np.log10(max(abs(m), 1e-12)), mchk))
    print("  v = %+.5f%+.5fj  |v| = %.5f (%.2f dBc)  model check %+.2f dB"
          % (v.real, v.imag, abs(v),
             20 * np.log10(max(abs(v) / AMP, 1e-12)), vchk))
    for nm, step, got, opt in (("k", DK, abs(m), "--dk"),
                               ("d", DD, abs(v), "--dd")):
        if got > 1e-9 and not 0.3 <= step / got <= 3.0:
            print("  probe step for %s is %.1fx the residual it measured "
                  "-- rerun with %s %.4g" % (nm, step / got, opt, got))
    sols.append((m, v, mchk, vchk))

if nruns > 1:
    print("\nacross %d runs -- the spread is the vector distance from the"
          % nruns)
    print("mean, which is what bounds the null a stored coefficient can reach:")
    for name, sol in (("m", [s[0] for s in sols]), ("v", [s[1] for s in sols])):
        a = np.array(sol)
        dev = float(np.max(np.abs(a - a.mean())))
        mag = float(abs(a.mean()))
        print("  %s spread %.5f on |%s| = %.5f -> %.0f%%, so expect no better "
              "than %.0f dB" % (name, dev, name, mag, 100 * dev / mag,
                                20 * np.log10(dev / mag)))

m = np.mean([s[0] for s in sols])
v = np.mean([s[1] for s in sols])
final = {"kr": -m.real, "ki": -m.imag, "dr": -v.real, "di": -v.imag,
         "txcal": None if TXCAL is None else list(TXCAL),
         "freq_hz": TX_CARRIER, "gain": TX_GAIN, "rate_hz": TX_RATE}
with open(OUT, "w") as fh:
    json.dump(final, fh, indent=2, sort_keys=True)
print("\nwrote %s" % OUT)
print("k = %+.5f%+.5fj  d = %+.5f%+.5fj"
      % (final["kr"], final["ki"], final["dr"], final["di"]))

if "--install" in sys.argv:
    r = subprocess.run(["scp", "-q", OUT, "%s:.cariboulite_iqcal.json" % PI],
                       capture_output=True, text=True, timeout=60)
    if r.returncode != 0:
        sys.exit("install failed: %s" % (r.stderr or r.stdout))
    print("installed on %s as ~/.cariboulite_iqcal.json -- the generators "
          "will pick it up" % PI)
else:
    print("pass --install to copy it to the Pi as ~/.cariboulite_iqcal.json")
print("Verify with --verify. Coefficients are per frequency and per gain.")
