#!/usr/bin/env python3
## /usr/bin/voxl-logger-auto-configure
##
## Interactive (and scriptable) editor for the voxl-logger-auto config file.
## Normally invoked as:  voxl-logger-auto configure [subcommand ...]
##
## With no subcommand it opens an interactive menu to change the trigger mode,
## add/remove/edit logged pipes (picking from what is live in /run/mpa), and
## tweak the other daemon settings. Subcommands do the same non-interactively:
##
##   show                       print the current config and pipe status
##   pipes [--all]              list pipes in /run/mpa the logger can record
##   trigger {manual|armed}     set the trigger mode
##   add <pipe> [--type T] [--skip N]
##                              add a pipe (type auto-detected from /run/mpa)
##   rm <pipe> [<pipe> ...]     remove pipe(s)
##   set <key> <value>          set base_dir, max_duration_s or control_pipe_name
##   preset vio [--replace]     add the VIO pipes found in vio_cams.conf etc.
##   reset                      restore the packaged default config
##
## Only the JSON body is rewritten; the /* */ header comment is kept verbatim.
## Saved changes are applied to a running daemon: a trigger-only change is sent
## live over the control pipe, anything else restarts the service.
##
## Written against the python3 stdlib only (3.6+) so it runs on every VOXL image.

import argparse
import io
import json
import os
import re
import select
import shutil
import subprocess
import sys
import termios
import tty

CONFIG_PATH  = "/etc/modalai/voxl-logger-auto.conf"
DEFAULT_PATH = "/usr/share/modalai/voxl-logger/voxl-logger-auto.conf"
STATUS_FILE  = "/run/voxl-logger-auto.status"
MPA_DIR      = "/run/mpa"
SERVICE      = "voxl-logger-auto"

# logger type -> one-line description, in the order menus present them
LOGGER_TYPES = [
    ("imu",                "IMU samples (imu_data_t)"),
    ("cam",                "camera frames (camera_image_metadata_t)"),
    ("vio",                "VIO output (vio_data_t)"),
    ("qvio",               "extended VIO (ext_vio_data_t)"),
    ("pose_6dof",          "6DOF pose (pose_vel_6dof_t)"),
    ("tof2",               "time-of-flight (tof2_data_t)"),
    ("baro",               "barometer (baro_data_t)"),
    ("rc",                 "RC channels (rc_channels_data_t)"),
    ("mav",                "all mavlink messages (mavlink_message_t)"),
    ("local_position_ned", "only LOCAL_POSITION_NED from a mavlink pipe"),
    ("ext_vfc",            "VFC offboard log (offboard_log_t)"),
    ("raw",                "raw bytes, any pipe"),
]
TYPE_NAMES = [t for t, _ in LOGGER_TYPES]

# MPA pipe type string -> logger types that can read it (first is the default).
# Anything not listed can still be logged as "raw".
MPA_TO_LOGGER = {
    "imu_data_t":              ["imu"],
    "camera_image_metadata_t": ["cam"],
    "vio_data_t":              ["vio"],
    "ext_vio_data_t":          ["qvio"],
    "pose_vel_6dof_t":         ["pose_6dof"],
    "tof2_data_t":             ["tof2"],
    "baro_data_t":             ["baro"],
    "rc_channels_data_t":      ["rc"],
    "mavlink_message_t":       ["mav", "local_position_ned"],
    "offboard_log_t":          ["ext_vfc"],
}
# pipes that carry a handle rather than data, never worth offering
UNLOGGABLE = {"mpa_ion_buf_t", "controller"}

TRIGGER_HELP = {
    "manual": "control-pipe start/stop only",
    "armed":  "also start on arm, stop on disarm",
}
KEY_ORDER = ["base_dir", "control_pipe_name", "max_duration_s", "trigger", "pipes"]


################################################################################
# terminal styling
################################################################################

USE_COLOR = sys.stdout.isatty() and os.environ.get("NO_COLOR") is None and \
            os.environ.get("TERM") != "dumb"

# symbols: the nicer ones when the output encoding can carry them, plain ASCII
# otherwise (no usable locale on the target), so the menu always works.
# VOXL_LOGGER_ASCII=1 forces ASCII for a console that cannot draw them even
# though the locale says it can (an old Windows console on the host end).
_FANCY = {"rule": "\u2500", "bad": "\u2717", "off": "\u25cb", "warn": "\u25b2", "on": "\u25cf",
          "cursor": "\u25b8", "up": "\u2191", "down": "\u2193", "updown": "\u2191\u2193",
          "leftright": "\u2190\u2192"}
_ASCII = {"rule": "-", "bad": "x", "off": "o", "warn": "!", "on": "*",
          "cursor": ">", "up": "^", "down": "v", "updown": "up/down",
          "leftright": "left/right"}
G = dict(_ASCII)

def _pick_glyphs(stream=None):
    stream = stream or sys.stdout
    G.update(_ASCII)
    if os.environ.get("VOXL_LOGGER_ASCII"):
        return
    try:
        "".join(_FANCY.values()).encode(getattr(stream, "encoding", None) or "ascii")
    except (UnicodeError, LookupError):
        return
    G.update(_FANCY)

def _sgr(code):
    return (lambda s: "\033[%sm%s\033[0m" % (code, s)) if USE_COLOR else (lambda s: s)

bold   = _sgr("1")
dim    = _sgr("2")
red    = _sgr("31")
green  = _sgr("32")
yellow = _sgr("33")
cyan   = _sgr("36")

_ANSI_RE = re.compile(r"\033\[[0-9;]*m")

def _pad(s, width):
    """ljust that ignores ANSI escapes when measuring"""
    return s + " " * max(0, width - len(_ANSI_RE.sub("", s)))

def _rule(width=72):
    return dim(G["rule"] * width)

