# controller.py
import time
import struct
from pathlib import Path

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


def auto_pick_iface() -> str:
    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 unitreeRemoteController:
    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_botton(self, data1, data2):
        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_key(self, data):
        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, remoteData):
        self.parse_key(remoteData)
        self.parse_botton(remoteData[2], remoteData[3])

    def get_state(self):
        return self.__dict__.copy()


class LowStateHub:
    def __init__(self, watchdog_timeout=0.25):
        self.low_state = None
        self.first_state = False
        self.last_state_time = 0.0
        self.watchdog_timeout = float(watchdog_timeout)

        self.remote = unitreeRemoteController()
        self.controller_state = self.remote.get_state()

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

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

    def combo_r2x(self) -> bool:
        s = self.controller_state
        return bool(s.get("R2", 0) and s.get("X", 0))

    def combo_r2l1(self) -> bool:
        s = self.controller_state
        return bool(s.get("R2", 0) and s.get("L1", 0))