def angle_cos(p0, p1, p2):
    d1, d2 = (p0-p1).astype('float'), (p2-p1).astype('float')
    return abs( np.dot(d1, d2) / np.sqrt( np.dot(d1, d1)*np.dot(d2, d2) ) )

def getCircles(img):#TEST THIS FUNCTION
     gray= cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
     circles = cv2.HoughCircles(gray, cv2.cv.CV_HOUGH_GRADIENT, 1.2, 100)
     return circles





def getSquares(img, num_angles=4, MinContourAre=1000):
    img = cv2.GaussianBlur(img, (5, 5), 0)
    squares = []
    for gray in cv2.split(img):
        for thrs in range(0, 255, 26):
            if thrs == 0:
                bin = cv2.Canny(gray, 0, 50, apertureSize=5)
                bin = cv2.dilate(bin, None)
            else:
                _retval, bin = cv2.threshold(gray, thrs, 255, cv2.THRESH_BINARY)
            bin, contours, _hierarchy = cv2.findContours(bin, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
            for cnt in contours:
                cnt_len = cv2.arcLength(cnt, True)
                cnt = cv2.approxPolyDP(cnt, 0.02*cnt_len, True)
                if len(cnt) == num_angles and cv2.contourArea(cnt) > MinContourArea and cv2.isContourConvex(cnt):
                    cnt = cnt.reshape(-1, 2)
                    max_cos = np.max([angle_cos( cnt[i], cnt[(i+1) % 4], cnt[(i+2) % 4] ) for i in range(4)])
                    if max_cos < 0.1:
                        squares.append(cnt)
    return squares
def getTriangles(img):#TEST THIS FUNCTION
     return getSquares(img,3)


import os
IMAGE_SHAPE=(360,640,3)
os.chdir('C:\\Users\\user\\Downloads\\fromcamera')

import cv2
import numpy as np
from copy import deepcopy
##[:,:,::-1] надо, чтобы на первом месте был красный, на втором зеленый и на третьем синий
yellow_square = cv2.imread('yellow.jpg')[:,:,::-1]
green_square = cv2.imread('green.jpg')[:,:,::-1]
yellow_triangle = cv2.imread('yellowtriangle.jpg')[:,:,::-1]
green_triangle = cv2.imread('greentriangle.jpg')[:,:,::-1]
yellow_circle = cv2.imread('yellowcircle.jpg')[:,:,::-1]
red_circle = cv2.imread('redcircle.jpg')[:,:,::-1]
green_circle = cv2.imread('greencircle.jpg')[:,:,::-1]

def inRange(image, color):
    threshs = [cv2.inRange(image[:,:,i], color[0][i], color[1][i]).astype(bool) for i in range(3)]
    mask = np.logical_and(threshs[0],threshs[1], threshs[2])
    return mask
def getRedboxCentre (image,color,image_name='gg.jpg',mask_color=[0,0,255],min_d_area=10):
     mask = inRange(image, color)
    if mask.sum()>min_d_area:
        print('Mask is found - area is '+str(mask.sum()))
        cv2.imwrite('mask_'+image_name,255*mask.astype(int))
        image1 = deepcopy(image)
        image1[mask]=mask_color
        cv2.imwrite(image_name, image)
    return 0

red_colormin =(95,20,20)
red_colormax=(110,33,33)
yellow_colormin=(125,135,75)
yellow_colormax=(145,155,95)
green_colormin=(13,57,57)
green_colormax=(35,85,85)
colors = [(red_colormin,red_colormax),(yellow_colormin,yellow_colormax),(green_colormin,green_colormax)]
color_names=['red','yellow','green']
red_images,red_names=[red_circle],['redcircle']#We consider brown-red color used to be some sort of ted
yellow_images,yellow_names = [yellow_circle, yellow_triangle, yellow_square],['yellowcircle','yellowtriangle','yellowsquare']
green_images, green_names=[green_circle, green_triangle, green_square],['greencircle','greentriangle','greensquare']
data_struct=[[red_images,red_names],[yellow_images, yellow_names],[green_images,green_names]]


def isPinRectangle(r, P): """ r: A list of four points, each has a x- and a y- coordinate P: A point """
        areaRectangle = 0.5*abs( # y_A y_C x_D x_B (r[0][1]-r[2][1])*(r[3][0]-r[1][0])
        # y_B y_D x_A x_C + (r[1][1]-r[3][1])*(r[0][0]-r[2][0]) )
        ABP = 0.5*( r[0][0]*(r[1][1]-r[2][1]) +r[1][0]*(r[2][1]-r[0][1]) +r[2][0]*(r[0][1]-r[1][1]) )
        BCP = 0.5*( r[1][0]*(r[2][1]-r[3][1]) +r[2][0]*(r[3][1]-r[1][1]) +r[3][0]*(r[1][1]-r[2][1]) )
        CDP = 0.5*( r[2][0]*(r[3][1]-r[0][1]) +r[3][0]*(r[0][1]-r[2][1]) +r[0][0]*(r[2][1]-r[3][1]) )
        DAP = 0.5*( r[3][0]*(r[0][1]-r[1][1]) +r[0][0]*(r[1][1]-r[3][1]) +r[1][0]*(r[3][1]-r[0][1]) )
        return areaRectangle == (ABP+BCP+CDP+DAP)

def is_into(point, contour, type_name):
     if type_name=='circle':
          (x,y,r)= contour
           cond = (point[0]-x)**2 +(point[1]-y)**2 < r**2)
           return cond
     elif type_name=='rectangle' or type_name=='triangle):
             val= cv2.pointPolygonTest(contour, point)
            return val==1
            #MAKE CONDITION FOR RECTANGLE
     else:
           raise Exception("Unknown type")




            #MAKE CONDITION FOR TRIANGLE
     else:
           raise Exception('Unknown type')
     pass
def get_percent(contour, mask, type_name):
      total, match=0.0,0.0
      for i in range(IMAGE_SHAPE[0]):
            for j in range(IMAGE_SHAPE[1]):
                     if is_into ((i,j), contour, type_name):
                         total+=1
                         if mask[i][j]:
                               match+=1
       if total==0:
             return 0
       else:
             return match/total
import itertools
data={}
for (images,names) in data_struct:
    for (image,name) in zip(images,names):
        circle_contours = getCircles(image)
        rectangle_contours = getRectangles(image)
        triangle_contours = getTriangles(image)
        mask_list, mask_names = [ inRange(image, color) for color in colors], colors
        contour_list, contour_names=(
        [circle_contours, rectangle_contours, triangle_contours],['circle','rectangle','triangle'])

        for contours, contour_name in zip(contour_list, contour_names):
              for mask, mask_name in zip(mask_list, mask_names):
                     total_name = '_'.join(contour_name,mask_name,name)
                     percents = [get_percent(contour, mask, total_name) for contour in contours]
                     max_percent, final_contour= max(percents),  contours[np.argmax(percents)]
                     data[total_name]=(max_percent, final_contour,image)

###filter data
MIN_MATCH=0.8
FILL_COLOR=[0,255,255]
for key in range(data.keys():
     image=data[key][2]
     contour=data[key][1]
     if data[key][0]<MIN_MATCH:
          del data[key]
     else:
          mode = key.split('_')[0]
          save_name ='_'.join(key)+'.jpg'
          image1=deepcopy(image)
          for i,j in itertools.product(IMAGE_SHAPE[0],IMAGE_SHAPE[1]):
                if is_into((i,j), contour,mode):
                     image1[i,j]=FILL_COLOR
          cv2.imwrite(save_name, image1)



def filter_squares(img):

    for square in squares:

        x_min = min([j[0] for j in squares])

        x_max = max([j[0] for j in squares])

        y_min = min([j[1] for j in squares])

        y_max = max(j[1] for j in squares)

squares = find_squares(yellow_square)

cv2.drawContours( yellow_square, squares, -1, (0, 255, 0), 1 )

cv2.imwrite('tmp2.jpg',yellow_square)