import cv2
import numpy as np
import os
import time
import smbus2 as smbus
import matplotlib.pyplot as plt
from scipy.optimize import leastsq
from imutils.video import VideoStream
import imutils

try:
    import picamera
    from picamera.array import PiRGBArray
except:
    sys.exit(0)

TestCount = 200
StartTime = 0
FocalDistSpace = 40
ThresholdCount = 2


def capturepic(focal_dist, pic_name):
    focusing(int(focal_dist))
    time.sleep(0.3)
    camera.capture(pic_name)
    #frame = camera.read()
    #cv2.imwrite(pic_name, frame)

def focusing(val):
    i2c = smbus.SMBus(1)
    addr = 0x0c
    reg = (((val << 4) & 0x3ff0) >> 8) & 0x3f
    value = ((val << 4) & 0x3ff0) & 0xf0
    try:
        i2c.write_byte_data(addr, reg, value)
    except:
        pass
    i2c.close()
    #value = (val << 4) & 0x3ff0
    #data1 = (value >> 8) & 0x3f
    #data2 = value & 0xf0
    #os.system("i2cset -y 1 0x0c %d %d" % (data1, data2))

def laplacian_var(camera):
    rawCapture = PiRGBArray(camera)
    camera.capture(rawCapture,format="bgr", use_video_port=True)
    img = rawCapture.array
    rawCapture.truncate(0)
    img_gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    return cv2.Laplacian(img_gray, cv2.CV_64F).var()

if __name__ == "__main__":

    camera = picamera.PiCamera()
    #camera.start_preview()
    camera.resolution = (640,480)
    time.sleep(2)

    camera.shutter_speed = camera.exposure_speed
    camera.exposure_mode = 'off'
    g = camera.awb_gains
    camera.awb_mode = 'off'
    camera.awb_gains = g

    time.sleep(2)

    for i in range (TestCount):

        print("-----Photo " + str(i) + "-----")
        max_index = 10
        max_value = 0.0
        last_value = 0.0
        dec_count = 0
        top_index = 0
        focal_distance = 200

        StartTime = time.time()

        RecordX = []
        RecordY = []

        while True:
            focusing(focal_distance)
            val = laplacian_var(camera)

            if val > max_value:
                max_index = focal_distance
                max_value = val

            if val < last_value:
                dec_count += 1
            else:
                dec_count = 0

            if dec_count > ThresholdCount:
                break

            focal_distance += FocalDistSpace

            if focal_distance > 1000:
                break
            #time.sleep(0.2)

        print("TopPoint = (%d , %lf)" %(max_index, max_value))
        capturepic(max_index, "OriginAlgo_" + str(i) + ".png")


    #camera.stop_preview()
    camera.exposure_mode = 'auto'
    camera.awb_mode = 'auto'
    camera.close()