"""dog_bridge.py — TCP bridge between Navigator and Freenove Robot Dog.

HARDWARE STATUS: NOT YET WIRED
This file is a ready-to-use client for when the Freenove Robot Dog Kit
is assembled and running its Server.py on the Raspberry Pi. It requires:

  Pi-side changes (NOT yet applied):
    - Command.py: add CMD_FOOTPAD = "CMD_FOOTPAD"
    - Server.py:  add measuring_footpad() method + elif handler

  Hardware wiring (NOT yet done):
    - 4x 500g FSR sensors on footpads, each with 10kOhm pull-down
    - FSR front-left  -> ADS7830 channel 1
    - FSR front-right -> ADS7830 channel 2
    - FSR rear-left   -> ADS7830 channel 3
    - FSR rear-right  -> ADS7830 channel 4

  Connection: WiFi TCP to Pi IP, port 5001 (commands), 8001 (video).
"""
import socket
import threading


class DogBridge:
    """TCP client for commanding the Freenove Robot Dog and reading sensors."""

    def __init__(self, host, cmd_port=5001):
        self.host = host
        self.cmd_port = cmd_port
        self.sock = None
        self.lock = threading.Lock()
        self._buffer = ""

    def connect(self):
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.connect((self.host, self.cmd_port))
        self.sock.settimeout(2.0)

    def disconnect(self):
        if self.sock:
            self.sock.close()
            self.sock = None

    def send_cmd(self, cmd_str):
        """Send a command string (e.g. 'CMD_MOVE_FORWARD#8')."""
        with self.lock:
            self.sock.sendall((cmd_str + "\n").encode("utf-8"))

    def recv_response(self):
        """Read one newline-delimited response from the dog."""
        with self.lock:
            while "\n" not in self._buffer:
                chunk = self.sock.recv(1024).decode("utf-8")
                if not chunk:
                    raise ConnectionError("Dog disconnected")
                self._buffer += chunk
            line, self._buffer = self._buffer.split("\n", 1)
            return line

    # --- Movement ---

    def move_forward(self, speed=8):
        self.send_cmd(f"CMD_MOVE_FORWARD#{speed}")

    def move_backward(self, speed=8):
        self.send_cmd(f"CMD_MOVE_BACKWARD#{speed}")

    def turn_left(self, speed=8):
        self.send_cmd(f"CMD_TURN_LEFT#{speed}")

    def turn_right(self, speed=8):
        self.send_cmd(f"CMD_TURN_RIGHT#{speed}")

    def stop(self):
        self.send_cmd("CMD_MOVE_STOP#")

    # --- Sensors ---

    def get_distance(self):
        """Ultrasonic distance in cm."""
        self.send_cmd("CMD_SONIC#")
        resp = self.recv_response()  # CMD_SONIC#<cm>
        return float(resp.split("#")[1])

    def get_battery(self):
        """Battery voltage (2S LiPo, ~6.4-8.4V range)."""
        self.send_cmd("CMD_POWER#")
        resp = self.recv_response()  # CMD_POWER#<volts>
        return float(resp.split("#")[1])

    def get_footpads(self):
        """Per-foot pressure readings (0-255 each, 500g FSR sensors).

        Returns dict with keys: front_left, front_right, rear_left, rear_right.
        Requires Pi-side CMD_FOOTPAD handler (see module docstring).
        """
        self.send_cmd("CMD_FOOTPAD#")
        resp = self.recv_response()  # CMD_FOOTPAD#FL#FR#RL#RR
        parts = resp.split("#")
        return {
            "front_left": int(parts[1]),
            "front_right": int(parts[2]),
            "rear_left": int(parts[3]),
            "rear_right": int(parts[4]),
        }

    # --- Posture & Head ---

    def set_head(self, angle):
        """Set head servo angle."""
        self.send_cmd(f"CMD_HEAD#{angle}")

    def relax(self):
        """Disengage all servos."""
        self.send_cmd("CMD_RELAX#")

    def balance_on(self):
        """Enable IMU-based self-balancing."""
        self.send_cmd("CMD_BALANCE#1")

    def balance_off(self):
        self.send_cmd("CMD_BALANCE#0")

    def set_height(self, height):
        """Adjust standing height."""
        self.send_cmd(f"CMD_HEIGHT#{height}")

    # --- LED & Buzzer ---

    def buzzer(self, state):
        """state: '1' on, '0' off."""
        self.send_cmd(f"CMD_BUZZER#{state}")

    def led(self, index, r, g, b):
        """Set LED color. index: 0-based LED number."""
        self.send_cmd(f"CMD_LED#{index}#{r}#{g}#{b}")