"""
controller.py — G1 wireless-remote DDS reader for Saqr.

Trimmed copy of the helper used by Project/Manual Photographer. The G1's
built-in remote publishes its button state on the `rt/lowstate` DDS topic;
LowStateHub subscribes to that topic and exposes the parsed button bits and
a couple of combo helpers for the bridge to poll.

Buttons follow the same byte layout as Manual Photographer:
    data1 bits: R1, L1, Start, Select, R2, L2, F1, F3
    data2 bits: A, B, X, Y, Up, Right, Down, Left
"""

from __future__ import annotations

import struct
import time
from pathlib import Path

from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_


def auto_pick_iface() -> str:
    """Best-effort guess of the DDS network interface."""
    try:
        net_dir = Path("/sys/class/net")
        if not net_dir.exists():
            return "lo"
        names = [p.name for p in net_dir.iterdir() if p.is_dir()]
        for pref in ("en", "eth", "wl"):
            for n in names:
                if n != "lo" and n.startswith(pref):
                    return n
        for n in names:
            if n != "lo":
                return n
        return "lo"
    except Exception:
        return "lo"


class UnitreeRemote:
    def __init__(self):
        self.Lx = 0; self.Rx = 0; self.Ry = 0; self.Ly = 0
        self.L1 = 0; self.L2 = 0; self.R1 = 0; self.R2 = 0
        self.A = 0;  self.B = 0;  self.X = 0;  self.Y = 0
        self.Up = 0; self.Down = 0; self.Left = 0; self.Right = 0
        self.Select = 0; self.F1 = 0; self.F3 = 0; self.Start = 0

    def _parse_buttons(self, data1: int, data2: int) -> None:
        self.R1 = (data1 >> 0) & 1; self.L1 = (data1 >> 1) & 1
        self.Start = (data1 >> 2) & 1; self.Select = (data1 >> 3) & 1
        self.R2 = (data1 >> 4) & 1; self.L2 = (data1 >> 5) & 1
        self.F1 = (data1 >> 6) & 1; self.F3 = (data1 >> 7) & 1
        self.A = (data2 >> 0) & 1; self.B = (data2 >> 1) & 1
        self.X = (data2 >> 2) & 1; self.Y = (data2 >> 3) & 1
        self.Up = (data2 >> 4) & 1; self.Right = (data2 >> 5) & 1
        self.Down = (data2 >> 6) & 1; self.Left = (data2 >> 7) & 1

    def _parse_axes(self, data: bytes) -> None:
        offsets = [4, 8, 12, 20]  # Lx, Rx, Ry, Ly
        self.Lx, self.Rx, self.Ry, self.Ly = [
            struct.unpack('<f', data[o:o + 4])[0] for o in offsets
        ]

    def parse(self, remote_data: bytes) -> None:
        self._parse_axes(remote_data)
        self._parse_buttons(remote_data[2], remote_data[3])

    def state(self) -> dict:
        return self.__dict__.copy()


class LowStateHub:
    """DDS subscriber callback target. Stores the latest parsed remote state."""

    def __init__(self, watchdog_timeout: float = 0.25):
        self.low_state: LowState_ | None = None
        self.first_state = False
        self.last_state_time = 0.0
        self.watchdog_timeout = float(watchdog_timeout)
        self.remote = UnitreeRemote()

    def handler(self, msg: LowState_) -> None:
        self.low_state = msg
        self.first_state = True
        self.last_state_time = time.time()
        try:
            self.remote.parse(msg.wireless_remote)
        except Exception:
            pass

    def fresh(self) -> bool:
        return (time.time() - self.last_state_time) < self.watchdog_timeout

    # ── Combo helpers ───────────────────────────────────────────────────────
    def combo_r2x(self) -> bool:
        return bool(self.remote.R2 and self.remote.X)

    def combo_r2y(self) -> bool:
        return bool(self.remote.R2 and self.remote.Y)