def _die(msg):
    print(("\033[31mERROR:\033[0m " if USE_COLOR and sys.stderr.isatty() else "ERROR: ")
          + msg, file=sys.stderr)
    sys.exit(1)


################################################################################
# config file I/O
################################################################################

class Config(object):
    """A parsed config: the verbatim header comment plus the JSON body."""

    def __init__(self, path, header, data):
        self.path   = path
        self.header = header
        self.data   = data

    @property
    def pipes(self):
        return self.data.setdefault("pipes", [])

    def find_pipe(self, name):
        name = _norm_pipe(name)
        for i, p in enumerate(self.pipes):
            if _norm_pipe(p.get("name", "")) == name:
                return i
        return -1

    def copy(self):
        return Config(self.path, self.header, json.loads(json.dumps(self.data)))


def _norm_pipe(name):
    """accept 'imu_apps', 'imu_apps/', or '/run/mpa/imu_apps/' interchangeably"""
    name = name.strip().rstrip("/")
    if name.startswith(MPA_DIR + "/"):
        name = name[len(MPA_DIR) + 1:]
    return name


_HEADER_RE = re.compile(r"(?:\s*(?:/\*.*?\*/|//[^\n]*))*\s*", re.S)


def load_config(path):
    src = path
    if not os.path.exists(path):
        # same behavior as the daemon: fall back to the packaged default
        if not os.path.exists(DEFAULT_PATH):
            _die("%s does not exist and no packaged default at %s" % (path, DEFAULT_PATH))
        src = DEFAULT_PATH
    with open(src, encoding="utf-8") as f:
        text = f.read()

    # the header is the run of comments before the JSON body (it can itself
    # contain braces, e.g. "{ type, name }"); the daemon's cJSON_Minify strips
    # it, so anything we keep here must stay a comment
    header = _HEADER_RE.match(text).group(0)
    body = text[len(header):]
    # tolerate stray /* */ or // comments inside the body the way cJSON_Minify does
    body = re.sub(r"/\*.*?\*/", "", body, flags=re.S)
    body = re.sub(r"(?m)^\s*//.*$", "", body)
    try:
        data = json.loads(body)
    except ValueError as e:
        _die("could not parse %s: %s" % (src, e))
    if not isinstance(data, dict):
        _die("%s: top level must be a JSON object" % src)
    return Config(path, header, data)


def _fmt_value(v):
    if isinstance(v, float) and v.is_integer():
        v = int(v)
    return json.dumps(v)


def render_config(cfg):
    """serialize in the same hand-formatted style as the packaged default"""
    keys = [k for k in KEY_ORDER if k in cfg.data] + \
           [k for k in cfg.data if k not in KEY_ORDER]
    lines = []
    for n, k in enumerate(keys):
        comma = "," if n < len(keys) - 1 else ""
        if k == "pipes":
            # one entry per line, type then name then any extras (skip), with
            # the columns padded so they line up like the hand-written template
            rows = []
            for p in cfg.data["pipes"]:
                keys_p = [x for x in ("type", "name") if x in p] + \
                         [x for x in p if x not in ("type", "name")]
                cells = ['"%s": %s' % (x, _fmt_value(p[x])) for x in keys_p]
                rows.append([s + "," for s in cells[:-1]] + cells[-1:])
            ncol = max([len(r) for r in rows] + [0])
            widths = [max([len(r[j]) for r in rows if len(r) > j + 1] + [0])
                      for j in range(ncol)]
            lines.append('\t"pipes": [')
            for i, r in enumerate(rows):
                entry = " ".join(_pad(s, widths[j]) if j < len(r) - 1 else s
                                 for j, s in enumerate(r))
                lines.append("\t\t{ %s }%s" % (entry, "," if i < len(rows) - 1 else ""))
            lines.append("\t]" + comma)
        else:
            lines.append('\t"%s": %s%s' % (k, _fmt_value(cfg.data[k]), comma))
    return cfg.header + "{\n" + "\n".join(lines) + "\n}\n"


def save_config(cfg):
    text = render_config(cfg)
    # sanity check that what we emit still parses before touching the real file
    json.loads(re.sub(r"/\*.*?\*/", "", text[len(cfg.header):], flags=re.S))
    d = os.path.dirname(cfg.path) or "."
    if not os.access(d, os.W_OK):
        _die("no write access to %s (try again with sudo)" % d)
    tmp = cfg.path + ".tmp"
    with open(tmp, "w", encoding="utf-8") as f:
        f.write(text)
    os.rename(tmp, cfg.path)


################################################################################
# discovery of what is live on the target
################################################################################

def mpa_pipes():
    """{pipe_name: mpa_type_string} for everything currently in /run/mpa"""
    out = {}
    try:
        names = os.listdir(MPA_DIR)
    except OSError:
        return out
    for n in names:
        info = os.path.join(MPA_DIR, n, "info")
        if not os.path.isfile(info):
            continue
        t = ""
        try:
            with open(info, encoding="utf-8") as f:
                t = json.load(f).get("type", "") or ""
        except (OSError, ValueError):
            pass
        out[n] = t
    return out


def logger_types_for(mpa_type):
    if mpa_type in UNLOGGABLE:
        return []
    return MPA_TO_LOGGER.get(mpa_type, []) + ["raw"]


def loggable_pipes(live, show_all=False):
    """sorted [(name, mpa_type, [logger types])] that the logger can record"""
    rows = []
    for n in sorted(live):
        mt = live[n]
        types = logger_types_for(mt)
        if not types:
            continue
        if not show_all and mt not in MPA_TO_LOGGER:
            continue
        rows.append((n, mt, types))
    return rows


