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 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