import datetime
import queue
from socketserver import BaseRequestHandler, UDPServer
import time
import os, sys
import open3d as o3d
from multiprocessing import  Pool
import numpy as np
import threading

from ..utils.tools import file_fliter, write_ply
from .servers import MicroWaveReceiver,MicroWaveUDPThread
from .parsers import MicroWaveParser


def udp_get_tool(microwaveHost,packetThreshold,deviceName,saveFolder,subFolder):

    # host = ''
    # host = '192.168.2.65' # use this host to get the udp data from microwave device
    port = 9911
    udp_queue = queue.Queue()
    udp_get_stop = threading.Event()

    if subFolder is not None:
        path = os.path.join(saveFolder,subFolder)
    else:
        start = datetime.datetime.now()
        path = os.path.join(saveFolder,f"{deviceName}-{start.strftime('%Y-%m-%d-%H-%M-%S')}")
    if not os.path.exists(path):
        os.makedirs(path)

    udp_thread = MicroWaveUDPThread(MicrowaveHost=microwaveHost,
                                MicrowavePort=port,
                                udp_queue=udp_queue,
                                udp_get_stop=udp_get_stop,
                                packet_threshold=packetThreshold,
                                timeusage_threshold=None)

    udp_thread.setDaemon(True)
    udp_thread.setName("UDPGet_Thread")

    print(time.asctime(),'Server started on port', port)
    print('....')
    print('ctrl-c to quit server.')
    try:
        udp_thread.start()

        run = True

        while run:
            packed_count = 0
            while not udp_queue.empty():
                value = udp_queue.get()
                if value is not False:
                    time_tag = datetime.datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
                    with ('{}/{}_packet{}.udp'.format(path,time_tag,packed_count), 'wb') as f:
                        f.write(udp_queue.get())
                        packed_count += 1
                else:
                    run = False

    except KeyboardInterrupt:
        udp_get_stop.set()
        print(time.asctime(),"Server Stopped")
    except:
        udp_get_stop.set()
        print(time.asctime(),"Server Stopped with unknown reasons")

# udp2bin
def udp2bin_tool(udp_folder_Or_list):
    Save = True
    bin_list = []

    if isinstance(udp_folder_Or_list, list):
        udp_files = udp_folder_Or_list
        Save = False
    else:
        UDPfolder = udp_folder_Or_list
        # example pod1-2022-02-21-13-25-03_packet125.udp --> 125
        sort_lambda = lambda x:int(x.split('packet')[-1][:-4])
        udp_files = file_fliter(UDPfolder,'udp', sort_lambda)
        Save = True

    receiver = MicroWaveReceiver()


    for file in udp_files:
        data = None
        if isinstance(file,str):
            filename = os.path.join(UDPfolder, file)
            with open(filename, "rb") as f:
                data = f.read()
        else:
            data = file
        
        receiver.AddBuf(data)
        if receiver.DataFrameReady:
            frame_number = receiver.Header.FrameNumber
            if Save:
                bin_name = os.path.join(UDPfolder, "{}.bin".format(frame_number))
                print(time.asctime(),"Frame {} ready".format(frame_number))
                with open(bin_name, "wb") as bin_file:
                    bin_file.write(receiver.DataFrame)
                    bin_file.close()
            
            bin_list.append((frame_number,receiver.DataFrame))

    return bin_list

def bin2ply_f(folder,file):

    with open(os.path.join(folder,file), "rb") as bin_file:
        print(f"Extracting the data in {file} ...")
        buf = bin_file.read()
        buf_data = MicroWaveParser.ParserFrame(buf)
        for frame in buf_data:
            
            Frame_id = frame[0]
            FrameNumber = frame[1]
            point_cloud = np.array(frame[2])
            if len(point_cloud) == 0 :
                print(f"Notice: Frame {FrameNumber} is empty!")
            else:
                write_ply(point_cloud,folder,str(FrameNumber))

# bin2ply
def bin2ply_tool(folder,nums_worker = 4):
    pool = Pool(processes = nums_worker)

    bin_files = file_fliter(folder,'bin')

    for file in bin_files:
        pool.apply_async(func=bin2ply_f,args=(folder,file,))

    pool.close()
    pool.join()


def plot_ply(path):
    if isinstance(path,str):
        abspath = os.path.abspath(path)
    else:
        raise ValueError("Not a path!")
    
    if os.path.isdir(abspath):
        # open folder to load all ply files
        ply_files = file_fliter(abspath,'ply')
        show = []
        for ply in ply_files:
            pcd = o3d.io.read_point_cloud(os.path.join(abspath,ply))
            show.append(pcd)
    else:
        show = [o3d.io.read_point_cloud(abspath)]

    o3d.visualization.draw_geometries(show)
    return