import cv2
import time
import numpy as np
from rknnpool import rknnPoolExecutor
# 图像处理函数，实际应用过程中需要自行修改
from func import myFunc
from PointList import PointList
from Point2bird import Point2bird 
import json
from send_json import J_send

cap = cv2.VideoCapture('./out.mp4')
# cap = cv2.VideoCapture(0)
modelPath = "./rknnModel/best.rknn"
# 线程数, 增大可提高帧率
TPEs = 3

point_list = PointList()
P2d = Point2bird()
Json_send = J_send()

HOST, PORT = "192.168.8.112", 8000

json_filepath="xiao.json"
with open(json_filepath,"r")as jsonfile:
    load_data=json.load(jsonfile)
    car_points = np.array(load_data['cars'])

global tmp


if __name__ == '__main__':
    tmp = []
    # 初始化rknn池
    pool = rknnPoolExecutor(
        rknnModel=modelPath,
        TPEs=TPEs,
        func=myFunc)

    # 初始化异步所需要的帧
    if (cap.isOpened()):
        for i in range(TPEs + 1):
            ret, frame = cap.read()
            if not ret:
                cap.release()
                del pool
                exit(-1)
            pool.put(frame)

    frames, loopTime, initTime = 0, time.time(), time.time()
    while (cap.isOpened()):
        frames += 1
        if(frames % 25 == 0):
            ret, frame = cap.read()
            if not ret:
                break
            points = pool.put(frame)
            frame, flag = pool.get()#获取myFunc函数的返回值
            new_points = []
            print("frame\n",frame)
            for i in range(len(frame)):
                temp_point = P2d.translateaxi(np.array([frame[i][0], frame[i][1]]).reshape(1, 2))
                new_points.append([temp_point[0][0],temp_point[1][0]])
            # print("*----------*",new_points)
            res = P2d.solve2(new_points)
            
            temp_points = []
            for a in range(len(res)):
                temp_points.append([res[a][0][0],res[a][0][1]+np.sqrt(res[a][0][1])])
            # print("temp_points",temp_points)
            temp_points = sorted(temp_points,key = lambda point:point[0],reverse= False)
            if flag == False:
                break
            if(frames == 25):
                point_list.baseline = temp_points
                tmp = temp_points
                point_list.old2new_list(temp_points)
                result_json={
                   "points":temp_points,
                    "car":car_points.tolist() 
                }
                jmsg = json.dumps(result_json)
                Json_send.client(HOST,PORT,jmsg)
            else:
                # x,y = point_list.get_xy_change(temp_points) 
                print("point_list.baseline\n",point_list.baseline)
                x,y = point_list.old2new_points(tmp,temp_points)
                tmp = temp_points
                sorted_points = sorted(point_list.baseline,key = lambda point:point[1],reverse= True)
                tail = sorted_points[:15]
                print("sorted_points\n",sorted_points)
                for i in range(4):
                    car_points[i][1] += y
                    car_points[i][0] += x
                result_json={
                   "points":tail,
                    "car":car_points.tolist()
                }
                jmsg = json.dumps(result_json)
                Json_send.client(HOST,PORT,jmsg)
            if frames % 30 == 0:
                print("帧数:\t", 30 / (time.time() - loopTime), "帧")
                loopTime = time.time()
    # 释放cap和rknn线程池
    cap.release()
    pool.release()