#! /usr/bin/env python
# -*- coding: UTF-8 -*-


try: import cv2
except ModuleNotFoundError:
   print('OpenCV not installed, you can just try')
   print('$ sudosudo apt-get install python3-opencv')
   exit(1)
import numpy as np
import detection as dtct
import matplotlib.pyplot as plt
from time import time, sleep


import serial
dev = '/dev/ttyUSB0'
baud=9600
try: arduino = serial.Serial(dev,baud)   # create serial object named arduino
except serial.serialutil.SerialException:
   print('Arduino is not connected')
   exit(1)
alpha0 = 120

cap = cv2.VideoCapture(4)
#kernel = np.ones((5,5),np.uint8)

print('Initial positioning')
x0_deg = 90
pos_x = bytes(str(x0_deg).encode('utf-8'))
arduino.write( pos_x )
sleep(1)
print('done')

flip = False
out = False
pos_x = '90'
pos_y = '90'
prev_angle = 90
while not out:
   a, img = cap.read()
   if flip: img = cv2.flip( img, 0 )
   vid_y, vid_x, _ = img.shape

   # Look for faces
   centers, corners = dtct.face(img)

   # Draw a box around each face
   dtct.box(corners, img)

   if len(centers) > 0:
      # Find centroid of all the faces (weighted by the size of the face)
      M = np.array(centers)
      X = M[:,0]
      Y = M[:,1]
      A = M[:,2]
      x0 = np.average(X,weights=A)  # position in pixels
      y0 = np.average(Y,weights=A)  #
      x0_deg = 180 - (170*(x0/vid_x)+10)

      x0_deg = (0.5-(x0/vid_x))*2  # \in [-1,1]
      if not flip: x0_deg *= -1
      if abs(x0_deg) > 0.05:
         x0_deg = 5*x0_deg  #/abs(x0_deg)
      else: x0_deg = 0
      new_angle = prev_angle+x0_deg
      pos_x = bytes(str(new_angle).encode('utf-8'))
      arduino.write( pos_x )
      # XXX To be included when I can implement the second servo
      #pos_y = bytes(str(x0).encode('utf-8'))
      #arduino.write( pos_y )

      prev_angle = new_angle

   # ===============================
   #    Shows the processed image
   # ===============================
   cv2.imshow('face',img)
   k = cv2.waitKey(5) & 0xFF  #   Waits for  <Esc> key
   if k == 27:                # to exit the infinite loop
     out = True               #
     break                    #

cv2.destroyAllWindows()
print('Final positioning')
x0_deg = '90'
pos_x = bytes(x0_deg.encode('utf-8'))
arduino.write( pos_x )
sleep(1)
arduino.write( pos_x )   # XXX For some reason some times it doesn't return to
sleep(1)                 # 90 position right away
print('done')
arduino.close()
