import cv2
import os
from PiRapidStream import PiRapidStream
import time
import numpy as np


def nothing(x):
    pass

cv2.namedWindow("detection", cv2.WINDOW_AUTOSIZE)
cv2.createTrackbar('thresh','detection',220,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',0,100,nothing)
cv2.createTrackbar('maxg','detection',15,100,nothing)
cv2.createTrackbar('cannylow','detection',50,255,nothing)
cv2.createTrackbar('cannyhigh','detection',100,255,nothing)
cv2.createTrackbar('angelres','detection',45,180,nothing)
###################################################################################################



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 find_squares(gray,rang):
##    gray = cv2.medianBlur(gray, 3)
##    squares = []
##    
##    for thrs in range(rang, rang+1):
##        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) == 4 and cv2.contourArea(cnt) > 1000 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 xrange(4)])
##                if max_cos < 0.1:
##                    squares.append(cnt)
##    return squares

def find_squares(binary):
    squares=[]
    bin, contours, hierarchy = cv2.findContours(binary, 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) == 4 and cv2.contourArea(cnt) > 1000 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 xrange(4)])
            if max_cos < 0.1:
                squares.append(cnt)
    return squares


def find_sqrs(srcc,tresh,minl,maxg,cannylow,cannyhigh,angelres):
    srcc = cv2.Canny(srcc, cannylow, cannyhigh,L2gradient=False)
    lines=cv2.HoughLinesP(srcc,1,np.pi/(angelres*1.0),tresh,minLineLength=minl,maxLineGap=maxg)
    blank=np.zeros((480,640,1),dtype=np.uint8)
    if lines is not None:
        for l in lines:
            cv2.line(blank,(l[0][0],l[0][1]),(l[0][2],l[0][3]),(255),1)
    return blank


def main():

    
    
    vs = PiRapidStream(resolution=(640,480),fps=20,form='yuv',exp=30000,gain=1.15)
    vs.startcapt()
    #lsd=cv2.createLineSegmentDetector()
    while cv2.waitKey(1) != ord("q") and vs.stream_done==False:
        blank=np.zeros((480,640,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

        src = vs.frame
        #filt = cv2.medianBlur(src, 3)
        retval, binary = cv2.threshold(src, rang, 255, cv2.THRESH_BINARY)
        #a=cv2.preCornerDetect(binary,ksize=1,borderType=cv2.BORDER_DEFAULT)
##        lines=lsd.detect(binary)[0]
##        for l in lines:
##            cv2.line(blank,(l[0][0],l[0][1]),(l[0][2],l[0][3]),(255),1)
                
        
        #bin, contours, hierarchy = cv2.findContours(binary.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
        #blank=find_sqrs(binary,thres,minl,maxg,cannylow,cannyhigh,angelres)
        #squares = find_squares(blank)
        #cv2.drawContours( src, squares, -1, 0, 5 )
        #cv2.drawContours( blank, squares, -1, 255, 1 )
        #cv2.imshow('raw',src)
        cv2.imshow('binary analysis', binary)
        cv2.imshow('blank', blank)
    cv2.destroyAllWindows()
    cv2.imwrite('fooo.jpg',binary)
    vs.stopcapt()
    print "quit"

    
    return

###################################################################################################
if __name__ == "__main__":
    main()
