#!/usr/bin/env python3
# SPDX-FileCopyrightText: 2026 branderson
# SPDX-License-Identifier: GPL-3.0-or-later

"""Drive an Insta360 Link gimbal from a game controller.

Reads the kernel joystick device directly (``/dev/input/js*``) and issues
relative ``linkctl`` moves while a stick is held off centre, in the same
velocity style as the Plasma widget. Qt 6 removed QtGamepad and offers no
replacement, so the applet cannot do this itself.

Only the standard library is used: the joystick event format is eight bytes
and reading it needs no third-party package.

Discovering the mapping for your controller:

    linkctl-gamepad --list                 # which devices exist
    linkctl-gamepad --monitor              # print events as you press things

Then run it, overriding any axis or button number that differs:

    linkctl-gamepad --button 3="tracking toggle"

Defaults target a DualSense on the in-tree ``hid_playstation`` driver. Button
and axis numbering is driver-specific, so treat them as a starting point and
confirm with ``--monitor``.
"""

from __future__ import annotations

import argparse
import errno
import glob
import os
import select
import struct
import subprocess
import sys
import time

# struct js_event { __u32 time; __s16 value; __u8 type; __u8 number; }
JS_EVENT = struct.Struct("<IhBB")
JS_EVENT_SIZE = JS_EVENT.size          # 8

JS_EVENT_BUTTON = 0x01
JS_EVENT_AXIS = 0x02
JS_EVENT_INIT = 0x80                   # ORed into type on the initial sync

AXIS_MAX = 32767.0

DEFAULT_BUTTONS = {
    0: "center",
    1: "preset load home",
    2: "tracking toggle",
}


# --------------------------------------------------------------------------
# Pure helpers. Kept free of I/O so tests/test_gamepad.py can exercise them.
# --------------------------------------------------------------------------

def decode_event(buf: bytes) -> tuple[int, int, int, int]:
    """Decode one joystick event into (time, value, type, number).

    The JS_EVENT_INIT flag is stripped from the type and returned separately
    by `is_init`, because callers almost always want the bare type.
    """
    if len(buf) != JS_EVENT_SIZE:
        raise ValueError(f"expected {JS_EVENT_SIZE} bytes, got {len(buf)}")
    tstamp, value, etype, number = JS_EVENT.unpack(buf)
    return tstamp, value, etype & ~JS_EVENT_INIT, number


def is_init(buf: bytes) -> bool:
    """Whether an event is part of the synthetic initial state burst.

    Opening the device replays the current state of every axis and button.
    Treating those as real presses would fire actions at startup.
    """
    return bool(JS_EVENT.unpack(buf)[2] & JS_EVENT_INIT)


def normalize_axis(raw: int) -> float:
    """Scale a raw axis reading to -1.0..1.0."""
    return max(-1.0, min(1.0, raw / AXIS_MAX))


def apply_deadzone(value: float, deadzone: float) -> float:
    """Zero out small readings and rescale the rest.

    Rescaling matters: without it, motion jumps to `deadzone` worth of speed
    the instant the stick leaves the dead area. With it, speed still starts
    from zero at the edge of the deadzone.
    """
    if deadzone < 0 or deadzone >= 1:
        raise ValueError("deadzone must be in [0, 1)")
    magnitude = abs(value)
    if magnitude <= deadzone:
        return 0.0
    scaled = (magnitude - deadzone) / (1.0 - deadzone)
    return scaled if value > 0 else -scaled


def movement_commands(
    nx: float,
    ny: float,
    step: float,
    invert_pan: bool = False,
    invert_tilt: bool = False,
) -> list[list[str]]:
    """Build the linkctl argument lists for one tick.

    `ny` is positive upwards. Each axis needs its own call because linkctl's
    relative commands are single-axis, so a diagonal yields two. Returns an
    empty list when nothing is worth sending.
    """
    x = -nx if invert_pan else nx
    y = -ny if invert_tilt else ny
    out: list[list[str]] = []
    dx = abs(x) * step
    dy = abs(y) * step
    # Below half a degree the camera's own 0.1 degree resolution makes the
    # step pointless, and it would otherwise creep on a near-centred stick.
    if dx >= 0.5:
        out.append(["right" if x > 0 else "left", f"{dx:.1f}"])
    if dy >= 0.5:
        out.append(["up" if y > 0 else "down", f"{dy:.1f}"])
    return out


def parse_button_spec(spec: str) -> tuple[int, str]:
    """Parse a ``N=linkctl args`` button mapping."""
    number, sep, action = spec.partition("=")
    if not sep:
        raise argparse.ArgumentTypeError(
            f"expected NUMBER=ACTION, got {spec!r}")
    try:
        index = int(number.strip())
    except ValueError:
        raise argparse.ArgumentTypeError(
            f"button number must be an integer, got {number!r}") from None
    action = action.strip()
    if not action:
        raise argparse.ArgumentTypeError(f"empty action for button {index}")
    return index, action


