import os,sys
import numpy as np
import time
from slam.slam import compose_ply
from utils import read_ply,write_ply,file_fliter,udp_get_tool,udp2bin_tool,bin2ply_tool
from mannul_slam import run as compose_muti_ply

cudir = src_path = os.path.abspath(os.path.join(__file__, *(['..'] * 1)))
exe_path = os.path.abspath(os.path.join(cudir,"../RUN/Test_Servo.exe"))

def run(port,speed,degre_step,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder):

    # 旋转机构复位
    os.system(f"{exe_path} {port} 100 0")

    # 间隔获取udp
    for i in range(0,360,degre_step):
        os.system(f"{exe_path} {port} {speed} {i}")
        time.sleep(3)
        subFolder = str(-i)
        udp_get_tool(microwaveHost,udp_packets,deviceName,saveFolder,subFolder)
    
    # 旋转机构复位
    os.system(f"{exe_path} {port} 100 0")

    # iterate the folder
    for root, dirs, files in os.walk(saveFolder,topdown=True):
        break

    for dir in dirs:
        # udp2bin bin2ply
        subdir = os.path.join(saveFolder,dir)
        udp2bin_tool(subdir)
        bin2ply_tool(subdir)


    # compose ply
    compose_muti_ply(saveFolder,alpha,beta,gamma,"data")

if __name__ == "__main__":
    def help():
        print("Usage: auto_slam.py [port] [speed] [degre_step] [alpha] [beta] [gamma] [microwaveHost] [udp_packets] [deviceName] [saveFolder]")
        print("EXAMPLE: auto_slam.py COM11 100 5 0 20 auto 192.168.2.65 1000 pod1 ./data/run_test/")
        exit(1)

    if len(sys.argv) != 11:
        help()
    try:
        port = str(sys.argv[1])
        speed = str(sys.argv[2])
        degre_step = str(sys.argv[3])
        alpha = str(sys.argv[4])
        beta = str(sys.argv[5])
        gamma = str(sys.argv[6])
        microwaveHost = str(sys.argv[7])
        udp_packets = int(sys.argv[8])
        deviceName = str(sys.argv[9])
        saveFolder = str(sys.argv[10])
    except Exception as e:
        print(e)
        help()

    run(port,speed,degre_step,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder)
    print("Done")