#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
# @Time    : 2024/9/7 11:49
# @Author  : hjw
# @File    : get_position_test.py
'''
from Vision.camera_coordinate_dete_img import Detection
from Trace.handeye_calibration import *
from Vision.tool.utils import get_disk_space
from Vision.tool.utils import remove_nan_mean_value
from Vision.detect_person import DetectionPerson
import platform
import cv2
import os
#
"""
   测试 Vision
"""
def detectionPosition_test():
    detection = Detection()

    while True:
        ret, img, xyz, nx_ny_nz, box = detection.get_position()
        if ret==1:
            print('xyz点云坐标：', xyz)
            print('nx_ny_nz法向量：', nx_ny_nz)
            print('矩形框四顶点：', box)
            # img = cv2.resize(img,(720, 540))
            if xyz!=None:
                transformation_matrix = R_matrix(521.534, 0.705, 850.03, 0.0, 90.0, 0.0)   # (x, y, z, u, v, w)
                target_position, noraml_base = getPosition(xyz[0], xyz[1], xyz[2], nx_ny_nz[0], nx_ny_nz[1], nx_ny_nz[2], transformation_matrix, box)
                print("target_position:", target_position)
                print("noraml_base", noraml_base)
            cv2.imshow('img', img)
            cv2.waitKey(1)


def detectionPerson_test():
    detectionPerson = DetectionPerson()
    video_path = ''.join([os.getcwd(), '/Vision/model/data/1.mp4'])
    cam = cv2.VideoCapture(video_path)
    while cam.isOpened():
        ret, frame = cam.read()
        if ret:
            person, img = detectionPerson.get_person(frame)
            print(person)
            cv2.imshow('img', img)
            cv2.waitKey(1)
        else:
            break

if __name__ == '__main__':
    detectionPosition_test()