#!/usr/bin/env python3
"""
voxl-calib-commit-chains -- make a committed calibration permanent.

Overlays the calibrator's committed snapshot (ov_calib_result.yaml) into the
active kalibr_imu_chain.yaml and kalibr_imucam_chain.yaml, then turns the
hot-start machinery OFF (calibrate_first, en_calib_hotstart,
calib_hotstart_on_reset -> false in the server conf): once the chains carry
the calibrated values directly, boot-time overlays are redundant.

WHAT MOVES (committed blocks only; everything else in the files is preserved,
comments included -- edits are textual value replacements):

  kalibr_imucam_chain.yaml (per camera)
    q_ItoC@i / p_IinC@i  -> T_cam_imu (the FED-frame -> camera transform,
                            written verbatim from the snapshot; a partial
                            commit composes with the chain's existing part)
    td@i                 -> timeshift_cam_imu (center-stamp convention,
                            consumed verbatim -- no exposure arithmetic)
    cam@i                -> intrinsics + distortion_coeffs
  kalibr_imu_chain.yaml (requires the full {dw, da, q_AtoI} gauge triplet)
    Tw = Dw_model^-1, Ta = Da_model^-1, R_IMUtoGYRO, R_IMUtoACC (+ Tg when
    committed), ported from the calibrator's gyro-aligned imu2 gauge into the
    chain's declared model ("kalibr"/"calibrated" or "rpng") and verified by
    a round trip through the estimator's parse semantics (Tw inverted,
    rotations transposed) before anything is written. Noise densities are
    NEVER touched (the filter keeps its inflated values by design).

FRAME SAFETY (sync_config with body corrections must not trip this):
  * The chain's T_cam_imu lives in the FED IMU frame -- the body frame when
    imu_body_frame_mode is on. The snapshot's imu_frame provenance is gated
    against the conf's mode; a mismatch refuses, a legacy snapshot warns.
  * With sync_config enabled, boot REGENERATES T_cam_imu from extrinsics.conf
    (with the body composition) and intrinsics from the camera-server cal.
    This tool then recomputes what that sync would produce and cross-checks
    it against the calibrated values: if extrinsics.conf does not carry the
    calibration yet (run voxl-calib-to-extrinsics), it says so loudly --
    otherwise the next boot would silently restore the nominals.
    timeshift_cam_imu and the whole IMU chain are sync-untouched and
    therefore durable either way.

By DEFAULT the tool also sets sync_config to false in the server conf
(inserting the key when absent -- an absent key means TRUE at boot): a boot
that regenerates the chain would defeat a definitive commit, so the chain is
frozen and the written values are the values, full stop. --keep-sync-config
leaves it untouched; the tool then cross-checks extrinsics.conf and warns
where a boot regeneration would diverge from the calibrated values.

Backups (<file>.bak) are written next to every modified file. --dry-run
reports everything and writes nothing.
"""
import argparse
import json
import math
import os
import re
import shutil
import subprocess
import sys

D2R = math.pi / 180.0
R2D = 180.0 / math.pi


# ----------------------------------------------------------------------------- linear algebra
def mm(a, b):
    return [[sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3)] for i in range(3)]


def mv(a, v):
    return [sum(a[i][k] * v[k] for k in range(3)) for i in range(3)]


def tp(a):
    return [[a[j][i] for j in range(3)] for i in range(3)]


def eye():
    return [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]


def madd(a, b, s=1.0):
    return [[a[i][j] + s * b[i][j] for j in range(3)] for i in range(3)]


def maxdev(a, b):
    return max(abs(a[i][j] - b[i][j]) for i in range(3) for j in range(3))


def det3(m):
    return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
            - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
            + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]))


def inv3(m):
    d = det3(m)
    if abs(d) < 1e-12:
        raise ValueError("singular 3x3")
    c = [[(m[(i + 1) % 3][(j + 1) % 3] * m[(i + 2) % 3][(j + 2) % 3]
           - m[(i + 1) % 3][(j + 2) % 3] * m[(i + 2) % 3][(j + 1) % 3]) / d
          for i in range(3)] for j in range(3)]
    return c


def rot_angle_deg(a, b):
    r = mm(tp(a), b)
    c = max(-1.0, min(1.0, (r[0][0] + r[1][1] + r[2][2] - 1.0) / 2.0))
    return math.acos(c) * R2D


