import os

import numpy as np
from scipy.spatial.transform import Rotation as R

def vec2rpy(normal):
    # 将法向量的反方向作为机械臂末端执行器的新Z轴
    z_axis = -normal / np.linalg.norm(normal)  # 归一化并取反向作为Z轴

    # 尝试选择一个合适的X轴方向，尽量避免与Z轴共线
    if abs(z_axis[2]) != 1:
        x_axis = np.array([1, 0, 0])
    else:
        x_axis = np.array([0, 1, 0])

    x_axis = x_axis - np.dot(x_axis, z_axis) * z_axis  # 投影到垂直于Z轴的平面
    x_axis = x_axis / np.linalg.norm(x_axis)

    # 计算Y轴方向，确保它与X轴和Z轴正交
    y_axis = np.cross(z_axis, x_axis)

    # 构造旋转矩阵
    rotation_matrix = np.vstack([x_axis, y_axis, z_axis]).T

    # 将旋转矩阵转换为RPY（roll, pitch, yaw）
    rpy = R.from_matrix(rotation_matrix).as_euler('xyz', degrees=True)

    return rpy

#张啸给我的xyzuvw
def R_matrix(x,y,z,u,v,w):
    rx = np.radians(u)
    ry = np.radians(v)
    rz = np.radians(w)
    # 定义绕 X, Y, Z 轴的旋转矩阵
    R_x = np.array([
        [1, 0, 0],
        [0, np.cos(rx), -np.sin(rx)],
        [0, np.sin(rx), np.cos(rx)]
    ])

    R_y = np.array([
        [np.cos(ry), 0, np.sin(ry)],
        [0, 1, 0],
        [-np.sin(ry), 0, np.cos(ry)]
    ])

    R_z = np.array([
        [np.cos(rz), -np.sin(rz), 0],
        [np.sin(rz), np.cos(rz), 0],
        [0, 0, 1]
    ])
    R = R_z @ R_y @ R_x
    T = np.array([x, y, z])

    # 构建齐次变换矩阵
    transformation_matrix = np.eye(4)
    transformation_matrix[:3, :3] = R
    transformation_matrix[:3, 3] = T

    return transformation_matrix


# 图像识别结果：xyz和法向量
def getPosition(x,y,z,a,b,c,rotation):
    target = np.asarray([x, y, z,1])
    camera2robot = np.loadtxt('./Trace/com_pose.txt', delimiter=' ') #相对目录且分隔符采用os.sep
    robot2base = rotation
    camera2base = robot2base @ camera2robot
    target_position = np.dot(camera2base, target)

    angle = np.asarray([a,b,c])
    noraml = camera2base[:3, :3]@angle
    noraml_base = vec2rpy(noraml)

    print(target_position, noraml_base)

    return target_position,noraml_base