# --------------------------------------------------------------------------
# Device discovery
# --------------------------------------------------------------------------

def device_name(path: str) -> str:
    """Read a joystick's advertised name from sysfs, or '' if unavailable."""
    node = os.path.basename(path)
    for candidate in (
        f"/sys/class/input/{node}/device/name",
        f"/sys/class/input/{node}/name",
    ):
        try:
            with open(candidate, "r") as handle:
                return handle.read().strip()
        except OSError:
            continue
    return ""


def list_devices() -> list[tuple[str, str]]:
    return [(p, device_name(p))
            for p in sorted(glob.glob("/dev/input/js*"))]


def pick_device(explicit: str | None, match: str | None) -> str | None:
    """Choose a joystick device, preferring an explicit path."""
    if explicit:
        return explicit if os.path.exists(explicit) else None
    devices = list_devices()
    if not devices:
        return None
    if match:
        needle = match.lower()
        for path, name in devices:
            if needle in name.lower():
                return path
        return None
    return devices[0][0]


# --------------------------------------------------------------------------
# Runner
# --------------------------------------------------------------------------

class Runner:
    """Invokes linkctl, with a shared base argument list."""

    def __init__(self, binary: str, device: str | None, force: bool,
                 dry_run: bool, verbose: bool):
        self.base = [binary]
        if force:
            self.base.append("--force")
        if device:
            self.base += ["--device", device]
        self.base.append("--quiet")
        self.dry_run = dry_run
        self.verbose = verbose

    def __call__(self, args: list[str]) -> None:
        cmd = self.base + args
        if self.dry_run:
            print("would run:", " ".join(cmd), flush=True)
            return
        try:
            # Blocking, with a timeout: these take milliseconds, and letting
            # them overlap would queue conflicting moves. A hung linkctl
            # costs one tick rather than wedging the daemon.
            result = subprocess.run(
                cmd, stdout=subprocess.DEVNULL, stderr=subprocess.PIPE,
                timeout=2.0,
            )
            if result.returncode != 0 and self.verbose:
                err = result.stderr.decode("utf-8", "replace").strip()
                print(f"linkctl exited {result.returncode}: {err}",
                      file=sys.stderr, flush=True)
        except subprocess.TimeoutExpired:
            print("linkctl timed out", file=sys.stderr, flush=True)
        except FileNotFoundError:
            print(f"linkctl not found: {self.base[0]}",
                  file=sys.stderr, flush=True)
            raise SystemExit(127) from None


def monitor(path: str) -> int:
    """Print events as they arrive, to discover axis and button numbers."""
    print(f"Monitoring {path} ({device_name(path) or 'unknown'}).")
    print("Move sticks and press buttons. Ctrl-C to stop.\n")
    with open(path, "rb", buffering=0) as handle:
        while True:
            buf = handle.read(JS_EVENT_SIZE)
            if not buf or len(buf) < JS_EVENT_SIZE:
                return 0
            _, value, etype, number = decode_event(buf)
            tag = " (initial)" if is_init(buf) else ""
            if etype == JS_EVENT_AXIS:
                print(f"axis   {number:>2}  value {value:>6}  "
                      f"({normalize_axis(value):+.2f}){tag}", flush=True)
            elif etype == JS_EVENT_BUTTON:
                state = "pressed" if value else "released"
                print(f"button {number:>2}  {state}{tag}", flush=True)


def run(args: argparse.Namespace, run_cmd: Runner) -> int:
    buttons = dict(DEFAULT_BUTTONS) if args.default_buttons else {}
    buttons.update(dict(args.button or []))

    axes: dict[int, float] = {}
    interval = args.interval / 1000.0

    while True:
        path = pick_device(args.js_device, args.match)
        if path is None:
            if not args.wait:
                print("no joystick found; use --list to see devices, or "
                      "--wait to keep retrying", file=sys.stderr)
                return 3
            time.sleep(2.0)
            continue

        try:
            handle = open(path, "rb", buffering=0)
        except OSError as exc:
            if exc.errno in (errno.EACCES, errno.EPERM):
                print(f"cannot open {path}: permission denied. Add yourself "
                      "to the 'input' group.", file=sys.stderr)
                return 6
            time.sleep(2.0)
            continue

        print(f"Using {path} ({device_name(path) or 'unknown'})", flush=True)
        axes.clear()
        next_tick = time.monotonic() + interval

        try:
            with handle:
                while True:
                    now = time.monotonic()
                    timeout = max(0.0, next_tick - now)
                    ready, _, _ = select.select([handle], [], [], timeout)

                    if ready:
                        chunk = handle.read(JS_EVENT_SIZE * 32)
                        if not chunk:
                            raise OSError(errno.ENODEV, "device went away")
                        for offset in range(0, len(chunk) - JS_EVENT_SIZE + 1,
                                            JS_EVENT_SIZE):
                            buf = chunk[offset:offset + JS_EVENT_SIZE]
                            _, value, etype, number = decode_event(buf)
                            if etype == JS_EVENT_AXIS:
                                axes[number] = normalize_axis(value)
                            elif (etype == JS_EVENT_BUTTON and value
                                    and not is_init(buf)):
                                action = buttons.get(number)
                                if action:
                                    run_cmd(action.split())
                                elif args.verbose:
                                    print(f"button {number} unmapped",
                                          flush=True)

                    now = time.monotonic()
                    if now < next_tick:
                        continue
                    # Skip rather than catch up, so a slow linkctl cannot
                    # build a backlog of stale moves.
                    next_tick = now + interval

                    raw_x = axes.get(args.pan_axis, 0.0)
                    raw_y = axes.get(args.tilt_axis, 0.0)
                    nx = apply_deadzone(raw_x, args.deadzone)
                    # Sticks report positive downwards; the rest of the
                    # codebase treats positive as up.
                    ny = -apply_deadzone(raw_y, args.deadzone)
                    for cmd in movement_commands(nx, ny, args.step,
                                                 args.invert_pan,
                                                 args.invert_tilt):
                        run_cmd(cmd)
        except KeyboardInterrupt:
            return 0
        except OSError as exc:
            print(f"{path} closed ({exc.strerror or exc}); waiting for the "
                  "controller to come back", file=sys.stderr, flush=True)
            if not args.wait:
                return 8
            time.sleep(2.0)