def qr_positive_diag(A):
    """Gram-Schmidt QR with a positive-diagonal canonicalization: A = Q * U.
    The inputs here are near-identity intrinsic products -- conditioning is
    excellent and the canonical factorization is unique."""
    cols = [[A[r][c] for r in range(3)] for c in range(3)]
    q = []
    U = [[0.0] * 3 for _ in range(3)]
    for c in range(3):
        v = cols[c][:]
        for p in range(c):
            U[p][c] = sum(q[p][r] * cols[c][r] for r in range(3))
            v = [v[r] - U[p][c] * q[p][r] for r in range(3)]
        n = math.sqrt(sum(x * x for x in v))
        if n < 1e-12:
            raise ValueError("rank-deficient QR input")
        U[c][c] = n
        q.append([x / n for x in v])
    Q = [[q[c][r] for c in range(3)] for r in range(3)]
    for k in range(3):
        if U[k][k] < 0.0:
            for j in range(3):
                U[k][j] = -U[k][j]
            for i in range(3):
                Q[i][k] = -Q[i][k]
    return Q, U


def ql_positive_diag(A):
    """A = Q * L with L lower-triangular, diag(L) > 0 (antidiagonal-flip QR)."""
    J = [[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0]]
    Qf, Uf = qr_positive_diag(mm(J, mm(A, J)))
    Q = mm(J, mm(Qf, J))
    L = mm(J, mm(Uf, J))
    for k in range(3):
        if L[k][k] < 0.0:
            for j in range(3):
                L[k][j] = -L[k][j]
            for i in range(3):
                Q[i][k] = -Q[i][k]
    return Q, L


# ----------------------------------------------------------------------------- gauge port
def imu2_to_chain(Dw_r, Da_r, R_AtoI, Tg_r, model):
    """
    Calibrator imu2 gauge (gyro-aligned: Dw/Da upper-tri INVERSE intrinsics,
    R_AtoI free) -> the chain model's corrected-signal quantities:
      kalibr: body = ACCEL frame; two sign-canonical QLs re-pin the gauge
      rpng:   body = GYRO frame; the identity case
    Returns (Dw_m, Da_m, R_GYROtoIMU, R_ACCtoIMU, Tg_m) -- the quantities the
    ESTIMATOR consumes; the FILE stores Tw=Dw_m^-1, Ta=Da_m^-1 and the
    TRANSPOSED rotations (R_IMUtoGYRO / R_IMUtoACC).
    """
    if model == "kalibr":
        Qa, La = ql_positive_diag(mm(R_AtoI, Da_r))
        Qg, Lg = ql_positive_diag(mm(tp(Qa), Dw_r))
        return Lg, La, Qg, eye(), mm(Tg_r, Qa)
    return Dw_r, Da_r, eye(), R_AtoI, Tg_r


def chain_to_imu2(Dw_chain, Da_chain, R_GYROtoIMU, R_ACCtoIMU, Tg_chain):
    """The estimator-side direction (QR path) -- the round-trip oracle."""
    Qw, Uw = qr_positive_diag(mm(R_GYROtoIMU, Dw_chain))
    Qa, Ua = qr_positive_diag(mm(tp(Qw), mm(R_ACCtoIMU, Da_chain)))
    return Uw, Ua, Qa, mm(Tg_chain, Qw)


# ----------------------------------------------------------------------------- file parsing
def parse_result_yaml(path):
    out = {}
    with open(path) as f:
        for line in f:
            line = line.split("#", 1)[0].rstrip()
            m = re.match(r"^([A-Za-z0-9_]+):\s*(.*)$", line)
            if not m:
                continue
            key, val = m.group(1), m.group(2).strip()
            if not val:
                continue
            def unq(s):
                return s[1:-1] if len(s) >= 2 and s[0] == s[-1] and s[0] in "\"'" else s
            if val.startswith("["):
                items = [unq(x.strip()) for x in val.strip("[]").split(",") if x.strip()]
                try:
                    out[key] = [float(x) for x in items]
                except ValueError:
                    out[key] = items
            else:
                val = unq(val)
                try:
                    out[key] = float(val)
                except ValueError:
                    out[key] = val
    return out


def load_json_conf(path):
    raw = open(path).read()
    s = re.sub(r"/\*.*?\*/", "", raw, flags=re.S)
    s = re.sub(r"//[^\n]*", "", s)
    return raw, json.loads(s)


def mat_from_flat(flat):
    return [flat[0:3], flat[3:6], flat[6:9]]


# ------- OpenCV-YAML section editing (chains): textual, comment-preserving ---
def section_span(text, section):
    m = re.search(r"(?m)^" + re.escape(section) + r":\s*$", text)
    if not m:
        return None
    start = m.end()
    n = re.search(r"(?m)^\S", text[start:])
    end = start + n.start() if n else len(text)
    return (m.start(), start, end)


