# import the necessary packages
import numpy as np;
from picamera.array import PiRGBArray
from picamera import PiCamera
import time
import cv2 as cv

resolution = (320, 240)

# control detector parameters
params = cv.SimpleBlobDetector_Params()

# params.minThreshold = 10
# params.maxThreshold = 200

params.filterByColor = True
params.filterByCircularity = False
params.filterByArea = True
params.filterByConvexity = False
params.filterByInertia = False
params.filterByCircularity = False

params.minArea = 1000
params.maxArea = 9999

params.blobColor = 0

print ("OpenCV Version: ", cv.__version__)

# check opencv version and construct the detector
is_v2 = cv.__version__.startswith("2.")
if is_v2:
    detector = cv.SimpleBlobDetector(params)
else:
    detector = cv.SimpleBlobDetector_create(params)

# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = resolution
rawCapture = PiRGBArray(camera, size=resolution)

# allow the camera to warmup
time.sleep(0.1)

once = True

while True:
    # grab an image from the camera
    camera.capture(rawCapture, format="bgr")
    image = rawCapture.array

    gray = cv.cvtColor(image, cv.COLOR_BGR2GRAY)

    # blurred = cv.GaussianBlur(gray, (5, 5), 0)
    # edged = cv.Canny(blurred, 75, 200)

    thr = cv.adaptiveThreshold(gray, 255, cv.ADAPTIVE_THRESH_GAUSSIAN_C, cv.THRESH_BINARY, 121, 2)
	
    if once:
        print("Image shape: ",thr.shape)

    # im2, contours, hierarchy = cv.findContours(thr, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE)
    # im2, contours, hierarchy = cv.findContours(thr, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_TC89_KCOS) #, cv.Point(0, 0))
    # im3 = cv.drawContours(image, contours, -1, (255,0,0), 3)
    # cv.imshow("Contours Image",im3)
	
    # Detect blobs.
    keypoints = detector.detect(thr)

    if once:
        print("keypoints", keypoints)

    # Draw detected blobs as red circles.
    # cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures the size of the circle corresponds to the size of blob
    # im_with_keypoints = cv.drawKeypoints(thr, keypoints, np.array([]), (0, 0, 255),
    #                                      cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)

    # work-around for drawKeypoints with drawMarker:
    imkp = thr.copy()
    for marker in keypoints:
        imkp = cv.drawMarker(imkp, tuple(int(i) for i in marker.pt), markerType=cv.MARKER_DIAMOND, color=(255, 0, 0))

    rawCapture.truncate(0)
    # display the image on screen and wait for a keypress
    cv.imshow("Camera Image", image)
    # cv.imshow("Greyscale Image",gray)
    # cv.imshow("Blurred Image",blurred)
    # cv.imshow("Edged Image",edged)
    # cv.imshow("thresholded image", thr)
    # cv.imshow("Image_with_Keypoints", im_with_keypoints)
    cv.imshow("Image_with_keypoint_Markers", imkp)
    # print("wait key")
    key = cv.waitKey(1)
    # print("key: "+str(key))
    if key == ord("q"):
        print("quit requested by keyboard input")
        break
    once = False