def pipe_status(p, live):
    """short colored status string for one configured pipe entry"""
    name = _norm_pipe(p.get("name", ""))
    typ = p.get("type", "")
    if typ not in TYPE_NAMES:
        return red(G["bad"] + " unknown type '%s'" % typ)
    if name not in live:
        return dim(G["off"] + " not running")
    if typ not in logger_types_for(live[name]):
        return yellow(G["warn"] + " pipe is %s" % (live[name] or "untyped"))
    return green(G["on"] + " live")


def service_state():
    """(active: bool, recording: bool, session: str, elapsed: float)"""
    try:
        active = subprocess.call(["systemctl", "is-active", "--quiet", SERVICE],
                                 stdout=subprocess.DEVNULL,
                                 stderr=subprocess.DEVNULL) == 0
    except OSError:
        active = False
    rec, sess, el = False, "", 0.0
    if active:
        try:
            with open(STATUS_FILE, encoding="utf-8") as f:
                st = json.load(f)
            rec, sess, el = bool(st.get("recording")), st.get("session", ""), st.get("elapsed_s", 0.0)
        except (OSError, ValueError):
            pass
    return active, rec, sess, el


################################################################################
# applying a saved config to the running daemon
################################################################################

def _send_control(cfg, cmd):
    """write a command to the daemon's control pipe without blocking"""
    path = os.path.join(MPA_DIR, cfg.data.get("control_pipe_name", SERVICE), "control")
    try:
        fd = os.open(path, os.O_WRONLY | os.O_NONBLOCK)
    except OSError:
        return False
    try:
        os.write(fd, cmd.encode())
    finally:
        os.close(fd)
    return True


def apply_changes(old, new, confirm=None):
    """push a saved config to the daemon. confirm(prompt)->bool is used for
    questions in interactive mode; None means non-interactive defaults."""
    if os.path.realpath(new.path) != os.path.realpath(CONFIG_PATH):
        print(dim("%s is not the service's config (%s), not applying." % (new.path, CONFIG_PATH)))
        return
    active, rec, sess, _ = service_state()
    if not active:
        print(dim("%s is not running; changes take effect when it starts." % SERVICE))
        return

    changed = set(k for k in set(old.data) | set(new.data) if old.data.get(k) != new.data.get(k))
    if changed == {"trigger"}:
        armed = new.data.get("trigger") == "armed"
        if _send_control(new, "arm_trigger %d" % (1 if armed else 0)):
            print(green("applied live: ") + "trigger=%s (no restart needed)" % new.data["trigger"])
            return
        # control pipe unreachable, fall through to a restart

    if rec:
        msg = "a session is recording (%s); restarting will end it" % sess
        if confirm is None:
            print(yellow("NOT restarting: ") + msg + ".")
            print("       run  " + bold("systemctl restart %s" % SERVICE) + "  when it is safe.")
            return
        if not confirm(yellow(msg) + ". restart now?"):
            print("not restarted; changes take effect on the next service restart.")
            return
    elif confirm is not None and not confirm("restart %s to apply?" % SERVICE, default=True):
        print("not restarted; changes take effect on the next service restart.")
        return

    if subprocess.call(["systemctl", "restart", SERVICE]) == 0:
        print(green("restarted %s" % SERVICE))
    else:
        print(red("failed to restart %s" % SERVICE))


################################################################################
# printing
################################################################################

def _fmt_duration(v):
    try:
        v = float(v)
    except (TypeError, ValueError):
        return str(v)
    if v <= 0:
        return "unbounded " + dim("(runs until stopped)")
    return ("%g s" % v) + dim("  (safety auto-stop)")


def _service_line():
    active, rec, sess, el = service_state()
    if not active:
        return dim(G["off"] + " not running")
    if rec:
        return red(G["on"] + " recording ") + "%s  %.1fs" % (sess, el)
    return green(G["on"] + " running") + dim(", idle")


def _pipe_rows(cfg, live):
    """one aligned 'type  pipe  skip  status' string per configured pipe"""
    nw = max([len(_norm_pipe(p.get("name", ""))) for p in cfg.pipes] + [4])
    rows = []
    for p in cfg.pipes:
        skip = p.get("skip", 0)
        rows.append("%s  %s  %s  %s" % (
            _pad(p.get("type", "?"), 18),
            _pad(_norm_pipe(p.get("name", "?")), nw),
            _pad(str(skip) if skip else dim("-"), 4),
            pipe_status(p, live)))
    return rows, dim("%s  %s  %s  %s" % (_pad("type", 18), _pad("pipe", nw),
                                         _pad("skip", 4), "status"))


def summary_lines(cfg, live, settings=True):
    lines = []
    rows = [("service", _service_line())]
    if settings:
        trig = cfg.data.get("trigger", "manual")
        rows += [
            ("trigger",  bold(trig) + dim("  (%s)" % TRIGGER_HELP.get(trig, "unknown mode!"))),
            ("max dur",  _fmt_duration(cfg.data.get("max_duration_s", 0))),
            ("base dir", cfg.data.get("base_dir", "")),
            ("control",  cfg.data.get("control_pipe_name", SERVICE)),
        ]
    for k, v in rows:
        lines.append("  %s %s" % (dim(_pad(k, 9)), v))

    lines.append("")
    lines.append("  " + bold("PIPES TO LOG") + dim(" (%d)" % len(cfg.pipes)))
    if not cfg.pipes:
        lines.append("  " + yellow("none -- the daemon refuses to start with an empty list"))
        return lines
    rows, head = _pipe_rows(cfg, live)
    lines.append("   " + head)
    lines += ["   " + r for r in rows]
    return lines


def print_summary(cfg, live):
    print("\n".join(summary_lines(cfg, live)))


