import cv2
import numpy as np
import math
import os, sys
import time
import serial
import paho.mqtt.client as mqtt

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

try:
 cap = cv2.VideoCapture(0)
except:
 os.exit()

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

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

def refocus():
  ser.write('66')
  time.sleep(1.85)
  ser.readline()
  flush()
  flush()
  
  laplace = []
  batch = 0
  count = 0
  stop = False
  while (stop == False):
   _, frame = cap.read()
   cv2.waitKey(10)
   batch += 1
   if batch == 8:
    batch = 0 
    count += 1
    lap = l(frame)
    print lap
    laplace.append(lap)
    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.02)
       print ser.readline()
      time.sleep(0.08)
      stop = True
     else:
      for i in range(3):
       ser.write('9')
       time.sleep(0.02)
       print ser.readline()
      time.sleep(0.08)

def on_connect(client, userdata, flags, rc):
 client.subscribe('/tab/send/microscope/serial')
 client.subscribe('/tab/send/microscope/startdiagnostic')

def on_message(client, userdata, msg):
 print msg.topic, msg.payload
 if msg.topic == '/tab/send/microscope/serial':
  if str(msg.payload) == '100' or str(msg.payload) == '101':
   ser.write(str(msg.payload))
   flush()
   flush()
  else:
   print 'commands'
   ser.write(str(msg.payload))
   time.sleep(0.02)
   flush()
   ser.readline()
   flush()
 elif msg.topic == '/tab/send/microscope/startdiagnostic' and str(msg.payload).split(';')[1] == '1':
  flush()

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

  flush()

  print 'reinit axis'
  
  ser.write('10')
  time.sleep(0.02)  
  print ser.readline()

  flush()

  for i in range(5):
   ser.write('66')
   time.sleep(1.85)
   print ser.readline()

  time.sleep(1)

  flush()
  flush()

  print 'center slide'

  for i in range(8):
   ser.write('4')
   time.sleep(0.02)
   print ser.readline()

  for i in range(25):
   ser.write('5')
   time.sleep(0.02)
   print ser.readline()

  for i in range(5):
   ser.write('66')
   time.sleep(1.85)
   ser.readline()

  flush()
  print 'Autofocus'
  refocus()

  print 'Sweep sample'

  rows = 0
  num_batch = 0
  img = 0
  while(rows < 6):

   for i in range(80):
    while (num_batch < 8):
     num_batch += 1
     _, frame = cap.read()
     cv2.waitKey(10)
    cv2.imwrite('img' + str(img) + '.png', frame)
    img += 1
    ser.write('5')
    time.sleep(0.02)
    ser.readline()
    flush()

   ser.write('4')
   time.sleep(0.02)
   print ser.readline()
   flush()
   num_batch = 0
   while(num_batch < 8):
    num_batch += 1
    _, frame = cap.read()
    cv2.waitKey(10)
   cv2.imwrite('img' + str(img) + '.png', frame)
   img += 1

   for i in range(80):
    while (num_batch < 8):
     num_batch += 1
     _, frame = cap.read()
     cv2.waitKey(10)
    cv2.imwrite('img' + str(img) + '.png', frame)
    img += 1
    ser.write('8')
    time.sleep(0.02)
    ser.readline()
    flush()

   ser.write('4')
   time.sleep(0.02)
   print ser.readline()
   flush()
   num_batch = 0
   while(num_batch < 8):
    num_batch += 1
    _, frame = cap.read()
    cv2.waitKey(10)
   cv2.imwrite('img' + str(img) + '.png', frame)
   img += 1
   flush()

if __name__ == '__main__': 
 time.sleep(1)
 print 'battery'
 time.sleep(3)
 client = mqtt.Client()
 client.on_message = on_message
 client.on_connect = on_connect
 client.connect('192.168.42.1', 1883, 60)
 client.loop_forever()