from __future__ import print_function
from __future__ import division
from time import sleep
import numpy as np
import pigpio
import cv2
from sys import exit
from multiprocessing.dummy import Pool
from timeit import default_timer as timer
import matplotlib.pyplot as plt
import heapq
from PiRapidStream import PiRapidStream

PWM_freq=28000
PWM_GPIO = 18
thresh=80
cv2.namedWindow("detection", cv2.WINDOW_AUTOSIZE)
pi = pigpio.pi()
vs = PiRapidStream(resolution=(576,512),fps=40,form='yuv',exp=30000,gain=1.15)
last_duty=0


def transition(new):
    global last_duty
    delta=new-last_duty
    if delta is not 0:
        for i in range(last_duty,new,2*cmp(delta,0)):
            pi.hardware_PWM(PWM_GPIO,PWM_freq,i*10000)
            sleep(0.008)

        pi.hardware_PWM(PWM_GPIO,PWM_freq,new*10000)
        last_duty=new
    pass


def findsharpness_prior(src):
    src=cv2.bilateralFilter(src,3,6,1.5)
    return src

def findsharpness_post(src):
   
    dx = cv2.Sobel(src, cv2.CV_16S,1,0, ksize=1)
    dy = cv2.Sobel(src, cv2.CV_16S,0,1, ksize=1)
    gradient = np.arctan2(dy,dx) * 180 / np.pi
    dx=np.absolute(dx)
    dy=np.absolute(dy)
    dx=np.uint8(dx)
    dy=np.uint8(dy)
    accu=cv2.addWeighted(dx,0.5,dy,0.5,0,-1)

    th,accu=cv2.threshold(accu,thresh,0,cv2.THRESH_TOZERO)
    
    return int(cv2.sumElems(accu)[0])

def findsharpness(src):
    src=cv2.bilateralFilter(src,3,6,1.5)
    dx = cv2.Sobel(src, cv2.CV_16S,1,0, ksize=1)
    dy = cv2.Sobel(src, cv2.CV_16S,0,1, ksize=1)
    gradient = np.arctan2(dy,dx) * 180 / np.pi
    dx=np.absolute(dx)
    dy=np.absolute(dy)
    dx=np.uint8(dx)
    dy=np.uint8(dy)
    accu=cv2.addWeighted(dx,0.5,dy,0.5,0,-1)

    th,accu=cv2.threshold(accu,thresh,0,cv2.THRESH_TOZERO)
    
    return int(cv2.sumElems(accu)[0])

def sumelem(src):
    return cv2.sumElems(src)[0]