def print_available(cfg, live, show_all):
    rows = loggable_pipes(live, show_all)
    if not rows:
        print(yellow("no loggable pipes found in %s" % MPA_DIR))
        return rows
    nw = max(len(r[0]) for r in rows)
    print(dim("  %s  %s  %s" % (_pad("pipe", nw), _pad("mpa type", 24), "logs as")))
    for n, mt, types in rows:
        used = cfg.find_pipe(n) >= 0
        line = "  %s  %s  %s" % (_pad(n, nw), _pad(mt or dim("(untyped)"), 24), " / ".join(types))
        print(dim(_ANSI_RE.sub("", line) + "  (configured)") if used else line)
    return rows


################################################################################
# presets: pipe lists derived from how the vehicle is actually configured
################################################################################

VIO_CAMS_PATH = "/etc/modalai/vio_cams.conf"
OV_CONF_PATH  = "/etc/modalai/voxl-open-vins-server.conf"


def _read_modal_json(path):
    """read a modalai .conf (JSON, may carry comments); None if missing/bad"""
    try:
        with open(path, encoding="utf-8") as f:
            text = f.read()
    except OSError:
        return None
    text = re.sub(r"/\*.*?\*/", "", text, flags=re.S)
    text = re.sub(r"(?m)^\s*//.*$", "", text)
    try:
        return json.loads(text)
    except ValueError:
        return None


def preset_vio():
    """Everything needed to replay and evaluate voxl-open-vins-server: the
    tracking cams and IMU it reads (from vio_cams.conf), its ov / ov_extended
    output, and the VFC offboard_log. Returns ([(type, name, why)], [notes])."""
    entries, notes = [], []

    def add(typ, name, why):
        if all(e[1] != name for e in entries):
            entries.append((typ, name, why))

    cams = _read_modal_json(VIO_CAMS_PATH)
    imus = []
    if cams is None:
        notes.append(yellow("%s not found or unreadable: no cameras added" % VIO_CAMS_PATH))
    else:
        enabled = [c for c in cams.get("cams", []) if c.get("enable", False)]
        if not enabled:
            notes.append(yellow("no enabled cameras in %s" % VIO_CAMS_PATH))
        for c in enabled:
            pipe = c.get("pipe_for_tracking", "")
            if not pipe:
                notes.append(yellow("camera %s has no pipe_for_tracking" % c.get("name", "?")))
                continue
            # the server may track from the ION buffer-handle pipe, which carries
            # no image data; the same frames are published without the suffix
            if pipe.endswith("_ion"):
                pipe = pipe[:-4]
            add("cam", pipe, "vio_cams.conf: %s" % c.get("name", "?"))
            imu = c.get("imu", "")
            if imu and imu not in imus:
                imus.append(imu)

    # the server remaps its IMU to <imu>_body in body-frame mode (its default)
    ov = _read_modal_json(OV_CONF_PATH)
    body = True if ov is None else bool(ov.get("imu_body_frame_mode", True))
    src = "vio_cams.conf imu" if imus else "default imu"
    for imu in imus or ["imu_apps"]:
        add("imu", imu + "_body" if body else imu,
            src + (" (body frame)" if body else ""))

    add("vio",     "ov",           "VIO estimate")
    add("raw",     "ov_extended",  "estimate + features, byte-exact")
    add("ext_vfc", "offboard_log", "VFC offboard log")
    return entries, notes


PRESETS = {
    "vio": ("VIO preset", preset_vio),
}


def apply_preset(cfg, entries, replace=False):
    """add (or with replace, set exactly) the preset pipes; returns names added"""
    if replace:
        cfg.data["pipes"] = []
    added = []
    for typ, name, _ in entries:
        if cfg.find_pipe(name) < 0:
            cfg.pipes.append({"type": typ, "name": name})
            added.append(name)
    return added


################################################################################
# edits shared by the CLI and the interactive menu (return an error or None)
################################################################################

def do_add(cfg, live, name, typ=None, skip=0):
    name = _norm_pipe(name)
    if not name:
        return "empty pipe name"
    if cfg.find_pipe(name) >= 0:
        return "%s is already configured" % name
    if typ is None:
        if name not in live:
            return "%s is not in %s; give a type explicitly (--type)" % (name, MPA_DIR)
        types = logger_types_for(live[name])
        if not types:
            return "%s (%s) cannot be logged" % (name, live[name])
        typ = types[0]
    if typ not in TYPE_NAMES:
        return "unknown type '%s' (one of: %s)" % (typ, ", ".join(TYPE_NAMES))
    entry = {"type": typ, "name": name}
    if skip:
        entry["skip"] = int(skip)
    cfg.pipes.append(entry)
    return None


def do_set(cfg, key, value):
    if key == "max_duration_s":
        try:
            v = float(value)
        except ValueError:
            return "max_duration_s must be a number of seconds"
        if v < 0:
            return "max_duration_s must be >= 0 (0 = unbounded)"
        cfg.data[key] = int(v) if v.is_integer() else v
    elif key == "base_dir":
        if not value.startswith("/"):
            return "base_dir must be an absolute path"
        cfg.data[key] = value if value.endswith("/") else value + "/"
    elif key == "control_pipe_name":
        if not re.match(r"^[A-Za-z0-9_\-]+$", value):
            return "control_pipe_name may only contain letters, digits, _ and -"
        cfg.data[key] = value
    elif key == "trigger":
        if value not in TRIGGER_HELP:
            return "trigger must be 'manual' or 'armed'"
        cfg.data[key] = value
    else:
        return "unknown key '%s' (base_dir, max_duration_s, control_pipe_name, trigger)" % key
    return None


################################################################################
# interactive menu: raw keystrokes, full-screen redraw
#
#   up/w   down/s   move          enter/right/d   select
#   space           tick (lists)  esc/left/a/q    back
#   pgup/pgdn/home/end            1-9             jump (main menu)
################################################################################

