import cv2
import numpy as np
import os
#from picamera.array import PiRGBArray
#from picamera import PiCamera
import time
from PiRapidStream import PiRapidStream
from timeit import default_timer as timer
from TheSquaire import TheSquaire
import pigpio
cv2.namedWindow("pipe", cv2.WINDOW_AUTOSIZE)        
cv2.namedWindow("detection", cv2.WINDOW_AUTOSIZE)
gpio=24
gp=pigpio.pi()
gp.set_mode(gpio,pigpio.OUTPUT)
gp.write(gpio,1)

def nothing(*x):
    pass    

enable_bin=cv2.createTrackbar('enable binarisation','detection',0,1,nothing)
th=cv2.createTrackbar('binary thresh','detection',120,255,nothing)
enable_dilate=cv2.createTrackbar('enable dilate','detection',0,1,nothing)
enable_erode=cv2.createTrackbar('enable erode','detection',0,1,nothing)
enable_median=cv2.createTrackbar('enable median','detection',0,1,nothing)
enable_canny=cv2.createTrackbar('enable canny','detection',0,1,nothing)
dil_kernel=cv2.createTrackbar('dilate kernel','detection',0,14,nothing)
erd_kernel=cv2.createTrackbar('erode kernel','detection',0,14,nothing)
dil_rep=cv2.createTrackbar('dilate reps','detection',1,10,nothing)
erd_rep=cv2.createTrackbar('erode reps','detection',1,10,nothing)
median_kernel=cv2.createTrackbar('median kernel','detection',0,14,nothing)
cannylow=cv2.createTrackbar('cannylow','detection',120,1024*4,nothing)
cannyhigh=cv2.createTrackbar('cannyhigh','detection',120,1024*4,nothing)
aperture=cv2.createTrackbar('cannyaperture','detection',1,14,nothing)

###################################################################################################


def main():
    ts=TheSquaire()
    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:
        enable_bin=cv2.getTrackbarPos('enable binarisation','detection')
        th=cv2.getTrackbarPos('binary thresh','detection')
        enable_dilate=cv2.getTrackbarPos('enable dilate','detection')
        enable_erode=cv2.getTrackbarPos('enable erode','detection')
        enable_median=cv2.getTrackbarPos('enable median','detection')
        enable_canny=cv2.getTrackbarPos('enable canny','detection')
        dil_kernel=cv2.getTrackbarPos('dilate kernel','detection')
        erd_kernel=cv2.getTrackbarPos('erode kernel','detection')
        dil_rep=cv2.getTrackbarPos('dilate reps','detection')
        erd_rep=cv2.getTrackbarPos('erode reps','detection')
        median_kernel=cv2.getTrackbarPos('median kernel','detection')
        cannylow=cv2.getTrackbarPos('cannylow','detection')
        cannyhigh=cv2.getTrackbarPos('cannyhigh','detection')
        aperture=cv2.getTrackbarPos('cannyaperture','detection')

        blank=np.zeros((512,576,1),dtype=np.uint8)

        start=timer()
        
        pipe=vs.frame
        img=pipe

        if enable_bin :
            retval, pipe = cv2.threshold(pipe, th, 255, cv2.THRESH_BINARY_INV)

        if enable_dilate:
            for i in xrange(dil_rep):
                pipe = cv2.dilate(pipe, np.ones((dil_kernel*2+1,dil_kernel*2+1),np.uint8))

        if enable_erode:
            for i in xrange(erd_rep):
                pipe = cv2.erode(pipe, np.ones((erd_kernel*2+1,erd_kernel*2+1),np.uint8))

        if enable_median :
            pipe = cv2.medianBlur(pipe,median_kernel*2+1)

        if enable_canny :
            pipe = cv2.Canny(pipe, cannylow, cannyhigh,apertureSize=aperture*2+1,L2gradient=False)

###################################
###Test Algo######################

        r=ts.rechtecke(src=pipe,th=th)
        
        
#################################
        end=timer()
        ts.zeichne_rechtecke(r,masking=True,mask=[blank])
        mean=cv2.bitwise_and(img,img,mask=blank)
        cv2.imshow("pipe", pipe)
        cv2.imshow("recs",mean)

        if cv2.waitKey(1) == ord("f"):
            print('Zykuluszeit: {} Sekunden'.format(end-start))
            print('Theoretische FPS: {}'.format(1/(end-start)))
        
  
    
    cv2.destroyAllWindows()
    gp.set_mode(gpio,pigpio.INPUT)
    vs.stopcapt()
    print "quit"

    return

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