
import cv2
import pigpio
from imutils.video.pivideostream_fixed import PiVideoStream
from time import sleep
from sys import exit
import numpy
from timeit import default_timer as timer
from multiprocessing.dummy import Pool
import matplotlib.pyplot as plt


PWM_GPIO = 18

pi = pigpio.pi()

def overshoot_duty(PWM_freq,old,new,speed,overshoot):
    if old<new:
        if new+overshoot<100:
            pi.hardware_PWM(PWM_GPIO,PWM_freq,(new+overshoot)*10000)
            for i in range(0,1030,30):
                pi.hardware_PWM(PWM_GPIO,PWM_freq,(new+overshoot*numpy.exp(-i*speed))*10000)
            pi.hardware_PWM(PWM_GPIO,PWM_freq,new*10000)
        pi.hardware_PWM(PWM_GPIO,PWM_freq,new*10000)
    elif old>new:
        if new-overshoot>0:
            pi.hardware_PWM(PWM_GPIO,PWM_freq,(new-overshoot)*10000) 
            sleep(1.0/speed)
            pi.hardware_PWM(PWM_GPIO,PWM_freq,new*10000)
        pi.hardware_PWM(PWM_GPIO,PWM_freq,new*10000)
    pass


def transition(pin,freq,old,new):
    delta=new-old
    for i in range(0,10,1):

        pi.hardware_PWM(pin,freq,int((old+delta*(1-numpy.exp(-i/2.0)))*10000))
    pi.hardware_PWM(pin,freq,new*10000)
    pass

def findsharpness(src):
    src = cv2.cvtColor(src, cv2.COLOR_BGR2GRAY)
    #cv2.imshow('detection',src)
    src=cv2.bilateralFilter(src,3,6,1.5)
    dx = cv2.convertScaleAbs(cv2.Sobel(src, cv2.CV_16S,1,0, ksize=3))
    dy = cv2.convertScaleAbs(cv2.Sobel(src, cv2.CV_16S,0,1, ksize=3))
    accu=cv2.addWeighted(dx,0.5,dy,0.5,0,-1)
    #cv2.imshow('detection',accu)
    th,accu=cv2.threshold(accu,50,0,cv2.THRESH_TOZERO)
    return int(cv2.sumElems(accu)[0])
p=Pool(processes=4)
def nothing(x):
    pass


vs = PiVideoStream((480,480),32)

if __name__ == '__main__':
    
    print("[INFO] sampling THREADED frames from `picamera` module...")
    vs.start()
    sleep(2)
    pi.set_mode(PWM_GPIO,pigpio.OUTPUT)
    
    cv2.namedWindow("detection", cv2.WINDOW_AUTOSIZE)

    cv2.createTrackbar('duty','detection',0,100,nothing)
    cv2.createTrackbar('recover','detection',0,100,nothing)
    cv2.createTrackbar('overshoot','detection',0,50,nothing)
    cv2.createTrackbar('freq','detection',18000,25000,nothing)
    done=False
    homed=False
    collect=[]
    while (cv2.waitKey(1) != ord("q")& 0xFF) and not done:

        for i in range(0,101,1):
            pi.hardware_PWM(PWM_GPIO,20000,i*10000)
            collect.append(vs.frame)
            sleep(0.1)
            #collect.append(vs.frame)
           
##        for i in range(90,-10,-10):
##            pi.hardware_PWM(PWM_GPIO,20000,i*10000)
##            collect.append(vs.frame)
##            sleep(0.1)
##        for i in range(0,110,10):
##            pi.hardware_PWM(PWM_GPIO,20000,i*10000)
##            collect.append(vs.frame)
##            sleep(0.1)
##        for i in range(90,-10,-10):
##            pi.hardware_PWM(PWM_GPIO,20000,i*10000)
##            collect.append(vs.frame)
##            sleep(0.1)
            



        for i in range(0,1030,30):
                pi.hardware_PWM(PWM_GPIO,20000,(100*numpy.exp(-i/300.0))*10000)
        pi.hardware_PWM(PWM_GPIO,20000,0)
        results=p.map(findsharpness,collect)
        #bad=results[::2]
        #good=results[1::2]
        plt.figure(1)
        #plt.plot(bad,'k')
        plt.plot(results,'k')
        #plt.plot(good,'r--')
        plt.show()
        
        done=True
        
##        duty=cv2.getTrackbarPos('duty','detection')
##        PWM_freq=cv2.getTrackbarPos('freq','detection')
##        recover=cv2.getTrackbarPos('recover','detection')+1
##        overshoot=cv2.getTrackbarPos('overshoot','detection')
##        
##        if not homed:
##            pi.hardware_PWM(PWM_GPIO,PWM_freq,0)
##            homed=True
##            sleep(1)
##            
##        if cv2.waitKey(1) == ord("a"):
##            print("a")
##            start=timer()
##            overshoot_duty(PWM_freq,0,duty,recover/1000.0,overshoot)
##            src1=vs.frame
##            end=timer()
##            sleep(1)
##            src2=vs.frame
##            homed=False
##            ssrc1=findsharpness(src1)
##            ssrc2=findsharpness(src2)
##            print (ssrc1-ssrc2)/ssrc1
##            print(end-start)
##
##        if cv2.waitKey(1) == ord("b"):
##            print("b")
##            start=timer()
##            pi.hardware_PWM(PWM_GPIO,PWM_freq,duty*10000)
##            src1=vs.frame
##            end=timer()
##            sleep(1)
##            src2=vs.frame
##            homed=False
##            ssrc1=findsharpness(src1)
##            ssrc2=findsharpness(src2)
##            print (ssrc1-ssrc2)/ssrc1
##            print(end-start)

          
##        pi.hardware_PWM(PWM_GPIO,25000,0)
##        sleep(1)
##        pi.hardware_PWM(PWM_GPIO,25000,50*10000)
##        sleep(1)
##        for i in range(100):
##            collect.append(int(findsharpness(vs.frame)))
##        print collect
##        print(sum(collect)/len(collect))
##        done=True
        




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


