#! /usr/bin/python3
import time
from multiprocessing import Queue
from typing import get_origin
from VIDEO_FORM import VIDEO_CONFI
from SOCKET import SOCKET_CLIENT_INIT, SEND_VIDEO
from RGBD_SENDER import RGBD_CAP,RGBD_INIT
from threading import Thread
import numpy as np

def PutIMG():
    global rgbd_queue,rgbd_confi
    color_confi = VIDEO_CONFI(shape=(480,640,3),
                    dWidth=8,
                    dType='np.uint8',
                    fps=24)
    
    depth_confi = VIDEO_CONFI(shape=(480,640,1),
                    dWidth=8,
                    dType='np.uint8',
                    fps=24)
    color_stream, depth_stream = RGBD_INIT()
    while 1:            
        image_RGBD = RGBD_CAP(color_stream, depth_stream, color_confi, depth_confi,rgbd_confi)

        try:
            rgbd_queue.put(image_RGBD.tobytes(),block=True,timeout=2)
        except:
            print('消息队列已经满，现在有消息数量：%s'%(rgbd_queue.qsize()))
            print('~~~~~~~~~~~~~~~~~~~~~~')

def SentIMG():
    global rgbd_queue,rgbd_confi
    while 1:
        state,conn = SOCKET_CLIENT_INIT('10.20.70.181', 6666)
        if state:
            break
        time.sleep(1)
    #SEND_VIDEO(conn,rgbd_confi,rgbd_queue)

    period = 1.0/rgbd_confi.fps
    while 1:
        time_before = time.perf_counter()
        
        img = (np.ones(rgbd_confi.shape,dtype=eval(rgbd_confi.dType ))).tobytes()
        print (len(img))
        try:
            img = rgbd_queue.get(block=True,timeout=2)
        except:
            print('rgbd_queue is EMPTY')
            pass
        #SEND_IMG(conn,confi,img)

        conn.send(img)

        while (time.perf_counter() - time_before) < period:
            time.sleep(0.001) 
     


if __name__ == '__main__':    
    global rgbd_confi,rgbd_queue

    rgbd_confi = VIDEO_CONFI(shape=(480,640,4),
                dWidth=8,
                dType='np.uint8',
                fps=24)

    rgbd_queue = Queue(3)

    putImg = Thread(target=PutIMG)
    putImg.start()

    sentImg = Thread(target=SentIMG)
    sentImg.start()


    # while 1:
    #     state,conn = SOCKET_CLIENT_INIT('10.20.70.181', 6666)
    #     if state:
    #         break
    #     time.sleep(1)
    
    #client_confi = VIDEO_CONFI(shape=(640,480,3),
    #                dWidth=8,
    #                dType='uint8',
    #                fps=24)


    
    #SEND_VIDEO(conn,rgbd_confi,rgbd_queue)