import os,sys
import numpy as np
from slam.slam import compose_ply
from utils import read_ply,write_ply,file_fliter

def run(folder,alpha,beta,gamma,savename):
    
    # literate the folder
    dir_List_ = os.listdir(folder)
    sort_lambda = lambda x:int(x)
    dir_List_.sort(key=sort_lambda)
    
    dir_List=dir_List_


    angle = []
    index = 0 
    for i,degree in enumerate([alpha,beta,gamma]):
        if degree == "auto":
            degree = dir_List
            index = i
        angle.append(degree)

    data = []
    
    # iterate the auto angle degree
    for an in angle[index]:
        ply_with_mutiangle = []
        for z in angle:
            if isinstance(z,list):
                z = an
            ply_with_mutiangle.append(-float(z))
        ply_name_list = file_fliter(os.path.join(folder,an), "ply",lambda x:int(x.split(".")[0]))
        # select the first ply file
        for i in range(len(ply_name_list)):
            ply_with_mutiangle_copy = ply_with_mutiangle.copy()
            ply = read_ply(os.path.join(folder,an,ply_name_list[i]))
            ply_with_mutiangle_copy.append(ply)
            data.append(ply_with_mutiangle_copy)

    # compose muti ply files to one ply file
    cloudpoint = compose_ply(data)

    # write the file
    write_ply(cloudpoint,".", savename)






if __name__ == "__main__":
    def help():
        print("Usage: mannul_slam.py [folder] [alpha] [beta] [gamma] [savename]")
        print("EXAMPLE: mannul_slam.py T:\\Data\\chuizhi60°_shuiping120° 0 60 auto test60120")
        exit(1)

    if len(sys.argv) != 6:
        help()
    try:
        folder = str(sys.argv[1])
        alpha = str(sys.argv[2])
        beta = str(sys.argv[3])
        gamma = str(sys.argv[4])
        savename = str(sys.argv[5])

    except Exception as e:
        print(e)
        help()

    run(folder,alpha,beta,gamma,savename)
    print("Done")