#!/usr/bin/env python
# -*- coding: UTF-8 -*-
'''
@Project ：AutoControlSystem-master 
@File    ：camera_coordinate_dete.py
@IDE     ：PyCharm 
@Author  ：hjw
@Date    ：2024/8/27 14:24 
'''

import numpy as np
import cv2
import open3d as o3d
from .tool.CameraRVC import camera
from .yolo.yolov8_pt_seg import yolov8_segment


class Detection_Position:

    def __init__(self, model_path, device):
        self.camera_rvc = camera()
        self.model = yolov8_segment()
        self.model.load_model(model_path, device)
        if self.camera_rvc.caminit_isok == True:
            print("RVC X Camera is opened!")
        else:
            print("RVC X Camera is not opened!")

    def get_position(self):
        ""
        '''
        :param api: None
        :return: ret , img, (x,y,z), (nx, ny, nz)
        '''
        ret, img, pm = self.camera_rvc.get_img_and_point_map()            # 拍照，获取图像及
        if self.camera_rvc.caminit_isok == True:
            if ret == 1:
                flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0)
                if flag == 1:
                    xyz = []
                    nx_ny_nz = []
                    for i, item in enumerate(det_cpu):

                        # 画box
                        box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32)
                        label = category_names[int(item[5])]
                        rand_color = (0, 255, 255)
                        score = item[4]
                        org = (int((box_x1 + box_x2) / 2), int((box_y1 + box_y2) / 2))
                        x_center = int((box_x1 + box_x2) / 2)
                        y_center = int((box_y1 + box_y2) / 2)
                        text = '{}|{:.2f}'.format(label, score)
                        cv2.putText(img, text, org=org, fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8,
                                    color=rand_color,
                                    thickness=2)
                        # 画mask
                        # mask = masks[i].cpu().numpy().astype(int)
                        mask = masks[i].cpu().data.numpy().astype(int)
                        # mask = masks[i].numpy().astype(int)
                        bbox_image = img[box_y1:box_y2, box_x1:box_x2]
                        h, w = box_y2 - box_y1, box_x2 - box_x1
                        mask_colored = np.zeros((h, w, 3), dtype=np.uint8)
                        mask_colored[np.where(mask)] = rand_color
                        ##################################
                        imgray = cv2.cvtColor(mask_colored, cv2.COLOR_BGR2GRAY)
                        # cv2.imshow('mask',imgray)
                        # cv2.waitKey(1)
                        # 2、二进制图像
                        ret, binary = cv2.threshold(imgray, 10, 255, 0)
                        # 阈值 二进制图像
                        # cv2.imshow('bin',binary)
                        # cv2.waitKey(1)
                        contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
                        # all_piont_list = contours_in(contours)
                        # print(len(all_piont_list))
                        max_contour = None
                        max_perimeter = 0
                        for contour in contours:                          # 排除小分割区域或干扰区域
                            perimeter = cv2.arcLength(contour, True)
                            if perimeter > max_perimeter:
                                max_perimeter = perimeter
                                max_contour = contour
                        '''
                        提取区域范围内的（x, y）
                        '''
                        mask_inside = np.zeros(binary.shape, np.uint8)
                        cv2.drawContours(mask_inside, [max_contour], 0, 255, -1)
                        pixel_point2 = cv2.findNonZero(mask_inside)
                        # result = np.zeros_like(color_image)
                        select_point = []
                        for i in range(pixel_point2.shape[0]):
                            # k = pixel_point2[i]
                            select_point.append(pm[pixel_point2[i][0][1], pixel_point2[i][0][0]])
                        select_point = np.array(select_point)
                        pm_seg = select_point.reshape(-1, 3)
                        pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=-1), :]      # 剔除 nan
                        # cv2.imshow('result', piont_result)

                        '''
                        拟合平面，计算法向量
                        '''
                        pcd = o3d.geometry.PointCloud()
                        pcd.points = o3d.utility.Vector3dVector(pm_seg)
                        # pcd = o3d.io.read_point_cloud("./Data/seg_point.xyz")
                        plane_model, inliers = pcd.segment_plane(distance_threshold=0.1,
                                                                 ransac_n=3,
                                                                 num_iterations=100)
                        [a, b, c, d] = plane_model
                        print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

                        # inlier_cloud = pcd.select_by_index(inliers)                # 点云可视化
                        # inlier_cloud.paint_uniform_color([1.0, 0, 0])
                        # outlier_cloud = pcd.select_by_index(inliers, invert=True)
                        # outlier_cloud.paint_uniform_color([0, 1, 0])
                        # o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])

                        '''
                         拟合最小外接矩形，计算矩形中心
                        '''

                        rect = cv2.minAreaRect(max_contour)
                        # cv2.boxPoints可以将轮廓点转换为四个角点坐标
                        box = cv2.boxPoints(rect)
                        # 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针
                        startidx = box.sum(axis=1).argmin()
                        box = np.roll(box, 4 - startidx, 0)
                        # 在原图上画出预测的外接矩形
                        box = box.reshape((-1, 1, 2)).astype(np.int32)
                        box = box + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]]]

                        x_rotation_center = int((box[0][0][0] + box[1][0][0] + box[2][0][0] + box[3][0][0]) / 4)
                        y_rotation_center = int((box[0][0][1] + box[1][0][1] + box[2][0][1] + box[3][0][1]) / 4)
                        point_x, point_y, point_z =pm[x_rotation_center, y_rotation_center]
                        cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5)  # 标出中心点
                        x1, y1, z1, = pm[x_rotation_center+1, y_rotation_center+1]
                        print('x1 y1 z1 :', x1, y1, z1)
                        #print('像素坐标（x, y）：', x_rotation_center,y_rotation_center)
                        print('x y z :', point_x, point_y, point_z)
                        # point_x=point_x*1000
                        # point_y=point_y*1000
                        # point_z=point_z*1000
                        print('nx ny nz :', a, b, c)
                        # getPosition(x, y, z, a, b, c)
                        if(point_x == np.nan):
                            continue
                        else:
                            xyz.append([x1*1000, y1*1000, z1*1000])
                            nx_ny_nz.append([a, b, c])
                        cv2.polylines(img, [box], True, (0, 255, 0), 2)

                    return 1, img, xyz, nx_ny_nz
                else:
                    return 1, img, None, None

            else:
                print("RVC X Camera capture failed!")
                return 0, None, None, None

        else:
            print("RVC X Camera is not opened!")
            return 0, None, None, None



