#!/usr/bin/env python3
"""Give tx_test access to the AT86RF215's own TX LO-leakage trim.

The modem calibrates its own carrier leakage into RFn_TXCI (0x125) and
RFn_TXCQ (0x126), automatically, on every transition into TX. Each value
is six bits, and the library keeps the median of five readings and
writes it back on every transition (override_cal). Coarse, and nothing
exposes it, so this patch adds:

  -I <n>        set TXCI to n (0..63)
  -Q <n>        set TXCQ to n (0..63)
  -S <list>     sweep: "i:q,i:q,..." applied in turn while streaming
  -D <n>        samples to dwell on each sweep entry

and makes tx_test print the codes it found and the codes it applied.

The sweep exists because one code per transmission is not comparable:
the absolute level wanders between runs. Stepping the register *during*
one transmission, in step with a waveform whose segments are the same
length, puts every code in a single capture and lets the analysis find
the boundaries from the envelope. Note that the switch happens when the
samples have been *pushed*, which is ahead of when they radiate by the
driver's buffer depth, so the waveform wants a gap of a few hundred ms
between segments for the change to land in.

The stored cal.low_ch_* is updated alongside the register, so that a
state transition re-applies the chosen code rather than the median.

  patch_tx_cal.py <path to tx_test.c>
"""
import sys

if len(sys.argv) != 2:
    sys.exit(__doc__)
path = sys.argv[1]
src = open(path).read()

EDITS = []

# ---- 1. include the modem header and a channel macro -------------------
EDITS.append((
    '#include "hat/hat.h"',
    '#include "hat/hat.h"\n'
    '#include "at86rf215/at86rf215_radio.h"\n'
    '\n'
    '/* lifted from cariboulite_radio.c, which does not export it */\n'
    '#define GET_MODEM_CH(rad_ch) ((rad_ch)==cariboulite_channel_s1g ? \\\n'
    '                             at86rf215_rf_channel_900mhz : \\\n'
    '                             at86rf215_rf_channel_2400mhz)\n'
    '\n'
    '#define MAX_CAL_STEPS 64\n'))

# ---- 2. state fields ---------------------------------------------------
EDITS.append((
    "    size_t samples_to_write;\n",
    "    size_t samples_to_write;\n"
    "    int cal_i;\n"
    "    int cal_q;\n"
    "    int cal_steps;\n"
    "    int cal_list_i[MAX_CAL_STEPS];\n"
    "    int cal_list_q[MAX_CAL_STEPS];\n"
    "    size_t cal_dwell;\n"))

EDITS.append((
    "    state.test_pattern = 0;\n",
    "    state.test_pattern = 0;\n"
    "    state.cal_i = -1;\n"
    "    state.cal_q = -1;\n"
    "    state.cal_steps = 0;\n"
    "    state.cal_dwell = 0;\n"))

# ---- 3. a helper that applies a code pair ------------------------------
EDITS.append((
    "//=================================================\nstatic int stop_program (void)",
    "//=================================================\n"
    "// Apply a TXCI/TXCQ pair, and store it where override_cal will find\n"
    "// it so a later state transition does not put the median back.\n"
    "static void apply_tx_cal(int cal_i, int cal_q)\n"
    "{\n"
    "    at86rf215_st *modem = &state.radio->sys->modem;\n"
    "    at86rf215_rf_channel_en ch = GET_MODEM_CH(state.radio->type);\n"
    "    if (ch == at86rf215_rf_channel_900mhz)\n"
    "    {\n"
    "        modem->cal.low_ch_i = cal_i & 0x3F;\n"
    "        modem->cal.low_ch_q = cal_q & 0x3F;\n"
    "    }\n"
    "    else\n"
    "    {\n"
    "        modem->cal.hi_ch_i = cal_i & 0x3F;\n"
    "        modem->cal.hi_ch_q = cal_q & 0x3F;\n"
    "    }\n"
    "    at86rf215_radio_set_tx_iq_calibration(modem, ch, cal_i & 0x3F,\n"
    "                                          cal_q & 0x3F);\n"
    "}\n"
    "\n"
    "//=================================================\nstatic int stop_program (void)"))

# ---- 4. option parsing -------------------------------------------------
EDITS.append((
    'while ((opt = getopt(argc, argv, "c:f:t:p:g:n:r:")) != -1) {',
    'while ((opt = getopt(argc, argv, "c:f:t:p:g:n:r:I:Q:S:D:")) != -1) {'))