def get_matrix(text, section, key):
    span = section_span(text, section)
    if not span:
        return None
    body = text[span[1]:span[2]]
    m = re.search(r"(?m)^  " + re.escape(key) + r":\s*\n((?:^    - \[[^\]]*\]\s*\n)+)", body)
    if not m:
        return None
    rows = []
    for rm in re.finditer(r"^    - \[([^\]]*)\]", m.group(1), re.M):
        rows.append([float(x) for x in rm.group(1).split(",")])
    return rows


def get_scalar(text, section, key):
    span = section_span(text, section)
    if not span:
        return None
    body = text[span[1]:span[2]]
    m = re.search(r"(?m)^  " + re.escape(key) + r":\s*([^\s#]+)", body)
    return float(m.group(1)) if m else None


def get_vec(text, section, key):
    span = section_span(text, section)
    if not span:
        return None
    body = text[span[1]:span[2]]
    m = re.search(r"(?m)^  " + re.escape(key) + r":\s*\[([^\]]*)\]", body)
    if not m:
        return None
    return [float(x) for x in m.group(1).split(",")]


def fmt_num(v):
    if v == int(v) and abs(v) < 1e15:
        return str(int(v))
    return f"{v:.17g}"


def set_matrix(text, section, key, rows):
    span = section_span(text, section)
    if not span:
        return None
    body = text[span[1]:span[2]]
    block = "".join("    - [" + ", ".join(fmt_num(x) for x in r) + "]\n" for r in rows)
    pat = re.compile(r"(?m)(^  " + re.escape(key) + r":\s*\n)((?:^    - \[[^\]]*\]\s*\n)+)")
    m = pat.search(body)
    if m:
        body2 = body[:m.start(2)] + block + body[m.end(2):]
    else:
        body2 = body.rstrip("\n") + "\n  " + key + ":\n" + block
    return text[:span[1]] + body2 + text[span[2]:]


def set_scalar(text, section, key, val):
    span = section_span(text, section)
    if not span:
        return None
    body = text[span[1]:span[2]]
    pat = re.compile(r"(?m)^(  " + re.escape(key) + r":\s*)([^\s#]+)([^\n]*)$")
    m = pat.search(body)
    rep = fmt_num(val)
    if m:
        body2 = body[:m.start(2)] + rep + body[m.end(2):]
    else:
        body2 = body.rstrip("\n") + "\n  " + key + ": " + rep + "\n"
    return text[:span[1]] + body2 + text[span[2]:]


def set_vec(text, section, key, vals):
    span = section_span(text, section)
    if not span:
        return None
    body = text[span[1]:span[2]]
    rep = "[" + ", ".join(fmt_num(x) for x in vals) + "]"
    pat = re.compile(r"(?m)^(  " + re.escape(key) + r":\s*)\[[^\]]*\](.*)$")
    m = pat.search(body)
    if m:
        body2 = body[:m.start(0)] + m.group(1) + rep + m.group(2) + "\n" + body[m.end(0) + 1:]
    else:
        body2 = body.rstrip("\n") + "\n  " + key + ": " + rep + "\n"
    return text[:span[1]] + body2 + text[span[2]:]


def insert_json_key(text, key, val):
    """Insert "key": val before the root object's closing brace, matching the
    file's indent and separator style. Comments and layout are untouched."""
    i = text.rfind("}")
    if i < 0:
        raise ValueError("no closing brace")
    head, tail = text[:i], text[i:]
    m = re.search(r"(?m)^(\s+)\"", text)
    indent = m.group(1) if m else "\t"
    sep = ":\t" if '":\t' in text else ": "
    k = len(head.rstrip())
    comma = "" if head.rstrip().endswith("{") else ","
    return head[:k] + comma + "\n" + indent + '"' + key + '"' + sep + val + "\n" + tail


# ------- vcc composition (what a sync_config boot would regenerate) ----------
def rotx(d):
    c, s = math.cos(d * D2R), math.sin(d * D2R)
    return [[1, 0, 0], [0, c, -s], [0, s, c]]


def roty(d):
    c, s = math.cos(d * D2R), math.sin(d * D2R)
    return [[c, 0, s], [0, 1, 0], [-s, 0, c]]


def rotz(d):
    c, s = math.cos(d * D2R), math.sin(d * D2R)
    return [[c, -s, 0], [s, c, 0], [0, 0, 1]]