# escape sequences (after the ESC byte) -> key names
_SEQ = {
    "[A": "up", "[B": "down", "[C": "right", "[D": "left",
    "OA": "up", "OB": "down", "OC": "right", "OD": "left",
    "[5~": "pgup", "[6~": "pgdn",
    "[H": "home", "[F": "end", "OH": "home", "OF": "end", "[1~": "home", "[4~": "end",
}
# key -> navigation action; anything unmapped is passed through as-is
_NAV = {
    "up": "up", "w": "up", "k": "up",
    "down": "down", "s": "down", "j": "down",
    "\n": "select", "\r": "select", "right": "select", "d": "select",
    "esc": "back", "left": "back", "a": "back", "q": "back",
    " ": "toggle",
}


class _Term(object):
    """Owns the tty for the interactive session: alternate screen, hidden
    cursor and cbreak mode (keys arrive unbuffered, Ctrl-C still signals).
    cooked() temporarily restores normal line input for typing text."""

    def __init__(self):
        self.fd = sys.stdin.fileno()
        self.saved = termios.tcgetattr(self.fd)

    def __enter__(self):
        sys.stdout.write("\033[?1049h")     # alternate screen, like less/vim
        self.raw()
        return self

    def __exit__(self, *exc):
        self.cooked()
        sys.stdout.write("\033[?1049l")
        sys.stdout.flush()

    def raw(self):
        tty.setcbreak(self.fd)
        sys.stdout.write("\033[?25l")       # hide cursor
        sys.stdout.flush()

    def cooked(self):
        termios.tcsetattr(self.fd, termios.TCSADRAIN, self.saved)
        sys.stdout.write("\033[?25h")
        sys.stdout.flush()


_term = None


def _read_key():
    fd = sys.stdin.fileno()
    c = os.read(fd, 1)
    if c == b"\x1b":
        # a lone ESC is the Esc key; otherwise collect the rest of the sequence
        seq = b""
        while select.select([fd], [], [], 0.05)[0]:
            seq += os.read(fd, 1)
            if len(seq) >= 2 and (seq[-1:].isalpha() or seq[-1:] == b"~"):
                break
        return _SEQ.get(seq.decode("ascii", "ignore"), "") if seq else "esc"
    if c == b"\x03":
        raise KeyboardInterrupt
    if c == b"\x04":
        return "esc"
    return c.decode("utf-8", "ignore")


def _nav(key):
    return _NAV.get(key.lower() if len(key) == 1 else key, key)


def _trunc(s, width):
    """cut a string to width visible columns, keeping its ANSI codes intact"""
    out, n, i = [], 0, 0
    while i < len(s):
        m = _ANSI_RE.match(s, i)
        if m:
            out.append(m.group(0))
            i = m.end()
            continue
        if n >= width:
            out.append("\033[0m" if USE_COLOR else "")
            break
        out.append(s[i])
        n += 1
        i += 1
    return "".join(out)


def _draw(lines):
    cols, rows = shutil.get_terminal_size()
    # home + overwrite each line + clear to end: no full clear, so no flicker
    sys.stdout.write("\033[H" + "\n".join(_trunc(l, cols - 1) + "\033[K"
                                          for l in lines[:rows]) + "\033[J")
    sys.stdout.flush()


def _highlight(s):
    return "\033[7m" + _ANSI_RE.sub("", s) + "\033[0m" if USE_COLOR else "> " + s


def _pick(header, rows, hint, cursor=0, multi=False, checked=None, disabled=(), keys=""):
    """Draw header + a scrolling list with a highlighted cursor row, and handle
    keys until one ends the pick. Returns (action, cursor, checked) where action
    is "select", "back", a digit (keys contains "#"), or a hotkey from keys."""
    checked = set(checked or ())
    top = 0
    while True:
        _, height = shutil.get_terminal_size()
        view = max(3, height - len(header) - 3)
        if rows:
            cursor = max(0, min(cursor, len(rows) - 1))
        top = min(top, cursor)
        top = max(top, cursor - view + 1)

        frame = list(header)
        frame.append(dim("     " + G["up"] + " more") if top > 0 else "")
        if not rows:
            frame.append(dim("     (nothing here)"))
        for i in range(top, min(len(rows), top + view)):
            text = rows[i]
            if multi:
                box = "[-] " if i in disabled else ("[x] " if i in checked else "[ ] ")
                text = (green(box) if i in checked else box) + text
            if i == cursor:
                frame.append(cyan("  " + G["cursor"] + " ") + _highlight(text))
            elif i in disabled:
                frame.append("    " + dim(_ANSI_RE.sub("", text)))
            else:
                frame.append("    " + text)
        frame.append(dim("     " + G["down"] + " more") if top + view < len(rows) else "")
        frame.append("  " + dim(hint))
        _draw(frame)

        key = _read_key()
        act = _nav(key)
        if key in keys and key not in ("", "#"):
            return key, cursor, checked
        if act == "up" and rows:
            cursor = (cursor - 1) % len(rows)
        elif act == "down" and rows:
            cursor = (cursor + 1) % len(rows)
        elif act == "pgup":
            cursor = max(0, cursor - view)
        elif act == "pgdn":
            cursor = min(len(rows) - 1, cursor + view)
        elif act == "home":
            cursor = 0
        elif act == "end":
            cursor = len(rows) - 1
        elif act == "toggle" and multi and rows and cursor not in disabled:
            checked ^= {cursor}
            cursor = min(cursor + 1, len(rows) - 1)
        elif act == "select":
            return "select", cursor, checked
        elif act == "back":
            return "back", cursor, checked
        elif "#" in keys and key.isdigit() and 1 <= int(key) <= len(rows):
            return "select", int(key) - 1, checked


