import Tkinter as tk
import cv2, sys, time, os, math
from PIL import Image, ImageTk
import numpy as numpy


from SimpleCV import Color, np
from SimpleCV import Image as SimpleImage

from os import listdir
from os.path import isfile, join

# Load the BCM V4l2 driver for /dev/video0
os.system('sudo modprobe bcm2835-v4l2')
# Set the framerate ( not sure this does anything! )
os.system('v4l2-ctl -p 4')

width, height = 320, 240
cap = cv2.VideoCapture(0)
cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,  width)
cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, height)

cascPath = '/home/pi/lbpcascade_frontalface.xml'
faceCascade = cv2.CascadeClassifier(cascPath)

root = tk.Tk()
root.attributes("-fullscreen", True)
root.bind('<Escape>', lambda e: root.quit())

lmain = tk.Label(root)
lmain.pack()

last_image_faces = []

def show_frame():
    _, frame = cap.read()
    frame = cv2.flip(frame, 1)
    frame = faceDetect(frame)
    cv2image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGBA)
    img = Image.fromarray(cv2image)
    imgtk = ImageTk.PhotoImage(image=img)
    lmain.imgtk = imgtk
    lmain.configure(image=imgtk)
    lmain.after(1, show_frame)

def faceDetect(frame):

    # Do face detection
    #faces = faceCascade.detectMultiScale(frame, 1.1, 3, 0, (10, 10))

    #Slower method
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    gray = cv2.equalizeHist( gray )
    faces = faceCascade.detectMultiScale(
	gray,
        scaleFactor=1.1,
        minNeighbors=4,
        minSize=(20, 20),
        flags=cv2.cv.CV_HAAR_SCALE_IMAGE | cv2.cv.CV_HAAR_FIND_BIGGEST_OBJECT | cv2.cv.CV_HAAR_DO_ROUGH_SEARCH
    )

    print "Found {0} faces!".format(len(faces))
    global last_image_faces
    image_faces = []

    for (x, y, w, h) in faces:
        # Draw a green rectangle around the face
        face = frame[y:(y+h), x:(x+w)]
        center_x = x + (w/2)
        center_y = y + (h/2)
        center = [center_x, center_y]
        image_faces.append(center)
        tracking = False
        for pos in last_image_faces:
            #dist = sqrt( (center_x - pos[0])**2 + (center_y - pos[1])**2 )
            dist = math.hypot(center_x - pos[0], center_y - pos[1])
            print("Distance from last point " + str(dist))
            if dist < 30:
                tracking = True
        if tracking == False:
            cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 0, 255), 2)
            recognizeFace(face)
        else:
            cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2)
        break
    last_image_faces = image_faces
    return frame

def recognizeFace(face):
    face = cv2.cvtColor(face, cv2.COLOR_BGR2RGBA)
    count = 0
    match_found = False
    for f in face_db:
        count = count + 1
        template = SimpleImage(f)
        cv_face = SimpleImage(face)
        print("Scaning DB face " + str(count))
        keypoints = cv_face.drawSIFTKeyPointMatch(template,distance=50)
        if keypoints:
            # found match
            print("Match Found! (" + str(count) +")")
            match_found = True
            break
    if match_found == False:
        # Save picture
        print("No match found! Searched " + str(count) + " records. Saving image.")
        cv2.imwrite("/home/pi/photos/faces/face-" + str(len(face_db)) + ".jpg", face)
        loadFaceDB()

def loadFaceDB():
    # Load faces
    face_db_path='/home/pi/photos/faces'
    onlyfiles = [ f for f in listdir(face_db_path) if isfile(join(face_db_path,f)) ]
    global face_db
    face_db = numpy.empty(len(onlyfiles), dtype=object)
    for n in range(0, len(onlyfiles)):
      face_db[n] = cv2.imread( join(face_db_path,onlyfiles[n]) )


loadFaceDB()
show_frame()
root.mainloop()