import numpy as np
import os, sys
# add python path of src to sys.path
src_path = os.path.join(__file__, *(['..'] * 2))
src_path = os.path.abspath(src_path)
sys.path.insert(0, src_path)

from utils.tools import read_ply



# 毫米波坐标系为右手坐标系。
# 毫米波设备分别绕xyz轴的旋转角度分别为 alpha, beta, gamma

# 左乘旋转矩阵分别为 Rx_alpha, Ry_beta, Rz_gamma

# Rx_alpha = [[1,0,0],[0,np.cos(alpha),np.sin(alpha)],[0,-np.sin(alpha),np.cos(alpha)]]
# Ry_beta = [[np.cos(beta),0,-np.sin(beta)],[0,1,0],[np.sin(beta),0,np.cos(beta)]]
# Rz_gamma = [[np.cos(gamma),np.sin(gamma),0],[-np.sin(gamma),np.cos(gamma),0],[0,0,1]]


def get_rotation_matrix(alpha_degree,beta_degree,gamma_degree,verbose=0):
    """generate a rotation matrix from the specific angel
    """
    rad_list = []
    for i, angle in enumerate([alpha_degree,beta_degree,gamma_degree]):

        if np.abs(float(angle))>360:
            raise ValueError(f"incorrect angle value: value in [-360,360] is excepted, but {angle} is offered.")
        rad_list.append(np.pi * float(angle) / 180)

    alpha = rad_list[0]
    Rx_alpha = np.matrix([[1,0,0],
                        [0,np.cos(rad_list[0]),np.sin(alpha)],
                        [0,-np.sin(alpha),np.cos(alpha)]])

    beta = rad_list[1]
    Ry_beta = np.matrix([[np.cos(beta),0,-np.sin(beta)],
                        [0,1,0],
                        [np.sin(beta),0,np.cos(beta)]])

    gamma = rad_list[2]
    Rz_gamma = np.matrix([[np.cos(gamma),np.sin(gamma),0],
                        [-np.sin(gamma),np.cos(gamma),0],
                        [0,0,1]])
         
    rotation_matrix = Rx_alpha * Ry_beta * Rz_gamma

    if verbose == 1:
        print(f"\n{alpha_degree}, rad {alpha}, Rx_alpha \n", Rx_alpha)
        print(f"\n{beta_degree}, {beta}, Ry_beta \n", Ry_beta)
        print(f"\n{gamma_degree}, {gamma}, Rz_gamma\n",Rz_gamma)
        print("\n rotation matrix\n",rotation_matrix)
    else:
        pass

    return rotation_matrix

def transferTo(current,delta_x,delta_y,delta_z,alpha,beta,gamma,reverse=False):
    # Notice: 假定当前坐标系为右手系
    # step 1: 平移；current_coor + delta(delta_x, delta_y, delta_z) = target_coor
    #               x' = x - delta_x, y' = y - delta_y, z' = z -delta_y
    # step 2: 旋转; 当前坐标系朝正方面给旋转到指定坐标系。
    # step 3: 检查是否需要从右手系转换到左手系。
    # 平移
    ply = current - np.array([delta_x,delta_y,delta_z])
    # 旋转
    # z = (X * Y^T)^T = (Y * X^T)
    # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
    rotation_matrix = get_rotation_matrix(alpha,beta,gamma)
    rotated_ply = np.matmul(ply,rotation_matrix)
    # 转为右手坐标系
    if reverse:
        rotated_ply[:,-1] = -rotated_ply.copy()[:,-1]

    data = rotated_ply
    return data

def compose_ply(muti_ply_with_angle: list):

    ply_nums = len(muti_ply_with_angle)
    composed_ply_array = None

    for i, ply_with_angle in enumerate( muti_ply_with_angle):
        angle = ply_with_angle[:-1]
        ply = ply_with_angle[-1]

        if isinstance(ply,str):
            ply = read_ply(ply)
        
        rotation_matrix = get_rotation_matrix(angle[0],angle[1],angle[2])

        # z = (X * Y^T)^T = (Y * X^T)
        # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
        rotated_ply = np.matmul(ply,rotation_matrix)
        if i == 0:
            composed_ply_array = np.array(rotated_ply)
        else:
            temp = np.array(rotated_ply)
            composed_ply_array = np.concatenate((composed_ply_array,temp),axis= 0 )
    
    composed_ply_array = composed_ply_array.reshape(-1,3)
    return composed_ply_array


if __name__ == "__main__":
    current = np.array([[1,1,1],[1,3,9],[4,8,5]])
    print(current)
    target = transferTo(current,1,1,1,0,0,0,True)
    print(target)