def rpy_to_m(rpy):
    return mm(rotx(rpy[0]), mm(roty(rpy[1]), rotz(rpy[2])))


def sync_would_regenerate(ext_conf, cam_child, imu_child, imu_body):
    """
    Mirror sync_cam_config's T_cam_imu build: vcc gives R_child_to_parent (the
    intrinsic-XYZ matrix) and T_child_wrt_parent; T_cam_imu = [R^T | -R^T t];
    under imu_body_frame_mode it composes with T_imu_body from the
    (body, imu_child) entry. Returns (R, t) of the fed-frame->cam transform or
    None when the entries are missing.
    """
    ents = {(e["parent"], e["child"]): e for e in ext_conf["extrinsics"]}
    cam_e = None
    for (p, c), e in ents.items():
        if c == cam_child:
            cam_e = e
            break
    if cam_e is None:
        return None
    R_cp = rpy_to_m(cam_e["RPY_parent_to_child"])
    t_pc = list(map(float, cam_e["T_child_wrt_parent"]))
    R = tp(R_cp)
    t = [-x for x in mv(tp(R_cp), t_pc)]
    if imu_body:
        body_e = ents.get(("body", imu_child))
        if body_e is None:
            return None
        R_ib = rpy_to_m(body_e["RPY_parent_to_child"])       # imu->body
        t_ib = list(map(float, body_e["T_child_wrt_parent"]))  # imu origin in body
        # T_imu_body (body->imu) = [R_ib^T | -R_ib^T t_ib]; T_final = T_cam_imu * T_imu_body
        Rb = tp(R_ib)
        tb = [-x for x in mv(tp(R_ib), t_ib)]
        t = [mv(R, tb)[k] + t[k] for k in range(3)]
        R = mm(R, Rb)
    return R, t