EDITS.append((
    "\t\tswitch (opt) {\n",
    "\t\tswitch (opt) {\n"
    "\t\tcase 'I': state.cal_i = atoi(optarg); break;\n"
    "\t\tcase 'Q': state.cal_q = atoi(optarg); break;\n"
    "\t\tcase 'D': state.cal_dwell = (size_t)atoll(optarg); break;\n"
    "\t\tcase 'S': {\n"
    "\t\t\tchar *s = optarg;\n"
    "\t\t\tstate.cal_steps = 0;\n"
    "\t\t\twhile (s && *s && state.cal_steps < MAX_CAL_STEPS)\n"
    "\t\t\t{\n"
    "\t\t\t\tint ci = 0, cq = 0;\n"
    "\t\t\t\tif (sscanf(s, \"%d:%d\", &ci, &cq) != 2)\n"
    "\t\t\t\t{\n"
    "\t\t\t\t\tfprintf(stderr, \"bad -S entry near '%s'\\n\", s);\n"
    "\t\t\t\t\treturn -1;\n"
    "\t\t\t\t}\n"
    "\t\t\t\tstate.cal_list_i[state.cal_steps] = ci;\n"
    "\t\t\t\tstate.cal_list_q[state.cal_steps] = cq;\n"
    "\t\t\t\tstate.cal_steps++;\n"
    "\t\t\t\ts = strchr(s, ',');\n"
    "\t\t\t\tif (s) s++;\n"
    "\t\t\t}\n"
    "\t\t\tbreak;\n"
    "\t\t}\n"))

# ---- 5. report and apply after the channel is live ---------------------
EDITS.append((
    "    cariboulite_radio_activate_channel(state.radio, cariboulite_channel_dir_tx, true);\n\n"
    "    // Open the input file for reading",
    "    cariboulite_radio_activate_channel(state.radio, cariboulite_channel_dir_tx, true);\n"
    "\n"
    "    // Report the trim the modem calibrated for itself, then override it\n"
    "    // if asked. Printed on stderr so it cannot land in a piped waveform.\n"
    "    {\n"
    "        int got_i = -1, got_q = -1;\n"
    "        at86rf215_radio_get_tx_iq_calibration(&state.radio->sys->modem,\n"
    "                                              GET_MODEM_CH(state.radio->type),\n"
    "                                              &got_i, &got_q);\n"
    "        fprintf(stderr, \"txcal: modem chose I=%d Q=%d\\n\", got_i, got_q);\n"
    "        if (state.cal_steps > 0)\n"
    "        {\n"
    "            apply_tx_cal(state.cal_list_i[0], state.cal_list_q[0]);\n"
    "            fprintf(stderr, \"txcal: sweep step 0 at sample 0 -> I=%d Q=%d\\n\",\n"
    "                    state.cal_list_i[0], state.cal_list_q[0]);\n"
    "        }\n"
    "        else if (state.cal_i >= 0 || state.cal_q >= 0)\n"
    "        {\n"
    "            int use_i = state.cal_i >= 0 ? state.cal_i : got_i;\n"
    "            int use_q = state.cal_q >= 0 ? state.cal_q : got_q;\n"
    "            apply_tx_cal(use_i, use_q);\n"
    "            fprintf(stderr, \"txcal: applied I=%d Q=%d\\n\", use_i, use_q);\n"
    "        }\n"
    "    }\n"
    "\n"
    "    // Open the input file for reading"))

# ---- 6. step the code inside the streaming loop -----------------------
EDITS.append((
    "    usleep(100000);\n    unsigned int it = 0;",
    "    usleep(100000);\n"
    "    unsigned int it = 0;\n"
    "    size_t pushed = 0;\n"
    "    int cal_step = 0;"))

EDITS.append((
    "        ret = cariboulite_radio_write_samples(state.radio, state.buffer, nsamples);\n",
    "        ret = cariboulite_radio_write_samples(state.radio, state.buffer, nsamples);\n"
    "\n"
    "        // Step the trim in time with the waveform's own segments. The\n"
    "        // count is of samples pushed, which leads what is radiated by\n"
    "        // the driver's buffer depth -- hence the gaps in the stimulus.\n"
    "        if (ret > 0 && state.cal_steps > 1 && state.cal_dwell > 0)\n"
    "        {\n"
    "            pushed += (size_t)ret;\n"
    "            int want = (int)(pushed / state.cal_dwell);\n"
    "            if (want > cal_step && want < state.cal_steps)\n"
    "            {\n"
    "                cal_step = want;\n"
    "                apply_tx_cal(state.cal_list_i[cal_step],\n"
    "                             state.cal_list_q[cal_step]);\n"
    "                fprintf(stderr, \"txcal: sweep step %d at sample %zu \"\n"
    "                        \"-> I=%d Q=%d\\n\", cal_step, pushed,\n"
    "                        state.cal_list_i[cal_step],\n"
    "                        state.cal_list_q[cal_step]);\n"
    "            }\n"
    "        }\n"))

# ---- 7. usage ---------------------------------------------------------
EDITS.append((
    '\t\t"\\t[-p ppm_error (default: 0)]\\n"\n',
    '\t\t"\\t[-p ppm_error (default: 0)]\\n"\n'
    '\t\t"\\t[-I TXCI, override the modem\'s own LO-leakage trim (0..63)]\\n"\n'
    '\t\t"\\t[-Q TXCQ, the same for the Q path]\\n"\n'
    '\t\t"\\t[-S i:q,i:q,... step the trim through these codes while streaming]\\n"\n'
    '\t\t"\\t[-D samples to dwell on each -S code]\\n"\n'))

for old, new in EDITS:
    n = src.count(old)
    if n != 1:
        sys.exit("anchor matched %d times, expected 1:\n---\n%s\n---" % (n, old[:200]))
    src = src.replace(old, new)

open(path, "w").write(src)
print("patched %s: -I/-Q/-S/-D added, and the modem's own trim is reported" % path)
