import numpy as np
import cv2

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'):
		break



# setup initial location of window
track_window = (c,r,w,h)

# set up the ROI for tracking
roi = frame[r:r+h, c:c+w]
hsv_roi =  cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_roi, np.array((0., 60.,32.)), np.array((180.,255.,255.)))
roi_hist = cv2.calcHist([hsv_roi],[0],mask,[180],[0,180])
cv2.normalize(roi_hist,roi_hist,0,255,cv2.NORM_MINMAX)

# Setup the termination criteria, either 10 iteration or move by atleast 1 pt
term_crit = ( cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1 )

while(1):
    ret ,frame = cap.read()
    frame = cv2.flip(frame,1)
    if ret == True:
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        dst = cv2.calcBackProject([hsv],[0],roi_hist,[0,180],1)

        # apply meanshift to get the new location
        ret, track_window = cv2.meanShift(dst, track_window, term_crit)

        # Draw it on image
        x,y,w,h = track_window
        cv2.rectangle(frame, (x,y), (x+w,y+h), 255,2)
        cv2.imshow('Seguimiento',frame)

        k = cv2.waitKey(60) & 0xff
        if k == 27:
            break
        else:
            cv2.imwrite(chr(k)+".jpg",frame)

    else:
        break

cv2.destroyAllWindows()
cap.release()