def build_parser() -> argparse.ArgumentParser:
    p = argparse.ArgumentParser(
        prog="linkctl-gamepad",
        description="Drive an Insta360 Link gimbal from a game controller.",
        formatter_class=argparse.RawDescriptionHelpFormatter,
        epilog="Run --monitor to discover your controller's axis and button "
               "numbers, which are driver-specific.",
    )
    p.add_argument("--list", action="store_true",
                   help="list joystick devices and exit")
    p.add_argument("--monitor", action="store_true",
                   help="print controller events and exit")

    p.add_argument("--js-device", metavar="PATH",
                   help="joystick device (default: first match)")
    p.add_argument("--match", metavar="TEXT", default="DualSense",
                   help="substring of the controller name to select "
                        "(default: %(default)s; use '' for any)")
    p.add_argument("--wait", action="store_true",
                   help="keep retrying when the controller is absent, and "
                        "reconnect when it returns")

    p.add_argument("--pan-axis", type=int, default=0, metavar="N",
                   help="axis for pan (default: %(default)s)")
    p.add_argument("--tilt-axis", type=int, default=1, metavar="N",
                   help="axis for tilt (default: %(default)s)")
    p.add_argument("--deadzone", type=float, default=0.15, metavar="F",
                   help="ignore deflection below this, 0..1 "
                        "(default: %(default)s)")
    p.add_argument("--step", type=float, default=6.0, metavar="DEG",
                   help="degrees per tick at full deflection "
                        "(default: %(default)s)")
    p.add_argument("--interval", type=int, default=160, metavar="MS",
                   help="milliseconds between ticks (default: %(default)s)")
    p.add_argument("--invert-pan", action="store_true")
    p.add_argument("--invert-tilt", action="store_true")

    p.add_argument("--button", action="append", type=parse_button_spec,
                   metavar="N=ACTION",
                   help="map a button to linkctl arguments, repeatable, "
                        "e.g. --button 3=\"tracking toggle\"")
    p.add_argument("--no-default-buttons", dest="default_buttons",
                   action="store_false",
                   help="do not apply the built-in button mappings")

    p.add_argument("--linkctl", default="linkctl", metavar="PATH",
                   help="linkctl binary (default: %(default)s)")
    p.add_argument("--device", metavar="PATH",
                   help="camera video node, passed to linkctl --device")
    p.add_argument("--no-force", dest="force", action="store_false",
                   help="respect linkctl's idle-camera guard")
    p.add_argument("--dry-run", action="store_true",
                   help="print commands instead of running them")
    p.add_argument("-v", "--verbose", action="store_true")
    return p


def main(argv: list[str] | None = None) -> int:
    args = build_parser().parse_args(argv)

    if args.deadzone < 0 or args.deadzone >= 1:
        print("--deadzone must be in [0, 1)", file=sys.stderr)
        return 2
    if args.interval < 10:
        print("--interval must be at least 10ms", file=sys.stderr)
        return 2

    if args.list:
        devices = list_devices()
        if not devices:
            print("no joystick devices found under /dev/input")
            return 3
        for path, name in devices:
            print(f"{path}  {name or '(unknown)'}")
        return 0

    if args.monitor:
        path = pick_device(args.js_device, args.match or None)
        if path is None:
            print("no matching joystick found; try --list or --match ''",
                  file=sys.stderr)
            return 3
        try:
            return monitor(path)
        except KeyboardInterrupt:
            return 0

    runner = Runner(args.linkctl, args.device, args.force,
                    args.dry_run, args.verbose)
    try:
        return run(args, runner)
    except KeyboardInterrupt:
        return 0


if __name__ == "__main__":
    sys.exit(main())
