import cv2
import numpy as np
import math

def findtraingle(tri):
    a=tri[0][0]
    b=tri[1][0]
    c=tri[2][0]
    line_ab=math.sqrt(((a[0]-b[0])**2)+((a[1]-b[1])**2))
    line_bc=math.sqrt(((c[0]-b[0])**2)+((c[1]-b[1])**2))
    line_ca=math.sqrt(((a[0]-c[0])**2)+((a[1]-c[1])**2))
    maxline=max(line_ab,line_bc,line_ca)
    if maxline==line_ab:
        ml1=a
        ml2=b
        halfmaxline=(a+b)/2
        peakpoint=c
    elif maxline==line_bc:
        ml1=b
        ml2=c
        halfmaxline=(c+b)/2
        peakpoint=a
    elif maxline==line_ca:
        ml1=c
        ml2=a
        halfmaxline=(c+a)/2
        peakpoint=b
    x1=ml1[0]
    y1=ml1[1]
    x2=ml2[0]
    y2=ml2[1]
    slope=(halfmaxline[0]-peakpoint[0])/(halfmaxline[1]-peakpoint[1])
    alpha=math.atan(slope)
    alpha=math.degrees(alpha)
    if peakpoint[1] > halfmaxline[1]:
        if peakpoint[0] < halfmaxline[0]:
            alpha=180+alpha
        elif peakpoint[0] > halfmaxline[0]:
            alpha=-(180-alpha)

    print("peakpoint[0]:{} halflinemax[0]:{}".format(peakpoint[0],halfmaxline[0]))
    return int(alpha),x1,y1,x2,y2,halfmaxline


def threshbycolor(blur,lower,upper,name):
    if(name=="red"):
        thresh=cv2.inRange(blur,lower,upper)

    thresh=cv2.inRange(blur,lower,upper)

    image, contours, hierarchy = cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)


    cnt = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)

    cv2.drawContours(cnt, contours, -1, (0, 255, 0), 2)

    cv2.imshow(str(name), cnt)
    cv2.imwrite(str(name)+".jpg",cnt)
    point=0
    for i in contours:

        M = cv2.moments(i)
        rect = cv2.minAreaRect(i)
        w,h=rect[1]
        angle = int(rect[2])
        theta=abs(angle)
        if abs(theta) > 180:
            theta = abs(theta)-180
        if w>h:
            theta=90+abs(theta)
        elif h>w:
            theta=theta


        approx = cv2.approxPolyDP(i,0.05*cv2.arcLength(i,True),True) ####epsilo$
        if len(approx)==3:
            shape=("Triangle")
            point=1
            (cen,tri) = cv2.minEnclosingTriangle(i)

            if cv2.contourArea(i)>1000:
                try:
                    alpha,x1,y1,x2,y2,halfmaxline=findtraingle(tri)
                    cv2.circle(copyframe,(tri[0][0][0],tri[0][0][1]), 5, (0,0,255), -1)
                    cv2.circle(copyframe,(tri[1][0][0],tri[1][0][1]), 5, (0,255,255), -1)
                    cv2.circle(copyframe,(tri[2][0][0],tri[2][0][1]), 5, (0,255,0), -1)
                    cv2.drawContours(copyframe,[i],0,255,2)
                    cv2.line(copyframe,(x1,y1),(x2,y2),(255,255,255),3)
                    cv2.circle(copyframe,(halfmaxline[0],halfmaxline[1]), 5, (0,0,0), -1)
                except:
                    pass
        elif len(approx)==4:
            shape=("Rectangle")
            point=1
            if cv2.contourArea(i)>1000:
                cv2.drawContours(copyframe,[i],0,255,2)

        M=cv2.moments(i)
        if (M['m00'] != 0):

            cx = int(M['m10'] / M['m00'])

            cy = int(M['m01'] / M['m00'])

            if point == 1 and cv2.contourArea(i)>1000:
                    point == 0
                    try:
                        if shape=="Rectangle":
                            cv2.putText(copyframe, shape+str(len(approx))+" "+str(cx)+","+str(cy)+name+','+str(theta), (cx - 30, cy - 20),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
                        elif shape=="Triangle":
                            cv2.putText(copyframe, shape+str(len(approx))+" "+str(cx)+","+str(cy)+name+','+str(alpha), (cx - 30, cy - 20),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
                    except:
                        pass

cap = cv2.VideoCapture(1)


lower_red = np.array([131,92,141]) #red
upper_red = np.array([179,255,255])

lower_green = np.array([50,38,169]) #green
upper_green = np.array([82,255,255])

lower_yellow = np.array([18,32,214]) #yellow
upper_yellow = np.array([31,255,255])

lower_blue = np.array([91,93,142]) #blue
upper_blue = np.array([105,255,255])

lower_orange = np.array([3,67,213]) #orange
upper_orange = np.array([19,255,255])

lower_violet = np.array([110,70,171]) #violet
upper_violet = np.array([121,255,255])

while(True):
    ret, frame = cap.read()
    copyframe=frame
    gray = cv2.cvtColor(copyframe, cv2.COLOR_BGR2HSV)
    blurred=cv2.GaussianBlur(gray,(5,5),0)
    threshbycolor(blurred,lower_red,upper_red,"red")
    threshbycolor(blurred,lower_yellow,upper_yellow,"yellow")
    threshbycolor(blurred,lower_green,upper_green,"green")
    threshbycolor(blurred,lower_blue,upper_blue,"blue")
    threshbycolor(blurred,lower_orange,upper_orange,"orange")

    cv2.imshow("out",copyframe)
    cv2.imwrite("out.jpg",copyframe)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()