from time import sleep
from PIL import Image
import os
import serial
import sys
import json
import datetime

piCamAvailable = False

try:
    import numpy as np
    from picamera import PiCamera
    from picamera.array import PiRGBArray
    piCamAvailable = True

except Exception as ex:
    print('[ Error ] some depandincies are missing\n'+ str(ex.args))
    piCamAvailable = False;

try:
    import cv2

except Exception as ex:
    print('Could not load OpenCV\Closing')
    sys.exit()

def init():
	go = True
	piCam = False
	WebCam = False
	print('Initilizing Camera ..')
	if piCamAvailable:
		try:
			camera = PiCamera()
			camera.resolution = (640, 480)
			go = True
			piCam = True
			print('[ OK ] Started PiCamera')
		except:
			print('[ Warning ] Could not start PiCamera. Trying Webcam.. ')
			piCam = False
			go = False
	else:
		try:
			camera = cv2.VideoCapture()
			print('[ OK ] WebCamera')
			go = True
			WebCam = True
		except Exception as ex:
			print('[ Error ] Could not start Webcam.. \nExiting')
			go = False
	sleep(0.2)
	try:
		face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
		sleep(1)
		print('[ OK ] CascadeClassifier')
	except:
		print('[ Error ] Can not load cascade File')
		go = False
	try:
		os.mkdir('unknown')
	except OSError as ex:
		print('Found (unknown) folder')

	if go:
		print('Starting Photo loop..')
		print('Known people are '+str(loadNames()))
		start(camera, face_cascade, WebCam)

	else:
		print('closing')

def start( camera, face_cascade, WebCam):
	j = 0
	if not WebCam:
		rawCapture = PiRGBArray(camera)
	# face_recognizer = cv2.face.LBPHFaceRecognizer_create() #cv2.face.createLBPHFaceRecognizer()
	face_recognizer = cv2.face_LBPHFaceRecognizer('faces/h')
	# face_recognizer.read('faces/h') # Try on Pi
    ##WIP
	try:
		face_recognizer.load('faces/h')
	except:
		pass

	names = loadNames()
	while 1:
		try:
			if not WebCam:
				camera.capture(rawCapture, format="bgr")
				img = rawCapture.array
			else:
				ret, img = camera.read()
		except:
			print('[ Error ] Can not capture image. Restarting..')
			init()
		#img = rawCapture.array
		gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
		faces = face_cascade.detectMultiScale(gray, 1.3,5)
		i = 0;
		for (x,y,w,h) in faces:
			i += 1
			j += 1
			try:
				roi_gray = gray[y:y+h, x:x+w]
				roi_color = img[y:y+h, x:x+w]
				person = recognize(roi_gray, face_recognizer, names)
				cv2.rectangle(img,(x,y),(x+w,y+h),(0,255,0),1)
				cv2.putText(img, person, (x, y), cv2.FONT_HERSHEY_PLAIN, 1.7, (0,255,0), 2)
			except:
				print('[ Error ] Recognition fail')
			if person == 'unknown':
				print('Found unknown person')
				try:
					RGBImage = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
					image = Image.fromarray(RGBImage)
					image.save('unknown/'+str(datetime.datetime.now())+'.png')
				except Exception as ex :
					print('[ Error ] Saving unkown fail')
					print(ex)
			else:
				print('Found '+person)
				OpenShifter()
		cv2.imshow('image',img)
		cv2.waitKey(1)
		rawCapture.truncate(0)
		if cv2.waitKey(1) & 0xFF == ord('q'):
			cv2.destroyAllWindows()
			break


def recognize(image,face_recognizer, names):

	try:
		img = image.copy()
		label= face_recognizer.predict(img)
	except:
		print('[ Error ] Error in Recognize() function')
	if label[1] > 70:
		print(str(label))
		return 'unknown'
	else:
		print(str(label) + ' >>'+names[label[0]])
		return names[label[0]]

def OpenShifter():
	try:
		ser = serial.Serial('/dev/ttyUSB0',9600)
		ser.open()
		ser.write('o')
		ser.close()
	except Exception as ex:
		print('[ Error ] Can not connect to Arduino at /dev/ttyUSB0 ..')
		try:
			ser = serial.Serial('/dev/ttyUSB1',9600)
			ser.open()
			ser.write('o')
			ser.close()
		except:
			print('[ Error ] Can not connect to Arduino at /dev/ttyUSB1 ..')

def loadNames():
	try:
		with open("faces/names.json", "r") as read_file:
			data = json.load(read_file)
			return list(data)
	except:
		print('[ Error ] Error in loadNames() function')


init()