# ----------------------------------------------------------------------------- selftest
def selftest():
    ok = True

    def check(name, cond):
        nonlocal ok
        print(("  [ok] " if cond else "  [FAIL] ") + name)
        ok = ok and cond

    # a plausible imu2 set: upper-tri inverse intrinsics near identity + small misalignment
    Dw_r = [[1.0021, 0.0012, -0.0034], [0.0, 0.9987, 0.0021], [0.0, 0.0, 1.0044]]
    Da_r = [[0.9979, -0.0008, 0.0015], [0.0, 1.0031, -0.0027], [0.0, 0.0, 0.9991]]
    R_AtoI = rpy_to_m([0.31, -0.22, 0.4])
    Tg_r = [[1.7e-4, -7.1e-5, 2.8e-5], [1.2e-4, 2.4e-6, 1.3e-4], [-7.3e-5, -3.8e-4, 9.9e-5]]

    for model in ("kalibr", "rpng"):
        Dw_m, Da_m, Rg, Ra, Tg_m = imu2_to_chain(Dw_r, Da_r, R_AtoI, Tg_r, model)
        # structural shape of the model gauge
        if model == "kalibr":
            check("kalibr Dw lower-tri", abs(Dw_m[0][1]) < 1e-12 and abs(Dw_m[0][2]) < 1e-12 and abs(Dw_m[1][2]) < 1e-12)
            check("kalibr R_ACCtoIMU = I", maxdev(Ra, eye()) < 1e-12)
        else:
            check("rpng Dw upper-tri", abs(Dw_m[1][0]) < 1e-12 and abs(Dw_m[2][0]) < 1e-12 and abs(Dw_m[2][1]) < 1e-12)
            check("rpng R_GYROtoIMU = I", maxdev(Rg, eye()) < 1e-12)
        # file storage semantics: Tw = Dw^-1, rotations transposed; then the
        # estimator-parse simulation and the QR path must reproduce imu2
        Tw, Ta = inv3(Dw_m), inv3(Da_m)
        R_IMUtoGYRO, R_IMUtoACC = tp(Rg), tp(Ra)
        Dw_b, Da_b, Ra_b, Tg_b = chain_to_imu2(inv3(Tw), inv3(Ta), tp(R_IMUtoGYRO), tp(R_IMUtoACC), Tg_m)
        dev = max(maxdev(Dw_b, Dw_r), maxdev(Da_b, Da_r), maxdev(Ra_b, R_AtoI), maxdev(Tg_b, Tg_r))
        check(f"{model} gauge round-trip through file semantics (dev {dev:.1e})", dev < 1e-9)

    # textual chain editors on a synthetic snippet
    snip = ("%YAML:1.0\n\ncam0:\n  T_cam_imu:\n    - [1, 0, 0, 0]\n    - [0, 1, 0, 0]\n"
            "    - [0, 0, 1, 0]\n    - [0, 0, 0, 1]\n  timeshift_cam_imu: 0.001 # note\n"
            "  intrinsics: [400, 400, 320, 240]\ncam1:\n  timeshift_cam_imu: 0.0\n")
    t2 = set_matrix(snip, "cam0", "T_cam_imu", [[0, 1, 0, 0.1], [1, 0, 0, 0.2], [0, 0, 1, 0.3], [0, 0, 0, 1]])
    t2 = set_scalar(t2, "cam0", "timeshift_cam_imu", 0.00371)
    t2 = set_vec(t2, "cam0", "intrinsics", [452.5, 452.6, 640.1, 400.2])
    check("matrix replaced", get_matrix(t2, "cam0", "T_cam_imu")[0][3] == 0.1)
    check("scalar replaced, comment kept", get_scalar(t2, "cam0", "timeshift_cam_imu") == 0.00371 and "# note" in t2)
    check("vec replaced", get_vec(t2, "cam0", "intrinsics")[0] == 452.5)
    check("cam1 untouched", get_scalar(t2, "cam1", "timeshift_cam_imu") == 0.0)
    # sync recomposition mirrors the server (fed frame = body)
    conf = {"extrinsics": [
        {"parent": "body", "child": "imu_apps", "T_child_wrt_parent": [0.012, 0.019, 0.016], "RPY_parent_to_child": [180, 0, 0]},
        {"parent": "imu_apps", "child": "cam", "T_child_wrt_parent": [0.06, 0.02, 0.02], "RPY_parent_to_child": [0, 90, -90]},
    ]}
    ri = sync_would_regenerate(conf, "cam", "imu_apps", imu_body=False)
    rb = sync_would_regenerate(conf, "cam", "imu_apps", imu_body=True)
    check("sync recomposition exists (imu + body)", ri is not None and rb is not None)
    check("body mode changes the fed frame", rot_angle_deg(ri[0], rb[0]) > 90.0)
    # sync_config conf patching: flip, already-false, and ABSENT (insert)
    import json as _json
    conf_true = '{\n\t"yaml_folder":\t"/x",\n\t"sync_config":\ttrue\n}\n'
    out = re.sub(r'("sync_config"\s*:\s*)true', r"\1false", conf_true, count=1)
    check("sync_config true -> false", _json.loads(out)["sync_config"] is False)
    conf_absent = '/** hdr */\n{\n\t"yaml_folder":\t"/x"\n}\n'
    out = insert_json_key(conf_absent, "sync_config", "false")
    parsed = _json.loads(re.sub(r"/\*.*?\*/", "", out, flags=re.S))
    check("sync_config inserted when absent", parsed.get("sync_config") is False and parsed["yaml_folder"] == "/x")
    check("insertion keeps the comment header", out.startswith("/** hdr */"))
    print("selftest:", "PASS" if ok else "FAIL")
    return 0 if ok else 2


