#!/usr/bin/env python

import argparse
import cv2 as cv
import numpy as np
import rcpcv.demo

def highlight_circles(frame, args):
    gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)

    # normalize levels to full range of pixel values
    gray = cv.normalize(gray, None, 0, 255, cv.NORM_MINMAX, dtype=cv.CV_8U)

    # reduce noise
    blur = cv.medianBlur(gray,5)

    # find circles
    circles = cv.HoughCircles(blur,
                              method=cv.HOUGH_GRADIENT,
                              dp=1,        # accumulator has same resolution as the image
                              minDist=8,   # minimum center to center distance
                              param1=200,  # with HOUGH_GRADIENT, Canny edge threshold
                              param2=20,   # with HOUGH_GRADIENT, accumulator threshold
                              minRadius=0,
                              maxRadius=50) # limit to small circles
    if circles is not None:
        # the result seems to be a 1 x num-circles x 3 matrix
        # this reshapes it to a 2D matrix, each row is then [x y r]
        num_circles = circles.shape[1]
        circles = circles.reshape((num_circles, 3))

        # discretize circle positions to integer pixel coordinates
        rounded = np.uint16(np.round(circles))

        # throw away excess circles
        if num_circles > 150:
            rounded = rounded[0:150]

        # draw each circle in frame
        for i in rounded:
            # draw the perimeter in green
            cv.circle(frame,(i[0],i[1]),i[2],(0,255,0),2)

    return frame

if __name__ == "__main__":
    parser = argparse.ArgumentParser( description = "Demo to find circles.")
    args = rcpcv.demo.parse_args(parser)
    rcpcv.demo.run_filter(args, highlight_circles)