def _ask(prompt, default=None):
    """line input for free text, dropping out of raw mode while typing"""
    suffix = dim(" [%s]" % default) if default not in (None, "") else ""
    _term.cooked()
    try:
        s = input("\n  %s%s: " % (prompt, suffix)).strip()
    except EOFError:
        s = ""
    finally:
        _term.raw()
    return s if s else (default if default is not None else "")


def _confirm(prompt, default=False):
    """inline Yes/No chooser on its own line: left/right (a/d) moves between the
    two buttons, enter/space picks, y/n answer directly, esc means No"""
    ans = default
    sys.stdout.write("\n")
    while True:
        buttons = []
        for label, val in (("Yes", True), ("No", False)):
            b = "  %s  " % label
            buttons.append(("\033[7m%s\033[0m" % b if USE_COLOR else "[%s]" % label)
                           if val == ans else b)
        sys.stdout.write("\r\033[K  %s   %s %s   %s" % (prompt, buttons[0], buttons[1],
                                                        dim(G["leftright"] + " choose  enter ok")))
        sys.stdout.flush()

        k = _read_key()
        kl = k.lower() if len(k) == 1 else k
        if kl in ("left", "right", "up", "down", "a", "d", "w", "s", "\t", "h", "l"):
            ans = not ans
            continue
        if kl == "y":
            ans = True
        elif kl in ("n", "esc", "q"):
            ans = False
        elif kl not in ("\n", "\r", " "):
            continue
        sys.stdout.write("\r\033[K  %s   %s\n" % (prompt, bold("Yes") if ans else bold("No")))
        sys.stdout.flush()
        return ans


def _wait_key():
    sys.stdout.write("\n  " + dim("press any key"))
    sys.stdout.flush()
    _read_key()


def _title(text, sub=""):
    return ["  " + bold(text) + ("    " + dim(sub) if sub else ""), _rule()]


def _pick_type(title, options, current):
    """single-choice list of logger types; returns the chosen type or None"""
    desc = dict(LOGGER_TYPES)
    rows = ["%s  %s" % (_pad(t, 18), dim(desc.get(t, ""))) for t in options]
    cur = options.index(current) if current in options else 0
    act, i, _ = _pick(_title(title), rows, G["updown"] + " move   enter choose   esc back", cursor=cur)
    return options[i] if act == "select" and options else None


def _menu_add(cfg, live):
    show_all = False
    cursor, checked = 0, set()
    while True:
        avail = loggable_pipes(live, show_all)
        configured = set(i for i, r in enumerate(avail) if cfg.find_pipe(r[0]) >= 0)
        nw = max([len(r[0]) for r in avail] + [4])
        rows = ["%s  %s  %s" % (_pad(n, nw), _pad(mt or "(untyped)", 24), " / ".join(types))
                for n, mt, types in avail]
        header = _title("Add pipes", "live in %s%s" % (MPA_DIR, "" if show_all else ", known types")) + \
                 ["        " + dim("%s  %s  %s" % (_pad("pipe", nw), _pad("mpa type", 24), "logs as"))]
        hint = "space tick   enter add   tab %s   n type a name   esc back" % \
               ("known types only" if show_all else "show all")
        act, cursor, checked = _pick(header, rows, hint, cursor, multi=True,
                                     checked=checked, disabled=configured, keys="\tn")
        if act == "back":
            return None
        if act == "\t":
            show_all = not show_all
            cursor, checked = 0, set()
            continue

        if act == "n":
            # for a pipe that isn't running yet, typed by hand with a chosen type
            name = _norm_pipe(_ask("pipe name"))
            if not name:
                continue
            typ = None
            if name not in live:
                typ = _pick_type("Type for %s (not running)" % name, TYPE_NAMES, None)
                if typ is None:
                    continue
            err = do_add(cfg, live, name, typ)
            return red(err) if err else green("added ") + name

        picks = sorted(checked) or ([cursor] if rows and cursor not in configured else [])
        if not picks:
            return yellow("nothing selected")
        added, errs = [], []
        for i in picks:
            err = do_add(cfg, live, avail[i][0])
            if err:
                errs.append(err)
            else:
                added.append(avail[i][0])
        msg = []
        if added:
            msg.append(green("added ") + ", ".join(added))
        if errs:
            msg.append(yellow("; ".join(errs)))
        return "  ".join(msg)


def _menu_remove(cfg, live):
    if not cfg.pipes:
        return yellow("no pipes to remove")
    rows, head = _pipe_rows(cfg, live)
    act, cursor, checked = _pick(_title("Remove pipes") + ["        " + head], rows,
                                 "space tick   enter remove   esc back", multi=True)
    if act != "select":
        return None
    picks = sorted(checked) or [cursor]
    gone = [_norm_pipe(cfg.pipes[i].get("name", "")) for i in picks]
    for i in reversed(picks):
        del cfg.pipes[i]
    return green("removed ") + ", ".join(gone)


def _menu_edit(cfg, live):
    if not cfg.pipes:
        return yellow("no pipes to edit")
    rows, head = _pipe_rows(cfg, live)
    act, i, _ = _pick(_title("Edit which pipe?") + ["    " + head], rows,
                      G["updown"] + " move   enter edit   esc back")
    if act != "select":
        return None
    p = cfg.pipes[i]
    name = _norm_pipe(p.get("name", ""))
    options = (logger_types_for(live[name]) if name in live else []) or TYPE_NAMES
    t = _pick_type("Type for %s%s" % (name, "  (%s)" % live[name] if name in live else ""),
                   options, p.get("type"))
    if t is None:
        return None
    s = _ask("skip: drop N of every N+1 samples, 0 = keep all", str(p.get("skip", 0)))
    if not s.isdigit():
        return red("skip must be a whole number")
    p["type"] = t
    if int(s):
        p["skip"] = int(s)
    else:
        p.pop("skip", None)
    return green("updated ") + name