# ----------------------------------------------------------------------------- main
def main():
    ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
    ap.add_argument("--result", default="/data/modalai/ov_calib/ov_calib_result.yaml")
    ap.add_argument("--conf", default="/etc/modalai/voxl-open-vins-server.conf")
    ap.add_argument("--extrinsics-conf", default="/etc/modalai/extrinsics.conf")
    ap.add_argument("--chain-dir", default=None, help="override the conf's yaml_folder")
    ap.add_argument("--cam", action="append", default=[], metavar="IDX=CHILD",
                    help="conf child name of result camera IDX (for the sync cross-check "
                         "when the result has no camN_name provenance)")
    ap.add_argument("--dry-run", action="store_true")
    ap.add_argument("--force", action="store_true", help="override the resolution mismatch refusal")
    ap.add_argument("--keep-hotstart", action="store_true",
                    help="do not flip calibrate_first/en_calib_hotstart/calib_hotstart_on_reset to false")
    ap.add_argument("-e", "--write-extrinsics", action="store_true",
                    help="also write the committed extrinsics into extrinsics.conf in its own format "
                         "(runs voxl-calib-to-extrinsics --in-place, .bak saved; the conf's body<->imu "
                         "extrinsics drive the frame composition on imu-body rigs)")
    ap.add_argument("--keep-sync-config", action="store_true",
                    help="leave sync_config untouched (default: set it to false, inserting the key when "
                         "absent -- an absent key means true at boot -- so boot stops regenerating the "
                         "imucam chain and cannot undo this commit)")
    ap.add_argument("--selftest", action="store_true")
    args = ap.parse_args()

    if args.selftest:
        sys.exit(selftest())

    res = parse_result_yaml(args.result)
    committed = set(res.get("committed_blocks", []) if isinstance(res.get("committed_blocks"), list) else [])
    if not committed:
        print(f"{args.result}: no committed_blocks -- nothing to commit")
        sys.exit(1)
    n_cams = int(res.get("num_cameras", 1))

    conf_raw, conf = load_json_conf(args.conf)
    yaml_folder = args.chain_dir or conf.get("yaml_folder", "/usr/share/modalai/voxl-open-vins/VoxlConfig/starling2")
    sync_config = bool(conf.get("sync_config", True))
    imu_body = bool(conf.get("imu_body_frame_mode", True))
    # warnings describe FUTURE boots: by default the chain is frozen (sync_config -> false)
    sync_after = sync_config and args.keep_sync_config

    # frame gate (CalibHotstart semantics): provenance must match the boot frame
    snap_frame = res.get("imu_frame")
    if snap_frame:
        want = "body" if imu_body else "imu"
        if snap_frame != want:
            print(f"REFUSED: snapshot imu_frame '{snap_frame}' != conf boot frame '{want}' "
                  f"(imu_body_frame_mode={int(imu_body)}) -- values would land in the wrong frame")
            sys.exit(2)
    else:
        print("WARNING: legacy snapshot without imu_frame provenance -- assuming it matches the "
              f"conf boot frame ({'body' if imu_body else 'imu'}); re-run the calibrator to record identity")

    imucam_path = yaml_folder.rstrip("/") + "/kalibr_imucam_chain.yaml"
    imu_path = yaml_folder.rstrip("/") + "/kalibr_imu_chain.yaml"
    imucam = open(imucam_path).read()
    imuch = open(imu_path).read()

    cam_map = {}
    for spec in args.cam:
        idx, _, child = spec.partition("=")
        cam_map[int(idx)] = child

    ext_conf = None
    ext_err = None
    try:
        _, ext_conf = load_json_conf(args.extrinsics_conf)
    except Exception as e:
        ext_err = str(e)

    changed_cams = []
    for i in range(n_cams):
        sfx = f"@{i}"
        has_q = ("q_ItoC" + sfx) in committed
        has_p = ("p_IinC" + sfx) in committed
        has_td = ("td" + sfx) in committed
        has_cam = ("cam" + sfx) in committed
        if not (has_q or has_p or has_td or has_cam):
            print(f"cam{i}: nothing committed -- skipped")
            continue
        sec = f"cam{i}"
        if section_span(imucam, sec) is None:
            print(f"REFUSED: {imucam_path} has no {sec} section (result has {n_cams} cameras)")
            sys.exit(2)

        # identity gate: resolution provenance vs the chain's resolution
        chain_res = get_vec(imucam, sec, "resolution")
        snap_res = res.get(f"cam{i}_resolution")
        if snap_res and chain_res and (int(snap_res[0]) != int(chain_res[0]) or int(snap_res[1]) != int(chain_res[1])):
            msg = (f"cam{i}: snapshot resolution {int(snap_res[0])}x{int(snap_res[1])} != chain "
                   f"{int(chain_res[0])}x{int(chain_res[1])} -- index mapping suspect")
            if not args.force:
                print("REFUSED: " + msg + " (pass --force to override)")
                sys.exit(2)
            print("WARNING: " + msg + " (--force)")

        # fed-frame pose: committed parts from the snapshot, the rest from the chain
        T_old = get_matrix(imucam, sec, "T_cam_imu")
        if (has_q or has_p) and T_old is None:
            print(f"REFUSED: {sec} has no T_cam_imu to compose a partial commit with")
            sys.exit(2)
        if has_q or has_p:
            Rfc = mat_from_flat(res[f"cam{i}_R_ItoC"]) if has_q else [r[0:3] for r in T_old[0:3]]
            pfc = res[f"cam{i}_p_IinC"] if has_p else [T_old[r][3] for r in range(3)]
            T_new = [Rfc[0] + [pfc[0]], Rfc[1] + [pfc[1]], Rfc[2] + [pfc[2]], [0.0, 0.0, 0.0, 1.0]]
            dR = rot_angle_deg([r[0:3] for r in T_old[0:3]], Rfc)
            dT = math.sqrt(sum((T_old[r][3] - pfc[r]) ** 2 for r in range(3))) * 1e3
            imucam = set_matrix(imucam, sec, "T_cam_imu", T_new)
            print(f"{sec}: T_cam_imu <- calibrated ({'R+p' if (has_q and has_p) else 'R only' if has_q else 'p only'});"
                  f" delta vs chain {dR:.3f} deg, {dT:.1f} mm")
            # sync_config cross-check: what would the next boot regenerate?
            if sync_config and not sync_after:
                print("  sync_config will be turned off -- boot keeps this T_cam_imu as written")
            if sync_after:
                child = cam_map.get(i) or res.get(f"cam{i}_name")
                regen = None
                if ext_conf is not None and child:
                    regen = sync_would_regenerate(ext_conf, child, "imu_apps", imu_body)
                if regen is not None:
                    gR = rot_angle_deg(regen[0], Rfc)
                    gT = math.sqrt(sum((regen[1][r] - pfc[r]) ** 2 for r in range(3))) * 1e3
                    if gR > 0.2 or gT > 2.0:
                        print(f"  ** sync_config=1 and extrinsics.conf would REGENERATE this T at boot "
                              f"{gR:.3f} deg / {gT:.1f} mm AWAY from the calibrated value.\n"
                              f"  ** Run voxl-calib-to-extrinsics (then this tool) or set sync_config to false, "
                              f"or the next boot silently restores the nominal.")
                    else:
                        print(f"  sync_config=1: extrinsics.conf already matches ({gR:.3f} deg / {gT:.1f} mm) -- "
                              f"boot regeneration is consistent")
                else:
                    why = (f"{args.extrinsics_conf}: {ext_err}" if ext_conf is None
                           else (f"no entry for '{child}'" if child else f"--cam {i}=<child> not given"))
                    print(f"  ** sync_config=1: could not cross-check extrinsics.conf ({why}). Boot will "
                          f"REGENERATE T_cam_imu from it -- make sure it carries the calibration "
                          f"(voxl-calib-to-extrinsics) or set sync_config to false.")
        if has_td:
            td = float(res[f"cam{i}_timeshift_cam_imu"])
            old = get_scalar(imucam, sec, "timeshift_cam_imu")
            imucam = set_scalar(imucam, sec, "timeshift_cam_imu", td)
            print(f"{sec}: timeshift_cam_imu {old if old is not None else '(absent)'} -> {td:.9f} "
                  f"(sync-durable: boot sync never touches it)")
        if has_cam:
            k = res[f"cam{i}_cam_k"]
            d = res[f"cam{i}_cam_d"]
            imucam = set_vec(imucam, sec, "intrinsics", k)
            imucam = set_vec(imucam, sec, "distortion_coeffs", d)
            note = (" ** sync_config=1 restores camera-server cal at next boot -- persists only with "
                    "sync_config=false" if sync_after else "")
            print(f"{sec}: intrinsics + distortion_coeffs <- calibrated{note}")
        changed_cams.append(sec)

    # ---- IMU chain: the full gauge triplet or nothing ----
    has_dw, has_da, has_qa = ("dw" in committed), ("da" in committed), ("q_AtoI" in committed)
    has_tg = "tg" in committed
    imu_written = False
    if has_dw and has_da and has_qa:
        model = None
        m = re.search(r'(?m)^  model:\s*"?([A-Za-z]+)"?', imuch)
        if m:
            model = m.group(1).lower()
        if model in ("kalibr", "calibrated"):
            model_path = "kalibr"
        elif model == "rpng":
            model_path = "rpng"
        else:
            print(f"REFUSED: {imu_path} declares unknown imu model '{model}'")
            sys.exit(2)
        Dw_r = mat_from_flat(res["Dw"])
        Da_r = mat_from_flat(res["Da"])
        R_AtoI = mat_from_flat(res["R_ACCtoIMU"])
        Tg_src = mat_from_flat(res["Tg"]) if "Tg" in res else [[0.0] * 3 for _ in range(3)]
        Dw_m, Da_m, Rg, Ra, Tg_m = imu2_to_chain(Dw_r, Da_r, R_AtoI, Tg_src, model_path)
        # round trip through the estimator's parse semantics BEFORE writing
        Tw, Ta = inv3(Dw_m), inv3(Da_m)
        Dw_b, Da_b, Ra_b, Tg_b = chain_to_imu2(inv3(Tw), inv3(Ta), Rg, Ra, Tg_m)
        dev = max(maxdev(Dw_b, Dw_r), maxdev(Da_b, Da_r), maxdev(Ra_b, R_AtoI), maxdev(Tg_b, Tg_src))
        if dev > 1e-9:
            print(f"REFUSED: IMU gauge port failed its round-trip self-check (max dev {dev:.3e})")
            sys.exit(2)
        imuch = set_matrix(imuch, "imu0", "Tw", Tw)
        imuch = set_matrix(imuch, "imu0", "Ta", Ta)
        imuch = set_matrix(imuch, "imu0", "R_IMUtoGYRO", tp(Rg))
        imuch = set_matrix(imuch, "imu0", "R_IMUtoACC", tp(Ra))
        if has_tg and "Tg" in res:
            imuch = set_matrix(imuch, "imu0", "Tg", Tg_m)
        mis = rot_angle_deg(R_AtoI, eye())
        print(f"imu0 ({model_path} gauge): Tw/Ta/R_IMUtoGYRO/R_IMUtoACC <- calibrated "
              f"(gyro-accel misalignment {mis:.3f} deg, Tg {'overlaid' if has_tg else 'kept from chain'}); "
              f"round-trip dev {dev:.1e}; noise densities untouched; sync-durable (boot sync never "
              f"rewrites this file)")
        imu_written = True
    elif has_dw or has_da or has_qa:
        print("imu0: NOT overlaid -- the {dw, da, q_AtoI} gauge triplet must be committed together "
              f"(this session committed{' dw' if has_dw else ''}{' da' if has_da else ''}"
              f"{' q_AtoI' if has_qa else ''} only); a partial overlay would mix gauges")

    if not changed_cams and not imu_written:
        print("nothing committed that these chains consume -- no files written")
        sys.exit(1)

    # ---- hot-start disarm in the server conf ----
    conf_out = conf_raw
    flips = []
    if not args.keep_hotstart:
        for key in ("calibrate_first", "en_calib_hotstart", "calib_hotstart_on_reset"):
            pat = re.compile(r'("' + key + r'"\s*:\s*)true')
            if pat.search(conf_out):
                conf_out = pat.sub(r"\1false", conf_out, count=1)
                flips.append(key)
            else:
                print(f"conf: {key} already false/absent")
        if flips:
            print(f"conf: {', '.join(flips)} -> false (the chains now carry the values; hot-start is redundant)")
    if not args.keep_sync_config:
        pat = re.compile(r'("sync_config"\s*:\s*)true')
        if pat.search(conf_out):
            conf_out = pat.sub(r"\1false", conf_out, count=1)
            flips.append("sync_config")
            print("conf: sync_config -> false (boot no longer regenerates the imucam chain)")
        elif re.search(r'"sync_config"\s*:\s*false', conf_out):
            print("conf: sync_config already false")
        else:
            conf_out = insert_json_key(conf_out, "sync_config", "false")
            flips.append("sync_config")
            print("conf: sync_config inserted as false (it was ABSENT, which means true at boot)")

    if args.dry_run:
        if args.write_extrinsics:
            print("--dry-run: would also run voxl-calib-to-extrinsics --in-place on " + args.extrinsics_conf
                  + f" (--imu-frame {'body' if imu_body else 'imu'})")
        print("--dry-run: no files written")
        sys.exit(0)

    for path, new, old in ((imucam_path, imucam, None), (imu_path, imuch, None)):
        shutil.copyfile(path, path + ".bak")
        with open(path, "w") as f:
            f.write(new)
        print(f"wrote {path} (backup {path}.bak)")
    if flips:
        shutil.copyfile(args.conf, args.conf + ".bak")
        with open(args.conf, "w") as f:
            f.write(conf_out)
        print(f"wrote {args.conf} (backup {args.conf}.bak)")

    if args.write_extrinsics:
        exe = os.path.join(os.path.dirname(os.path.abspath(__file__)), "voxl-calib-to-extrinsics")
        if not os.path.exists(exe):
            exe = "voxl-calib-to-extrinsics"  # fall back to PATH (both ship to /usr/bin)
        cmd = [exe, "--result", args.result, "--conf", args.extrinsics_conf, "--in-place",
               "--imu-frame", "body" if imu_body else "imu"]
        for idx, child in cam_map.items():
            cmd += ["--cam", f"{idx}={child}"]
        print("extrinsics.conf writeback: " + " ".join(cmd))
        rc = subprocess.call(cmd)
        if rc == 1:
            print("extrinsics.conf writeback: nothing committed to write (see above)")
        elif rc != 0:
            print(f"ERROR: extrinsics.conf writeback failed (exit {rc}); the chains and conf above "
                  f"were still written")
            sys.exit(rc)
    sys.exit(0)


if __name__ == "__main__":
    main()
