#!/usr/bin/python
import numpy as np
import cv2
import cv2.cv
import hal

h = hal.component("capture")
h.newpin("blur", hal.HAL_FLOAT, hal.HAL_IN)
h.newpin("xloc", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("yloc", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("diam", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("xmax", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("ymax", hal.HAL_FLOAT, hal.HAL_OUT)
h.newpin("trigger", hal.HAL_BIT, hal.HAL_IN)
h.newpin("capt-diam", hal.HAL_FLOAT, hal.HAL_IN)
h.newpin("capt-tol", hal.HAL_FLOAT, hal.HAL_IN)
h.newpin("triggered", hal.HAL_BIT, hal.HAL_OUT)
h.newpin("samples", hal.HAL_FLOAT, hal.HAL_IN)
h.newpin("samptol", hal.HAL_FLOAT, hal.HAL_IN)


h.ready()

cap = cv2.VideoCapture(0)
h['xmax'] = float(cap.get(3))
h['ymax'] = float(cap.get(4))

# h['blur'] = 1
xpos = 0
ypos = 0
xmin = 0
ymin = 0
xmax = 0
ymax = 0
radc = 0
track = 0


try:
    while(True):
        # Capture frame-by-frame
        ret, frame = cap.read()

        # Our operations on the frame come here
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        blurit = cv2.blur(gray,(int(h['blur']),int(h['blur'])))
        # Display the resulting frame
    
        circles = cv2.HoughCircles(blurit,cv2.cv.CV_HOUGH_GRADIENT,1,20,param1=50,param2=35,minRadius=int(h['capt-diam']/2)-int(h['capt-tol']),maxRadius=int(h['capt-diam']/2)+int(h['capt-tol']))

        cv2.circle(blurit,(320,240),int(h['capt-diam']/2)-int(h['capt-tol']),(246,11,11),1)
        cv2.circle(blurit,(320,240),int(h['capt-diam']/2)+int(h['capt-tol']),(246,11,11),1)
        if  h['xloc'] > 0 and not h['trigger']:
             cv2.circle(blurit,(int(h['xloc']),int(h['yloc'])),int(h['diam']/2)+int(h['capt-tol']),(0,255,0),1)


        # print circles

        if not h['trigger'] and h['triggered']:
            h['triggered'] = False
            track = 0
            xpos = 0
            ypos = 0
            radc = 0
            xmin = 0
            ymin = 0
            xmax = 0
            ymax = 0

        if circles is not None and h['trigger'] and not h['triggered']:
            # print circles
            circconv = np.uint16(np.around(circles))
            for i in circconv[0,:]:
                # draw the outer circle
                cv2.circle(blurit,(i[0],i[1]),i[2],(246,11,11),1)
                # draw the center of the circle
                cv2.circle(blurit,(i[0],i[1]),2,(246,11,11),1)
                xpos = xpos + i[0]
                ypos = ypos + i[1]
                radc = radc + i[2]
                
                track = track + 1

                if i[0] < xmin or xmin is 0:
                    xmin = i[0]
                if i[0] > xmax or xmax is 0:
                    xmax = i[0]
                if i[1] < ymin or ymin is 0:
                    ymin = i[1]
                if i[1] > ymax or ymax is 0:
                    ymax = i[1]


                if abs(ymax-ymin) > h['samptol'] or abs(xmax-xmin)>h['samptol']:
                    track = 0
                    xpos = 0
                    ypos = 0
                    radc = 0
                    xmin = 0
                    ymin = 0
                    xmax = 0
                    ymax = 0                    
                    
            if track >= h['samples']:                                
                h['xloc'] = float(xpos)/track
                h['yloc'] = float(ypos)/track
                h['diam'] = (float(radc)/track)*2
                h['triggered'] = True
        img = cv2.flip(blurit, 1)
        cv2.line(img,(320,0),(320,480),(255,0,0),1)
        cv2.line(img,(0,240),(640,240),(255,0,0),1)
        cv2.imshow('frame',img)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

except KeyboardInterrupt:
    cap.release()
    cv2.destroyAllWindows()
    raise SystemExit