#!/usr/bin/env python3
"""Search the AT86RF215's own TXCI/TXCQ LO-leakage trim codes.

Runs on the HOST with a HackRF watching the CaribouLite. Needs
patch_tx_cal.py applied to tx_test.c, which exposes the trim as -I/-Q
and adds -S/-D to step it while streaming.

Why step it mid-transmission. The trim is six bits per path and the
modem picks its own value automatically on every transition into TX --
from a median of five readings, which scatter. One code per run is
therefore not comparable with the next, both because the absolute level
wanders and because the thing being improved on is itself re-chosen each
run. So every code goes in a single transmission: tx_test switches the
register every -D samples, the stimulus is a segmented tone with the
same period, and the analysis finds the boundaries from the envelope and
maps segment i to code i.

The alignment that mapping depends on is not free. tx_test switches when
the samples have been *pushed* to the driver, which runs ahead of what is
radiating by the buffer depth B, so a write at segment i's period
boundary takes effect while file position (i*period - B) is on the air.
For it to land in the gap before segment i rather than inside segment
i-1, the stimulus needs gap >= lead + B -- hence the short --lead given
to gen_probe_sweep.py. Run the `align` mode first, which alternates two
codes 21 dB apart: the measured carrier must alternate in step, starting
with the first code.

  sweep_txcal.py align                      check the mapping
  sweep_txcal.py i <lo> <hi> <step> <q>     sweep TXCI at fixed TXCQ
  sweep_txcal.py q <lo> <hi> <step> <i>     sweep TXCQ at fixed TXCI
  sweep_txcal.py grid <il> <ih> <ql> <qh>   both axes at once
  sweep_txcal.py list i:q,i:q,...           explicit codes

and two options:

  --rx "<hackrf args>"      receive chain, to repeat a result at another gain
  --cal kr:ki:dr:di         hold a digital pre-distortion in every segment

The two axes are close to independent, so alternating single-axis sweeps
converge in two passes. Sweep the axis you expect to be worse first:
whichever residual is larger sets the floor of the other axis's curve
and flattens it, which is how a 5 dB-deep I curve with a minimum in the
right place sat on top of a Q error worth 18 dB.

--cal is what settles whether the hardware trim and the digital
correction of calibrate.py can be stacked. They cannot: the digital d is
an absolute offset in units of full scale, so on its own it radiates a
carrier at 20*log10(|d|/amp), and no trim code was found that cancels
it. Use --cal to re-derive that on your own board rather than taking it
on trust -- it is one transmission.
"""
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 = 0.6
GAP_S = 0.4
LEAD_S = 0.05
TRIM_S = 0.15

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

CAL = (0.0, 0.0, 0.0, 0.0)
if "--cal" in sys.argv:
    CAL = tuple(float(v)
                for v in sys.argv[sys.argv.index("--cal") + 1].split(":"))
    if len(CAL) != 4:
        sys.exit("--cal takes kr:ki:dr:di")

NBLK = 1 << 16
EBLK = 1 << 12
DELTA = 250000.0
TMP = os.environ.get("TXCAL_TMP", "/tmp")


def parse_args():
    a = [v for v in sys.argv[1:] if not v.startswith("--")]
    for opt in ("--rx", "--cal"):
        if opt in sys.argv:
            a = [v for v in a if v != sys.argv[sys.argv.index(opt) + 1]]
    if not a:
        sys.exit(__doc__)
    if a[0] == "align":
        return [(35, 29), (0, 0)] * 4, "align"
    if a[0] == "list":
        return [tuple(int(v) for v in t.split(":")) for t in a[1].split(",")], \
            "list"
    if a[0] == "grid":
        il, ih, ql, qh = (int(v) for v in a[1:5])
        return [(i, q) for i in range(il, ih + 1)
                for q in range(ql, qh + 1)], "grid"
    if a[0] in ("i", "q"):
        lo, hi, step, fixed = (int(v) for v in a[1:5])
        vals = list(range(lo, hi + 1, step))
        if a[0] == "i":
            return [(v, fixed) for v in vals], "i"
        return [(fixed, v) for v in vals], "q"
    sys.exit(__doc__)


