import cv2
import numpy as np

cap = cv2.VideoCapture('/home/hydrofire/Study/ITS/Lab5/Material-lab5-2018/motion.avi')

tracking_params = dict(winSize = (10, 10),
                 maxLevel = 4,
                 criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

feature_params = dict( maxCorners = 50,
                       qualityLevel = 0.15,
                       minDistance = 15)

_, old_frame = cap.read()
old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)
p0 = cv2.goodFeaturesToTrack(old_gray, **feature_params)

red = [0,0,255]
green = [0,255,0]

mask = np.zeros_like(old_frame)
while cap.isOpened():
    _, frame = cap.read()


    if frame is not None:

        frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

        p0 = cv2.goodFeaturesToTrack(frame_gray, **feature_params)
        p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None, **tracking_params)


        for i, (new, old) in enumerate(zip(p1,p0)):
            a, b = new.ravel()
            c, d = old.ravel()
            #frame = cv2.circle(frame, (a, b), 3, (0, 255, 0), -1)
            #frame = cv2.circle(frame, (c, d), 3, (0, 0, 255), -1)
            mask = cv2.line(mask, (a, b), (c, d), red, 2)
            frame = cv2.circle(frame, (a, b), 5, green)

        img = cv2.add(frame, mask)
        cv2.imshow('frame', img)

        #cv2.imshow('frame', frame)
        old_gray = frame_gray.copy()


    if cv2.waitKey(1) & 0xFF == ord('q'):
        break



cap.release()
cv2.destroyAllWindows()