import numpy as np
import cv2

cap = cv2.VideoCapture(0)

# params for ShiTomasi corner detection
feature_params = dict( maxCorners = 100,
                       qualityLevel = 0.3,
                       minDistance = 7,
                       blockSize = 7 )

# Parameters for lucas kanade optical flow
lk_params = dict( winSize  = (15,15),
                  maxLevel = 2,
                  criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

# Create some random colors
color = np.random.randint(0,255,(100,3))

drawing = True
touches = 0
r,h,c,w = 0,0,0,0
r2,c2 = 0,0

def mousePressed(event,x,y,flags,param):
	global drawing,touches,r,c,r2,c2,w,h
	if drawing == True:	
		if event == cv2.EVENT_LBUTTONUP:
			print(x,y)
			if touches == 0:
				r=y
				c=x
				touches = touches + 1
			elif touches == 1:
				r2=y
				c2=x
				touches = touches + 1
				w=abs(c2-c)
				h=abs(r2-r)
				drawing = False

cv2.namedWindow('Seguimiento')
cv2.setMouseCallback('Seguimiento',mousePressed)
cap = cv2.VideoCapture(0)

while(1):
# take first frame of the video
	ret,frame = cap.read()
	frame = cv2.flip(frame,1)
	if touches == 1:
		cv2.circle(frame,(c,r),3,(255,0,0),-1)
	elif touches == 2:
		cv2.circle(frame,(c,r),3,(255,0,0),-1)
		cv2.circle(frame,(c2,r2),3,(0,0,255),-1)
		cv2.rectangle(frame,(c,r),(c2,r2),(0,255,0),2)
	cv2.imshow('Seguimiento',frame)
	k = cv2.waitKey(1) & 0xFF
	if k == ord('m'):
		drawing = True
		touches = 0
		r,r2,c,c2,w,h = 0,0,0,0,0,0
	elif k == ord('g'):
		if drawing==False:
			break


cv2.destroyWindow('Seguimiento')
# Take first frame and find corners in it
roi = frame[r:r+h, c:c+w]
#ret, old_frame = cap.read()
old_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
old_gray_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
p0 = cv2.goodFeaturesToTrack(old_gray_roi, mask = None, **feature_params)
for punto in p0:
   punto[0][0] = punto[0][0] + c
   punto[0][1] = punto[0][1] + r
# Create a mask image for drawing purposes
mask = np.zeros_like(frame)

while(1):
    ret,frame = cap.read()
    frame = cv2.flip(frame,1)
    frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # calculate optical flow
    p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None, **lk_params)

    # Select good points
    good_new = p1[st==1]
    good_old = p0[st==1]

    # draw the tracks
    for i,(new,old) in enumerate(zip(good_new,good_old)):
        a,b = new.ravel()
        c,d = old.ravel()
        cv2.line(mask, (a,b),(c,d), color[i].tolist(), 1)
        cv2.circle(frame,(a,b),3,color[i].tolist(),-1)
    img = cv2.add(frame,mask)

    cv2.imshow('frame',img)
    cv2.imshow('mask',mask)
    k = cv2.waitKey(30) & 0xff
    if k == 27:
        break

    # Now update the previous frame and previous points
    old_gray = frame_gray.copy()
    p0 = good_new.reshape(-1,1,2)

cv2.destroyAllWindows()
cap.release()