def transmit_and_capture(codes, tag):
    period = int((SEG_S + GAP_S) * TX_RATE)
    probe = "%.9g,%.9g,%.9g,%.9g" % CAL
    g = subprocess.run(
        ["ssh", "-o", "ConnectTimeout=10", PI,
         "cd %s && python3 -I ~/gen_probe_sweep.py %g %g ~/probe.bin %g %g %g "
         "%s --lead %g" % (REMOTE_DIR, TX_RATE, OFFSET, SEG_S, GAP_S, AMP,
                           " ".join([probe] * len(codes)), LEAD_S)],
        capture_output=True, text=True, timeout=900)
    if g.returncode != 0:
        sys.exit("generation failed: %s" % (g.stderr or g.stdout))
    print("  " + g.stdout.strip().splitlines()[0])

    cap_s = LEAD_S + len(codes) * (SEG_S + GAP_S) + 4.0
    iq = os.path.join(TMP, "txcal_%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)
    slist = ",".join("%d:%d" % c for c in codes)
    tx = subprocess.run(
        ["ssh", "-o", "ConnectTimeout=10", PI,
         "cd %s && ./tx_test -c 1 -f %d -r %d -g %d -S %s -D %d ~/probe.bin "
         "2>&1 | grep txcal" % (REMOTE_DIR, int(TX_CARRIER), int(TX_RATE),
                                TX_GAIN, slist, period)],
        capture_output=True, text=True, timeout=1800)
    cap.wait()

    lines = tx.stdout.splitlines()
    nsteps = len([l for l in lines if "sweep step" in l])
    chose = [l.split("modem chose ")[1] for l in lines if "modem chose" in l]
    if nsteps != len(codes):
        sys.exit("tx_test stepped %d of %d codes -- the dwell and the "
                 "waveform period disagree" % (nsteps, len(codes)))

    want = int(RX_RATE * cap_s) * 2
    if os.path.getsize(iq) < want:
        sys.exit("capture short -- rerun")
    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, (chose[0] if chose else "unknown")


def find_segments(x, n_expected):
    """Locate the segments by correlating the envelope with the known pattern.

    Thresholding the envelope is the obvious approach and it breaks: the
    gaps are not silent (the carrier leaks through them), and the output
    level of this board can droop by 19 dB over a few seconds, which puts
    later segments below any single threshold and silently returns too few
    runs. The *timing* is fixed by the file, so correlating a mean-removed
    on/off template against the envelope in dB finds the start regardless
    of level, and the segment count can never come out wrong.
    """
    nb = len(x) // EBLK
    pdb = 10 * np.log10(np.asarray(
        [np.mean(np.abs(x[i*EBLK:(i+1)*EBLK]) ** 2)
         for i in range(nb)]) + 1e-20)
    bseg = int(SEG_S * RX_RATE / EBLK)
    bper = bseg + int(GAP_S * RX_RATE / EBLK)
    tmpl = np.zeros(n_expected * bper)
    for i in range(n_expected):
        tmpl[i * bper: i * bper + bseg] = 1.0
    tmpl -= tmpl.mean()
    if len(tmpl) >= nb:
        sys.exit("capture shorter than the expected pattern")
    best_off, best_score = 0, -np.inf
    for off in range(0, nb - len(tmpl)):
        s = float(np.dot(pdb[off:off + len(tmpl)], tmpl))
        if s > best_score:
            best_score, best_off = s, off
    print("  pattern locked at %.3f s" % (best_off * EBLK / RX_RATE))
    out = []
    nblk = (int(SEG_S * RX_RATE) - 2 * int(TRIM_S * RX_RATE)) // NBLK
    if nblk < 2:
        sys.exit("segment too short to measure (%d blocks)" % nblk)
    for i in range(n_expected):
        out.append(((best_off + i * bper) * EBLK + int(TRIM_S * RX_RATE),
                    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


codes, mode = parse_args()
print("TXCI/TXCQ sweep: %d codes, %.2f s segments, %.2f s gaps, %.2f s lead"
      % (len(codes), SEG_S, GAP_S, LEAD_S))
print("transmit %.4f MHz, -g %d; receive %.4f MHz, %s"
      % (TX_CARRIER / 1e6, TX_GAIN, RX_TUNE / 1e6, " ".join(RX_ARGS)))
if any(CAL):
    print("digital    k=%+.5f%+.5fj d=%+.5f%+.5fj held in every segment"
          % CAL)

x, occ, chose = transmit_and_capture(codes, mode)
segs = find_segments(x, len(codes))
ft = refine(x, segs[0][0], segs[0][1], (TX_CARRIER + OFFSET) - RX_TUNE)
# The fourth frequency is empty spectrum, 125 kHz clear of the tone, the
# carrier and the image. It is the measurement floor for this segment, and
# a null that lands within a few dB of it has not been measured at all --
# which is the difference between a -57 dBc carrier and a weak segment.
freqs = [ft, ft - DELTA, ft - 2 * DELTA, ft - 1.5 * DELTA]
print("  ADC %d/127, tone %+.1f kHz, modem chose %s\n" % (occ, ft / 1e3, chose))

print("%-4s %-11s %-12s %-12s %-12s %-10s %s"
      % ("seg", "code", "carrier", "image", "worst", "tone", "floor"))
rows = []
for (ci, cq), (s0, nblk) in zip(codes, segs):
    pt, pc, pi_, pf = power_at(x, s0, nblk, freqs)
    c, im = 10 * np.log10(pc / pt), 10 * np.log10(pi_ / pt)
    fl = 10 * np.log10(pf / pt)
    rows.append((ci, cq, c, im, fl))
    print("%-4d I=%2d Q=%2d   %7.2f dBc  %7.2f dBc  %7.2f dBc  %6.2f dBfs  "
          "%7.2f dBc%s"
          % (len(rows) - 1, ci, cq, c, im, max(c, im), 10 * np.log10(pt), fl,
             "  <- carrier is in the floor" if c - fl < 3.0 else ""))

bc = min(rows, key=lambda r: r[2])
bw = min(rows, key=lambda r: max(r[2], r[3]))
print("\nlowest carrier : I=%d Q=%d at %.2f dBc (image %.2f) -- modem chose %s"
      % (bc[0], bc[1], bc[2], bc[3], chose))
print("lowest worst   : I=%d Q=%d at %.2f dBc (carrier %.2f, image %.2f)"
      % (bw[0], bw[1], max(bw[2], bw[3]), bw[2], bw[3]))
img = [r[3] for r in rows]
print("image across the sweep: %.2f to %.2f dBc -- this trim is a DC control "
      "only" % (min(img), max(img)))
