import numpy as np
import cv2
import time
import serial

def flush():
 for i in range(300):
  ser.flushInput()

def l(frame):
 return cv2.Laplacian(frame, cv2.CV_64F).var()

cap = cv2.VideoCapture(0)

try:
 ser = serial.Serial('/dev/ttyACM0', 115200)
except:
 ser = serial.Serial('/dev/ttyACM1', 115200)

time.sleep(1)

_, frame = cap.read()
frame = frame[50:380,200:540]

print frame.shape

l(frame)

flush()
flush()

print 'battery'
time.sleep(2)

ser.write('100')
time.sleep(0.3)

flush()
flush()

ser.write('3')
flush()
time.sleep(0.02)
print ser.readline()

for i in range(10):
 ser.write('66')
 flush()
 time.sleep(1.85)

# Here we must go up, taking the biggest

laplace = []
count = 0

stop = False
batch = 0

while(stop == False):
 _, frame = cap.read()
 frame = frame[50:380,200:540]
 batch += 1
 cv2.imshow('ff', frame)
 cv2.waitKey(10)
 if batch == 20:
  #cv2.imwrite('r' + str(count) + '.jpg', frame)
  batch = 0
  v = l(frame)
  print v
  laplace.append(v)
  count += 1
  if count >= 3:
   if (laplace[len(laplace)-2] > (laplace[len(laplace)-1]+2)) and (laplace[len(laplace)-2] > (laplace[len(laplace)-3]+2)):
    for i in range(5):
     ser.write('6')
     time.sleep(0.05)
     ser.readline()
     flush()
    time.sleep(0.08)
    stop = True
   else:
    for i in range(3):
     ser.write('9')
     time.sleep(0.05)
     ser.readline()
     flush()
    time.sleep(0.08)

cv2.imwrite('finalfocus.png', frame)

while(1):
 _, frame = cap.read()
 frame = frame[50:380,200:540]
 cv2.imshow('ff', frame)
 cv2.waitKey(10)

print 'done'