def _menu_preset(cfg, live, key):
    title, build = PRESETS[key]
    entries, notes = build()
    nw = max([len(e[1]) for e in entries] + [4])
    lines = []
    for typ, name, why in entries:
        p = {"type": typ, "name": name}
        st = dim("already configured") if cfg.find_pipe(name) >= 0 else pipe_status(p, live)
        lines.append("      %s  %s  %s  %s" % (_pad(typ, 8), _pad(name, nw), _pad(st, 18), dim(why)))
    header = _title(title, "built from this vehicle's config") + notes + \
        ["      " + dim("%s  %s  %s  %s" % (_pad("type", 8), _pad("pipe", nw), _pad("status", 18), "from"))] + \
        lines + ["", "  " + bold("Apply it how?")]
    choices = [
        ("add",     "Add to current pipes", dim("keeps what is there, adds what is missing")),
        ("replace", "Replace current pipes", dim("the pipe list becomes exactly the above")),
        ("cancel",  "Cancel", ""),
    ]
    rows = ["%s  %s" % (_pad(label, 22), hint) for _, label, hint in choices]
    act, i, _ = _pick(header, rows, G["updown"] + " move   enter choose   esc back")
    if act != "select" or choices[i][0] == "cancel" or not entries:
        return None
    added = apply_preset(cfg, entries, replace=(choices[i][0] == "replace"))
    if choices[i][0] == "replace":
        return green("pipe list set to the %s (%d pipes)" % (title, len(entries)))
    return green("added ") + ", ".join(added) if added else dim("nothing to add, all already configured")


def _menu_browse(cfg, live):
    names = sorted(live)
    nw = max([len(n) for n in names] + [4])
    rows = []
    for n in names:
        types = logger_types_for(live[n])
        if cfg.find_pipe(n) >= 0:
            how = cyan("configured")
        elif not types:
            how = dim("not loggable")
        elif live[n] in MPA_TO_LOGGER:
            how = green(" / ".join(types))
        else:
            how = dim("raw only")
        rows.append("%s  %s  %s" % (_pad(n, nw), _pad(live[n] or dim("(untyped)"), 26), how))
    cursor = 0
    while True:
        act, cursor, _ = _pick(_title("Everything in %s" % MPA_DIR, "%d pipes" % len(names)),
                               rows, G["updown"] + " move   enter add this pipe   esc back", cursor)
        if act != "select" or not names:
            return None
        n = names[cursor]
        if cfg.find_pipe(n) >= 0 or not logger_types_for(live[n]):
            continue
        err = do_add(cfg, live, n)
        return red(err) if err else green("added ") + n


def interactive(path, no_apply=False):
    global _term
    if not (sys.stdin.isatty() and sys.stdout.isatty()):
        _die("interactive mode needs a terminal; see 'voxl-logger-auto configure --help'")
    original = load_config(path)
    cfg = original.copy()
    msg = "" if os.path.exists(path) else yellow("%s missing, editing the packaged default" % path)
    cursor = 0

    with _Term() as _term:
        while True:
            live = mpa_pipes()
            dirty = render_config(cfg) != render_config(original) or not os.path.exists(path)
            trig = cfg.data.get("trigger", "manual")
            items = [
                ("trigger", "Trigger mode", bold(trig) + dim("  " + TRIGGER_HELP.get(trig, "?"))),
                ("add",     "Add pipes",    dim("from what is live in %s" % MPA_DIR)),
                ("preset",  "VIO preset",   dim("cams + imu from vio_cams.conf, ov, offboard_log")),
                ("remove",  "Remove pipes", ""),
                ("edit",    "Edit a pipe",  dim("type / skip")),
                ("dur",     "Max duration", _fmt_duration(cfg.data.get("max_duration_s", 0))),
                ("base",    "Base dir",     cfg.data.get("base_dir", "")),
                ("browse",  "Browse pipes", dim("everything in %s" % MPA_DIR)),
                ("save",    "Save & apply", yellow("* unsaved changes") if dirty else dim("no changes")),
                ("quit",    "Quit",         ""),
            ]
            rows = ["%s  %s  %s" % (dim("%2d" % (n + 1)), _pad(label, 14), val)
                    for n, (_, label, val) in enumerate(items)]
            header = _title("voxl-logger-auto configure", path) + \
                     summary_lines(cfg, live, settings=False) + [_rule(), "  " + msg, ""]
            msg = ""

            try:
                act, cursor, _ = _pick(header, rows,
                                       G["updown"] + "/ws move   enter/d select   1-9 jump   q quit",
                                       cursor, keys="#")
                what = items[cursor][0] if act == "select" else "quit"

                if what == "trigger":
                    cfg.data["trigger"] = "armed" if trig == "manual" else "manual"
                    msg = "trigger: %s -> %s" % (trig, bold(cfg.data["trigger"]))
                elif what == "add":
                    msg = _menu_add(cfg, live) or ""
                elif what == "preset":
                    msg = _menu_preset(cfg, live, "vio") or ""
                elif what == "remove":
                    msg = _menu_remove(cfg, live) or ""
                elif what == "edit":
                    msg = _menu_edit(cfg, live) or ""
                elif what == "dur":
                    v = _ask("max duration in seconds, 0 = unbounded",
                             _fmt_value(cfg.data.get("max_duration_s", 0)))
                    err = do_set(cfg, "max_duration_s", v)
                    msg = red(err) if err else green("max_duration_s = %s" % v)
                elif what == "base":
                    err = do_set(cfg, "base_dir", _ask("base dir", cfg.data.get("base_dir", "")))
                    msg = red(err) if err else green("base_dir = %s" % cfg.data["base_dir"])
                elif what == "browse":
                    msg = _menu_browse(cfg, live) or ""
                elif what == "save":
                    if not cfg.pipes:
                        msg = red("add at least one pipe before saving")
                        continue
                    save_config(cfg)
                    print("\n\n  " + green("saved ") + path)
                    if not no_apply:
                        apply_changes(original, cfg, confirm=_confirm)
                    original = cfg.copy()
                    _wait_key()
                elif what == "quit":
                    if not dirty or _confirm(yellow("discard unsaved changes and quit?")):
                        return 0
            except KeyboardInterrupt:
                try:
                    if not dirty or _confirm(yellow("discard unsaved changes and quit?")):
                        return 130
                except KeyboardInterrupt:
                    return 130


