import sys

import cv2

import math

import time

import threading

import numpy as np

import HiwonderSDK.yaml_handle as yaml_handle

if sys.version_info.major == 2:

     print('Please run this program with python3!')

     sys.exit(0)

range_rgb = {

     'red':   (0, 0, 255),

     'blue':  (255, 0, 0),

     'green': (0, 255, 0),

     'black': (0, 0, 0),

     'white': (255, 255, 255)}

__target_color = ('red', 'green', 'blue')

lab_data = yaml_handle.get_yaml_data(yaml_handle.lab_file_path)

# 找出面积最大的轮廓

# 参数为要比较的轮廓的列表

def getAreaMaxContour(contours):

     contour_area_temp = 0

     contour_area_max = 0

     area_max_contour = None

     for c in contours:  # 历遍所有轮廓

         contour_area_temp = math.fabs(cv2.contourArea(c))  # 计算轮廓面积

         if contour_area_temp > contour_area_max:

             contour_area_max = contour_area_temp

             if contour_area_temp > 300:  # 只有在面积大于 300 时，最大面积的轮廓才是有效的，以过滤干扰

                 area_max_contour = c

     return area_max_contour, contour_area_max  # 返回最大的轮廓

detect_color = None

color_list = []

start_pick_up = False

size = (640, 480)

def run(img):

     global rect

     global detect_color

     global start_pick_up

     global color_list

     img_copy = img.copy()

     frame_resize = cv2.resize(img_copy, size, interpolation=cv2.INTER_NEAREST)

     frame_gb = cv2.GaussianBlur(frame_resize, (3, 3), 3)

     frame_lab = cv2.cvtColor(frame_gb, cv2.COLOR_BGR2LAB)  # 将图像转换到 LAB 空间

     color_area_max = None

     max_area = 0

     areaMaxContour_max = 0

     if not start_pick_up:

         for i in lab_data:

             if i in __target_color:

                 frame_mask = cv2.inRange(frame_lab,

                                              (lab_data[i]['min'][0],

                                               lab_data[i]['min'][1],

                                               lab_data[i]['min'][2]),

                                              (lab_data[i]['max'][0],

                                               lab_data[i]['max'][1],

                                               lab_data[i]['max'][2]))  # 对原图像和掩模进行位运算

                 opened = cv2.morphologyEx(frame_mask, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))  # 开运算

                 closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8))  # 闭运算

                 contours =跟单网gendan5.com cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)[-2]  # 找出轮廓

                 areaMaxContour, area_max = getAreaMaxContour(contours)  # 找出最大轮廓

                 if areaMaxContour is not None:

                     if area_max > max_area:  # 找最大面积

                         max_area = area_max

                         color_area_max = i

                         areaMaxContour_max = areaMaxContour

         if max_area > 500:  # 有找到最大面积

             rect = cv2.minAreaRect(areaMaxContour_max)

             box = np.int0(cv2.boxPoints(rect))

             y = int((box[1][0]-box[0][0])/2+box[0][0])

             x = int((box[2][1]-box[0][1])/2+box[0][1])

             print('X:',x,'Y:',y) # 打印坐标

             cv2.drawContours(img, [box], -1, range_rgb[color_area_max], 2)

             if not start_pick_up:

                 if color_area_max == 'red':  # 红色最大

                     color = 1

                 elif color_area_max == 'green':  # 绿色最大

                     color = 2

                 elif color_area_max == 'blue':  # 蓝色最大

                     color = 3

                 else:

                     color = 0

                 color_list.append(color)

                 if len(color_list) == 3:  # 多次判断

                     # 取平均值

                     color = int(round(np.mean(np.array(color_list))))

                     color_list = []

                     if color == 1:

                         detect_color = 'red'

                     elif color == 2:

                         detect_color = 'green'

                     elif color == 3:

                         detect_color = 'blue'

                     else:

                         detect_color = 'None'

##    cv2.putText(img, "Color: " + detect_color, (10, img.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.65, detect_color, 2)

     return img

if __name__ == '__main__':

     cap = cv2.VideoCapture(-1) # 读取摄像头

     __target_color = ('red',)

     while True:

         ret, img = cap.read()

         if ret:

             frame = img.copy()

             Frame = run(frame)           

             cv2.imshow('Frame', Frame)

             key = cv2.waitKey(1)

             if key == 27:

                 break

         else:

             time.sleep(0.01)

     cv2.destroyAllWindows()