# -*- coding: utf-8 -*-
from __future__ import print_function
from __future__ import division
from time import sleep
import numpy as np
import cv2
from multiprocessing.dummy import Pool



class Autofokus:

    def __init__(self,freq=28000,pin=18,thresh=80):
        self.PWM_freq=freq
        self.PWM_GPIO = pin
        self.thresh=thresh

        self.last_duty=0
        self.p=None
        self.sharpness_map=None
        self.best_duty=0
#Stream_Object und PWM_Object als Referenz einfügen(...([stream_object],[pwm_object])), und nicht als Kopie...        
    def start_focus(self,Stream_Object,PWM_Object):
        
        self.p=Pool(processes=4)
        self.pi = PWM_Object[0]
        self.vs = Stream_Object[0]
    
    def stop_focus(self):
    
##        if self.pi != None:
##            self.vs.stopcapt()
##            self.pi.stop()
            
        if self.p != None:
            self.p.close()
            self.p.join()
        
    def transition(self,new):
        delta=new-self.last_duty
        if delta is not 0:
            for i in range(self.last_duty,new,2*cmp(delta,0)):
                self.pi.hardware_PWM(self.PWM_GPIO,self.PWM_freq,i*10000)
                sleep(0.01)
    
            self.pi.hardware_PWM(self.PWM_GPIO,self.PWM_freq,new*10000)
            self.last_duty=new
        pass

    
    def findsharpness_prior(self,src):
        src=cv2.bilateralFilter(src,3,6,1.5)
        return src
 
        
    def findsharpness_post(self,src):
       
        dx = cv2.Sobel(src, cv2.CV_16S,1,0, ksize=1)
        dy = cv2.Sobel(src, cv2.CV_16S,0,1, ksize=1)
        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,self.thresh,0,cv2.THRESH_TOZERO)
        return int(cv2.sumElems(accu)[0])

    def global_search(self,t=0.02,step=2):
        self.last_duty
        self.transition(0)
        shots=np.empty(((100//step)+1),dtype=object)
        src=self.vs.frame
        for i in xrange(step,100+step,step):
            self.pi.hardware_PWM(self.PWM_GPIO,self.PWM_freq,i*10000)
            shots[(i//step)-1]=self.findsharpness_prior(src)
            sleep(t)
            self.last_duty=i
            src=self.vs.frame
        
        shots[i//step]=self.findsharpness_prior(src)
        self.sharpness_map=self.p.map(self.findsharpness_post,shots)
        self.best_duty=self.sharpness_map.index(max(self.sharpness_map))*step
        self.transition(self.best_duty)
        pass
