from time import sleep
from picamera import PiCamera
from io import BytesIO
import cv2
import numpy as np

def init():
	go = True
	print('Initilizing Camera and cascade components..')
	try:
		camera = PiCamera()
		camera.resolution = (640, 480)
		sleep(2)
		print('[ OK ] Camera')
	except:
		print('[ Error ] Can not initialize PiCamera')
		go = False

	try:
		face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
		sleep(1)
		print('[ OK ] CascadeClassifier')
	except:
		print('[ Error ] Can not load cascade File')
		go = False
	if (go):
		print('Starting Photo loop..')
		start(camera, face_cascade)
	else: 
		print('closing')
	
def start( camera, face_cascade):
	while True:
		try:
			print('Taking photo')
			camera.capture('img.jpg')
			print('Photo Captured')
			img = cv2.imread('img.jpg',1)
			gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
			print('Photo Converted')
			faces = face_cascade.detectMultiScale(gray, 1.3, 5)
			print('Photo Processed')
			i = 0;
			for (x,y,w,h) in faces:
				i = i + 1
				cv2.rectangle(img,(x,y),(x+w,y+h),(255,0,0),2)
				#cv2.imwrite('result.jpg', gray)
				roi_gray = gray[y:y+h, x:x+w]
				roi_color = img[y:y+h, x:x+w]
				print('Drawing on face ', i)
				
			cv2.imshow('image',img) 
			cv2.waitKey(100)
			if cv2.waitKey(1) & 0xFF == ord('q'):
				cv2.destroyAllWindows()
				break
		
		except:
			print('[ Error ] Unexpected exception')
			print('Closing')
			break
	
init()