import numpy as np
import pandas as pd
from pathlib import Path


def get_T_cp():
    pass

# ---------- 用户给出的 T_ee->cam ----------
# R_ec = np.array([[ 0.92095   ,  0.33864198,  0.19280227],
#                  [-0.2465026 ,  0.88946636, -0.38481952],
#                  [-0.30180718,  0.30687328,  0.90263017]])
# t_ec = np.array([[-0.00596568],
#                  [-0.61356453],
#                  [ 0.02675161]])

R_ec = np.array([[ 1   ,  0,  0],
                 [0 ,  1, 0],
                 [0,  0,  1]])
t_ec = np.array([[-0.035],
                 [-0.18],
                 [ 0.08]])

T_ec = np.eye(4)
T_ec[:3, :3] = R_ec
T_ec[:3,  3] = t_ec.ravel()

# ---------- 读取 RobotToolPose.csv ----------
csv_path = Path("RobotToolPose.csv")
if not csv_path.exists():
    raise FileNotFoundError("RobotToolPose.csv 未上传到工作目录")

mat = np.loadtxt(csv_path, delimiter=',')   # 3 × (4*N)
if mat.shape[0] != 3 or mat.shape[1] % 4 != 0:
    raise ValueError("RobotToolPose.csv 格式应为 3×(4*N)")

N = mat.shape[1] // 4
T_list = []
for i in range(N):
    block = mat[:, 4*i:4*i+4]          # 3×4
    T = np.eye(4)
    T[:3,:4] = block
    T_list.append(T)

# ---------- 计算 T_base->cam = T_base->ee * T_ee->cam ----------
T_bc_list = [T_be @ T_ec for T_be in T_list]

# 简单检查：平移范数 & 旋转角度（相对于第一帧）
def rot_to_axis_angle(R):
    angle = np.arccos(max(min((np.trace(R)-1)/2,1),-1))
    return angle

trans_norms = [np.linalg.norm(T[:3,3]) for T in T_bc_list]
rot_angles  = [rot_to_axis_angle(T[:3,:3] @ T_bc_list[0][:3,:3].T) for T in T_bc_list]

df = pd.DataFrame({
    'frame': np.arange(N),
    'tx': [T[0,3] for T in T_bc_list],
    'ty': [T[1,3] for T in T_bc_list],
    'tz': [T[2,3] for T in T_bc_list],
    '||t|| (m)': trans_norms,
    'Δθ w.r.t #0 (rad)': rot_angles
})

print(df)