def slicer(src):
    size=src.shape[0:2:1]
    return (src[0:size[0]//2,0:size[1]//2],
            src[0:size[0]//2,size[1]//2:size[1]],
            src[size[0]//2:size[0],0:size[1]//2],
            src[size[0]//2:size[0],size[1]//2:size[1]])

p=Pool(processes=4)



def global_search(t=0.02):
    global last_duty
    transition(0)
    shots=[]
    src=vs.frame
    for i in xrange(4,104,4):
        #start=timer()
        pi.hardware_PWM(PWM_GPIO,PWM_freq,i*10000)
        shots.append(findsharpness_prior(src))
        #shots.append(findsharpness(src))
        sleep(t)
        #end=timer()
        #print(end-start)
        src=vs.frame
    shots.append(findsharpness_prior(src))
    #shots.append(findsharpness(src))

    last_duty=i
    return (p.map(findsharpness_post,shots))
    #return(shots)

def globalwichs_search(t=0):
    global last_duty
    transition(0)
    shots=[]
    src=vs.frame
    for i in xrange(4,104,4):
        #start=timer()
        pi.hardware_PWM(PWM_GPIO,PWM_freq,i*10000)
        shots.append(findsharpness_prior(src))
        #shots.append(findsharpness(src))
        sleep(t)
        #end=timer()
        #print(end-start)
        src=vs.frame
    shots.append(findsharpness_prior(src))
    #shots.append(findsharpness(src))

    last_duty=i
    return (p.map(findsharpness_post,shots))
    #return(shots)

##def fine_search(center):
##    global last_duty
##    shots=[]
##    steps=[(x+center) for x in xrange(-5,11,1) if (x+center)>=0 and (x+center)<=100]
##    print(steps)
##    transition(steps[0])
##    sleep(1)
##    src=vs.frame
##    for i in steps:
##        pi.hardware_PWM(PWM_GPIO,PWM_freq,i*10000)
##        sleep(0.1)
##        shots.append(findsharpness_prior(src))
##        src=vs.frame
##    shots.append(findsharpness_prior(src))
##
##    last_duty=i
##    return (p.map(findsharpness_post,shots))


def fullscan(step):
    global last_duty
    transition(0)
    shots=[]
    t=0
    if step==1:
        t=0.5
    src=vs.frame
    for i in xrange(step,100+step,step):
        pi.hardware_PWM(PWM_GPIO,PWM_freq,i*10000)
        sleep(t)
        shots.append(findsharpness(src))
        src=vs.frame
    shots.append(findsharpness(src))
    if step==1:
        sleep(0.5)
        src=vs.frame
        for i in xrange(100-step,-step,-step):
            pi.hardware_PWM(PWM_GPIO,PWM_freq,i*10000)
            sleep(t)
            shots[(i+step)//step]=(shots[(i+step)//step]+findsharpness(src))//2
            src=vs.frame
        shots[0]=(shots[0]+findsharpness(src))//2
    else:
        transition(50)
        transition(0)
        sleep(0.5)
    last_duty=0
    return(shots)

def fullwichsscan(step):
    global last_duty
    transition(0)
    shots=[]
    t=0
    if step==1:
        t=0.5
    src=vs.frame
    for i in xrange(step,100+step,step):
        pi.hardware_PWM(PWM_GPIO,PWM_freq,i*10000)
        sleep(t)
        shots.append(findsharpness_post(src))
        src=vs.frame
    shots.append(findsharpness_post(src))
    if step==1:
        sleep(0.5)
        src=vs.frame
        for i in xrange(100-step,-step,-step):
            pi.hardware_PWM(PWM_GPIO,PWM_freq,i*10000)
            sleep(t)
            shots[(i+step)//step]=(shots[(i+step)//step]+findsharpness_post(src))//2
            src=vs.frame
        shots[0]=(shots[0]+findsharpness_post(src))//2
    else:
        transition(50)
        transition(0)
        sleep(0.5)
    last_duty=0
    return(shots)


    

def nothing(x):
    pass

key=cv2.waitKey(1)
if __name__ == '__main__':
    
    print("[INFO] sampling THREADED frames from `picamera` module...")
    vs.startcapt()
##    pi.set_mode(PWM_GPIO,pigpio.OUTPUT)
##    
##    
##    
##
##    cv2.createTrackbar('duty','detection',4,100,nothing)
##    cv2.createTrackbar('freq','detection',280,400,nothing)
##    cv2.createTrackbar('thresh','detection',50,255,nothing)
##
##    olddt=4
##    oldf=18
##    while key != ord("q"):
##        key=cv2.waitKey(1)
##        src=vs.frame
##        duty=cv2.getTrackbarPos('duty','detection')*10000
##        freq=cv2.getTrackbarPos('freq','detection')*100
##        thresh=cv2.getTrackbarPos('thresh','detection')
##        cv2.imshow('detection',src)
##        if cv2.waitKey(1) == ord("s"):
##            print(sum([item for item in p.map(findsharpness,slicer(src),1)]))
##            #print(findsharpness(src))
##        if cv2.waitKey(1) == ord("c"):
##            start=timer()
##            #cv2.sumElems(src)[0]
##            sharpi=findsharpness(src)
##            end=timer()
##            print(end-start)
##            start=timer()
##            #sum([item for item in p.map(sumelem,slicer(src),1)])
##            sharpi=sum([item for item in p.map(findsharpness,slicer(src),1)])
##            end=timer()
##            print(end-start)
##
##        if duty!=olddt or freq!=oldf:
##            pi.hardware_PWM(18,freq,duty)
##            olddt=duty
##            oldf=freq
##            
##    transition(0)





    cv2.createTrackbar('freq','detection',280,400,nothing)   
        
    while key != ord("q"):
        key=cv2.waitKey(1)
        PWM_freq=cv2.getTrackbarPos('freq','detection')*100
        if key == ord("a") & 0xFF:
            print("doing something")
            
            start=timer()
            
            s1=fullscan(1)
            end=timer()
            print("1-step: {} s".format(end-start))
            transition(0)
            sleep(1)
            start=timer()
            
            s2=fullscan(1)
            end=timer()
            print("2-step: {} s".format(end-start))
            transition(0)
            sleep(1)
            start=timer()
            
            s4=fullwichsscan(1)
            end=timer()
            print("4-step: {} s".format(end-start))
            transition(0)
            sleep(1)
            start=timer()
           
            s5=fullwichsscan(1)
            end=timer()
            print("5-step: {} s".format(end-start))
            transition(0)
            sleep(1)
            start=timer()
            
            s10=fullscan(1)
            end=timer()
            print("10-step: {} s".format(end-start))
            transition(0)
            sleep(1)
            start=timer()
            
            s20=fullwichsscan(1)
            end=timer()
            print("2-step: {} s".format(end-start))
            transition(0)


            

            plt.figure(1)
            
            plt.plot(range(0,101,1),s1,label='s {i}'.format(i=1))
            plt.plot(range(0,101,1),s2,label='s {i}'.format(i=2))
            plt.plot(range(0,101,1),s4,label='s {i}'.format(i=4))
            plt.plot(range(0,101,1),s5,label='s {i}'.format(i=5))
            plt.plot(range(0,101,1),s10,label='s {i}'.format(i=10))
            plt.plot(range(0,101,1),s20,label='s {i}'.format(i=2))

            
            plt.legend(loc='best')
            plt.show()


##    start=timer()
##    gls=globalwichs_search(0.1/50)
##    end=timer()
##    print("Global Search in {a} Sekunden:{b}".format(a=end-start,b=gls))
####    start=timer()
####    fls=fine_search(30)
####    end=timer()
####    print("Fein Search in {a} Sekunden:{b}".format(a=end-start,b=fls))
##    transition(50)
##    transition(25)
##    transition(10)
##    transition(0)


    vs.stopcapt()
    cv2.destroyAllWindows()
    pi.set_mode(PWM_GPIO,pigpio.INPUT)
    pi.stop()
    p.close()
    p.join()

    print("exit")
    exit()

        
