import numpy as np
import cv2
import os
from os import listdir
from os.path import isfile, join
import math

#Image folder
impath = str('E:/gp/chalan')
files = [f for f in listdir(impath) if isfile(join(impath, f))]
images = np.empty(len(files), dtype=object)

counter = 0
while counter < len(files):

    #Optical flow lists initialization
    ls_kpc = []
    ls_kpn = []

    #Read Images
    imgc = cv2.imread(join(impath, files[counter]), 0) #First image
    imgn = cv2.imread(join(impath, files[counter+1]), 0) #Next image

    #SURF Keypoint Detection
    orb = cv2.ORB(100)

    kpc, desc = orb.detectAndCompute(imgc, None)
    kpn, desn = orb.detectAndCompute(imgn, None)

    #BFMatcher
    bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)

    #Matches
    matches = bf.match(desc, desn)
    matches = sorted(matches, key = lambda x:x.distance)

    #Matched Points
    for match in matches:
        imgc_idx = match.queryIdx
        imgn_idx = match.trainIdx
        (x1, y1) = kpc[imgc_idx].pt
        (x2, y2) = kpn[imgn_idx].pt

        ls_kpc.append((x1,y1))
        ls_kpn.append((x2,y2))

    #Assign optical flow points to x, y coordinates lists
    matc = np.array(ls_kpc, dtype=np.int16)
    matn = np.array(ls_kpn, dtype=np.int16)
    mag = np.empty((len(matc), 1), dtype=np.int32)
    speed = np.empty((len(matc),1), dtype=np.int32)
    vDir = np.empty((len(matc),1), dtype=np.float32)

    #Speed, displacement magnitude and vector direction
    for i in range(0, len(matc)):
        dX = matn[i,0]-matc[i,0]
        dY = matn[i,1]-matc[i,1]
        mag[i, 0] = math.sqrt((dX**2)+(dY**2))
        speed[i, 0] = (mag[i, 0]/2)
        vDir[i, 0] = math.degrees(math.atan2(dX, dY))
        #print vDir[i], '\n'
    avMag = np.sum(mag)/len(matc)
    avSpeed = np.sum(speed)/len(matc)
    avDir = np.std(vDir)
    print avDir

    counter += 1