#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
# @Time    : 2024/12/26 16:24
# @Author  : hjw
# @File    : Mechanical_arm_error_test.py.py
'''
from camera_coordinate_dete import Detection
from Trace.handeye_calibration import *
from Vision.tool.CameraPe_depth2color import camera_pe as camera_pe_depth2color
import cv2

def dete_center_point_circle(img):
    gay_img = cv2.cvtColor(img, cv2.COLOR_BGRA2GRAY)
    img = cv2.medianBlur(gay_img, 7)  # 进行中值模糊，去噪点
    # 要对图像进行滤波去除噪点并转为灰度图像，滤波方法视图像而定
    circles = cv2.HoughCircles(img, cv2.HOUGH_GRADIENT, 1, 60, param1=100, param2=30, minRadius=0, maxRadius=0)
    #  dp: 图像解析的反向比例。1为原始大小，2为原始大小的一半
    #  minDist: 圆心之间的最小距离。过小会增加圆的误判，过大会丢失存在的圆
    #  param1: Canny检测器的高阈值
    #  param2: 检测阶段圆心的累加器阈值。越小的话，会增加不存在的圆；越大的话，则检测到的圆就更加接近完美的圆形
    #  minRadius: 检测的最小圆的半径
    #  maxRadius: 检测的最大圆的半径

    print(np.shape(circles))
    circles = np.uint16(np.around(circles))
    print(circles)

    for i in circles[0, :]:  # 遍历矩阵每一行的数据
        cv2.circle(img, (i[0], i[1]), i[2], (0, 255, 0), 2)
        cv2.circle(img, (i[0], i[1]), 2, (0, 0, 255), 3)
        print('圆心：', (i[0], i[1]))
    cv2.imshow('circle', img)
    return i[0], i[1]

detection = Detection(cameraType = 'Pe')
camera = camera_pe_depth2color()

while True:
    ret, img, pm, _depth_align = camera.get_img_and_point_map()
    if ret == 1:
        center_x, center_y = dete_center_point_circle(img)
        point_x, point_y, point_z = pm[center_y, center_x]

        # print('xyz点云坐标：', xyz)
        # print('nx_ny_nz法向量：', nx_ny_nz)
        # print('矩形框四顶点：', box)
        # img = cv2.resize(img,(720, 540))
        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(point_x, point_y, point_z, 1, 1, 1, transformation_matrix, None)
        print("target_position:", target_position)
        print("noraml_base", noraml_base)
        cv2.imshow('img', img)
        cv2.waitKey(0)