################################################################################
# non-interactive subcommands
################################################################################

def _commit(old, new, args):
    if not new.pipes:
        _die("refusing to save a config with no pipes")
    save_config(new)
    print(green("saved ") + new.path)
    if not args.no_apply:
        apply_changes(old, new)


def _safe_stdio():
    # _pick_glyphs keeps our own symbols within what the output can encode; a
    # config can still carry other characters (a pipe name, a path): print those
    # as '?' rather than die when the locale is ASCII (LANG/LC_ALL unset, or a
    # LANG forwarded by ssh/adb that the image lacks).
    for name in ("stdout", "stderr"):
        s = getattr(sys, name)
        try:
            s.reconfigure(errors="replace")
        except AttributeError:      # python 3.6 has no reconfigure
            setattr(sys, name, io.TextIOWrapper(s.buffer, encoding=s.encoding, errors="replace",
                                                line_buffering=s.line_buffering))


def main():
    _safe_stdio()
    _pick_glyphs()
    ap = argparse.ArgumentParser(
        prog="voxl-logger-auto configure",
        description="Edit the voxl-logger-auto config. Run with no subcommand "
                    "for an interactive menu.")
    ap.add_argument("-c", "--config", default=CONFIG_PATH,
                    help="config file to edit (default %(default)s)")
    ap.add_argument("-n", "--no-apply", action="store_true",
                    help="save only; don't push changes to the running service")
    sub = ap.add_subparsers(dest="cmd")

    sub.add_parser("show", help="print the current config and pipe status")
    p = sub.add_parser("pipes", help="list pipes in /run/mpa the logger can record")
    p.add_argument("-a", "--all", action="store_true", help="include pipes loggable only as raw")
    p = sub.add_parser("trigger", help="set the trigger mode")
    p.add_argument("mode", choices=sorted(TRIGGER_HELP))
    p = sub.add_parser("add", help="add a pipe (type auto-detected from /run/mpa)")
    p.add_argument("pipe")
    p.add_argument("-t", "--type", choices=TYPE_NAMES, help="logger type (overrides auto-detect)")
    p.add_argument("-s", "--skip", type=int, default=0, help="drop N of every N+1 samples")
    p = sub.add_parser("rm", help="remove pipe(s)")
    p.add_argument("pipe", nargs="+")
    p = sub.add_parser("set", help="set base_dir, max_duration_s or control_pipe_name")
    p.add_argument("key", choices=["base_dir", "max_duration_s", "control_pipe_name"])
    p.add_argument("value")
    sub.add_parser("reset", help="restore the packaged default config")
    p = sub.add_parser("preset", help="add a preset pipe list built from the vehicle's config")
    p.add_argument("name", choices=sorted(PRESETS),
                   help="vio: tracking cams + imu from vio_cams.conf, ov, ov_extended, offboard_log")
    p.add_argument("-r", "--replace", action="store_true",
                   help="make the pipe list exactly the preset instead of adding to it")

    args = ap.parse_args()

    if args.cmd is None:
        return interactive(args.config, args.no_apply)

    live = mpa_pipes()
    old = load_config(args.config)
    cfg = old.copy()

    if args.cmd == "show":
        print(dim(args.config))
        print_summary(cfg, live)
        return 0

    if args.cmd == "pipes":
        print_available(cfg, live, args.all)
        return 0

    if args.cmd == "trigger":
        err = do_set(cfg, "trigger", args.mode)
    elif args.cmd == "add":
        # already present is not an error, so a list of adds can be re-run
        if cfg.find_pipe(args.pipe) >= 0:
            print(dim("%s is already configured" % _norm_pipe(args.pipe)))
            return 0
        err = do_add(cfg, live, args.pipe, args.type, args.skip)
    elif args.cmd == "rm":
        err = None
        for n in args.pipe:
            i = cfg.find_pipe(n)
            if i < 0:
                err = "%s is not configured" % _norm_pipe(n)
                break
            del cfg.pipes[i]
    elif args.cmd == "set":
        err = do_set(cfg, args.key, args.value)
    elif args.cmd == "preset":
        entries, notes = PRESETS[args.name][1]()
        for n in notes:
            print(n)
        for typ, name, why in entries:
            print("  %s  %s  %s" % (_pad(typ, 8), _pad(name, 28), dim(why)))
        if not entries:
            _die("preset produced no pipes")
        apply_preset(cfg, entries, args.replace)
        err = None
    elif args.cmd == "reset":
        if not os.path.exists(DEFAULT_PATH):
            _die("no packaged default at %s" % DEFAULT_PATH)
        cfg = load_config(DEFAULT_PATH)
        cfg.path = args.config
        err = None
    if err:
        _die(err)

    if render_config(cfg) == render_config(old) and os.path.exists(args.config):
        print(dim("no change"))
        return 0
    _commit(old, cfg, args)
    return 0


if __name__ == "__main__":
    try:
        sys.exit(main())
    except KeyboardInterrupt:
        print()
        sys.exit(130)
