import numpy as np
from timeit import default_timer as timer
import cv2
from PiRapidStream import PiRapidStream

def nothing(*x):
    pass

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) ) )


cv2.namedWindow("detection", cv2.WINDOW_AUTOSIZE)
cv2.createTrackbar('thresh','detection',215,255,nothing)
cv2.createTrackbar('exposure','detection',30000,100000,nothing)
cv2.createTrackbar('gain','detection',115,800,nothing)
cv2.createTrackbar('iso','detection',800,800,nothing)
cv2.createTrackbar('thres','detection',0,255,nothing)
cv2.createTrackbar('minl','detection',5,100,nothing)
cv2.createTrackbar('maxg','detection',10,100,nothing)
cv2.createTrackbar('cannylow','detection',50,255,nothing)
cv2.createTrackbar('cannyhigh','detection',100,255,nothing)
cv2.createTrackbar('angelres','detection',45,180,nothing)
vs = PiRapidStream(resolution=(576,512),fps=30,form='yuv',exp=30000,gain=1.15)
vs.startcapt()


while cv2.waitKey(1) != ord("q") and vs.stream_done==False:

    conties=np.empty((100),dtype=object)
    blank=np.zeros((512,576,1),dtype=np.uint8)
    rang=cv2.getTrackbarPos('thresh','detection')
    exp=cv2.getTrackbarPos('exposure','detection')
    g=cv2.getTrackbarPos('gain','detection')/100.0
    iso=cv2.getTrackbarPos('iso','detection')
    thres=cv2.getTrackbarPos('thres','detection')
    angelres=cv2.getTrackbarPos('angelres','detection')
    minl=cv2.getTrackbarPos('minl','detection')
    maxg=cv2.getTrackbarPos('maxg','detection')
    cannylow=cv2.getTrackbarPos('cannylow','detection')
    cannyhigh=cv2.getTrackbarPos('cannyhigh','detection')
    vs.camera.shutter_speed=exp
    vs.camera.awb_gains=g
    vs.camera.iso=iso



    #print conties
    src = vs.frame
    retval, binary = cv2.threshold(src, rang, 255, cv2.THRESH_BINARY_INV)
    bin,cont, hierarchy = cv2.findContours(binary.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
    i=0
    for  cnt in cont:
        
        cnt_len = cv2.arcLength(cnt, True)
        cnt = cv2.approxPolyDP(cnt, minl/100.0*cnt_len, True)
        #cnt=cv2.convexHull(cnt)
        if len(cnt)>3 and len(cnt)<maxg and cv2.contourArea(cnt) > 20000:

            rect=cv2.minAreaRect(cnt)
            box=np.int0(cv2.boxPoints(rect))
            conties[i]=box
            i+=1
    
    
            

    cv2.drawContours( blank, conties, -1, 128, 3 )
    cv2.imshow('binary analysis', binary)
    cv2.imshow('blank', blank)






cv2.destroyAllWindows()

vs.stopcapt()
print "quit"
