"""PD Standing Controller for Spot robot."""
import numpy as np
import os
import importlib.util

# Import BaseController using importlib to work when loaded via importlib
_controllers_dir = os.path.dirname(os.path.abspath(__file__))
_base_path = os.path.join(_controllers_dir, "base.py")
_spec = importlib.util.spec_from_file_location("spot_base", _base_path)
_base_module = importlib.util.module_from_spec(_spec)
_spec.loader.exec_module(_base_module)
BaseController = _base_module.BaseController


class PDStandingController(BaseController):
    """Simple PD controller to hold standing pose for Spot."""

    # Standing pose from the model's keyframe
    # hx=0, hy=1.04, kn=-1.8 for all legs
    STANDING_POSE = np.array([
        0.0, 1.04, -1.8,  # FL: hx, hy, kn
        0.0, 1.04, -1.8,  # FR
        0.0, 1.04, -1.8,  # HL
        0.0, 1.04, -1.8,  # HR
    ], dtype=np.float32)

    def __init__(self, num_joints: int = 12):
        super().__init__(num_joints)
        self.target_pose = self.STANDING_POSE.copy()

        # Velocity command (for compatibility with G1 interface)
        self.cmd = np.array([0.0, 0.0, 0.0], dtype=np.float32)

    def set_command(self, vx: float = 0.0, vy: float = 0.0, vyaw: float = 0.0):
        """Set velocity command (used for gait controller, ignored here)."""
        self.cmd[0] = np.clip(vx, -1.0, 1.0)
        self.cmd[1] = np.clip(vy, -0.5, 0.5)
        self.cmd[2] = np.clip(vyaw, -1.5, 1.5)

    def get_command(self):
        """Get current velocity command."""
        return self.cmd.copy()

    def compute_action(self, obs: np.ndarray, data) -> np.ndarray:
        """Compute target joint positions for position actuators.

        Since Spot uses position actuators with built-in PD control,
        we just return the target positions.
        """
        # The model's position actuators handle PD control internally
        # We just need to return target positions
        return self.target_pose.copy()

    def reset(self):
        super().reset()
        self.target_pose = self.STANDING_POSE.copy()
        self.cmd = np.array([0.0, 0.0, 0.0], dtype=np.float32)

    @property
    def name(self) -> str:
        return "PD Standing"