import os
import re
import subprocess
import numpy as np

root_path = os.path.join(os.path.dirname(__file__), "../..")

def get_pose(joint_positions, base_link="PELVIS_S", target_link="R_FINGER_TIP",
             exec_path=os.path.join(root_path, "cmake-build-debug/example/solve_fk")):
    joint_str = [str(j) for j in joint_positions]
    cmd = [exec_path, base_link, target_link, *joint_str]
    res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, check=True, text=True)
    lines = res.stdout.splitlines()
    start_idx = next((i for i, l in enumerate(lines) if re.search(r"变换矩阵|T\s*:", l)), None)
    if start_idx is None or start_idx + 4 >= len(lines):
        raise RuntimeError("未在 solve_fk 输出中找到 4×4 变换矩阵:\n" + res.stdout)
    try:
        mat = np.array([[float(x) for x in lines[start_idx + 1 + r].split()] for r in range(4)], dtype=np.float64)
        if mat.shape != (4, 4):
            raise ValueError
    except Exception:
        raise RuntimeError("矩阵解析失败，原始输出:\n" + res.stdout)
    return mat


def solve_ik(pose, base_link="PELVIS_S", target_link="R_WRIST_R_S",
             exec_path=os.path.join(root_path, "cmake-build-debug/example/solve_ik")):
    [x, y, z, rx, ry, rz] = pose
    cmd = [exec_path, base_link, target_link, f"{x}", f"{y}", f"{z}", f"{rx}", f"{ry}", f"{rz}"]
    print(" ".join(cmd))
    res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, check=True)
    lines = res.stdout.splitlines()
    success_line = next((l for l in lines if "[IK Solve]" in l), "")
    success = "success=true" in success_line.lower()
    if not success:
        return False, None, None

    def _parse(tag):
        pats = rf"^{tag}\s*:\s*(.+)$"
        for l in lines:
            m = re.match(pats, l.strip())
            if m:
                return [float(v) for v in m.group(1).split()]
        raise RuntimeError(f"success=true 但未找到 {tag}: 行！\n{res.stdout}")

    left_q  = _parse("left")
    right_q = _parse("right")
    return True, left_q, right_q