import numpy as np

rotation = R_matrix()#张啸给我的值填这里

def flip_coefficient_if_positive(coefficient):
    # 检查 coefficient[2] 是否大于0
    if coefficient[2] > 0:
        # 取反所有分量
        coefficient = [-x for x in coefficient]
        print("翻转：\n")

    return coefficient

def vec2ola(coefficient):#首先是相机到法兰的转换，之后是法兰到新坐标系的转换（新坐标系就是与Z轴与法向量一致的坐标系)
    #如果不转换坐标轴，
    coefficient_raw = flip_coefficient_if_positive(coefficient)
    coefficient = coefficient_raw.reshape(-1)
    curZ = np.array(coefficient)# 定义 Z 方向的向量
    # print(curZ)

    curX = np.array([1, 1, 0],dtype=np.float64)# 定义初始 X 方向的向量
    curX /= np.linalg.norm(curX)  # 归一化 X 向量

    curY = np.cross(curZ, curX)# 计算 Y 方向的向量
    curY /= np.linalg.norm(curY)  # 归一化 Y 向量

    curX = np.cross(curY, curZ)# 重新计算 X 方向的向量
    curX /= np.linalg.norm(curX)  # 归一化 X 向量

    # 创建旋转矩阵
    rotM = np.array([
        [curX[0], curY[0], curZ[0]],
        [curX[1], curY[1], curZ[1]],
        [curX[2], curY[2], curZ[2]]
    ])

    # 打印旋转矩阵
    # print("Rotation Matrix:")
    # print(rotM)

    # 计算欧拉角（XYZ顺序）并转换为度
    r = R.from_matrix(rotM)
    euler_angles = r.as_euler('xyz', degrees=True)#或者是zxy

    # print("Euler Angles (degrees):")
    # print(euler_angles)
    return euler_angles

#张啸给我的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):
    target = np.asarray([x, y, z,1])
    camera2robot = np.loadtxt('D:\BaiduNetdiskDownload\机械臂\GRCNN\\real\cam_pose.txt', delimiter=' ')
    robot2base = rotation
    camera2base = robot2base @ camera2robot
    target_position = np.dot(camera2base, target)

    rpy = vec2ola(a, b, c)
    r, p, y = rpy[0], rpy[1], rpy[2]  # r表示u,p表示v，y表示w
    target_angle_raw = np.asarray([r, p, y])
    target_angle = np.dot(camera2robot[0:3, 0:3], target_angle_raw)
    print(target_position, target_angle)

    return target_position,target_angle