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

from gpiozero import LED

import cv2 as cv

feed = LED(17)								# the feed solenoid
switch = LED(18)							# the switch solenoid, on = flip piece, off = let piece face down

feed.off()
switch.off()

print("solenoids off")

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__)

majvers, minvers, updvers = cv.__version__.split('.')

majvers = int(majvers)
minvers = int(minvers)
updvers = int(updvers)

print ('OpenCV Version: {0}.{1}.{2}'.format(majvers,minvers,updvers));

# check opencv version and construct the detector
is_v2 = majvers in range(2,4)
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:
    # feed one piece from the hopper
    feed.on()
    time.sleep(0.2)
    feed.off()
    time.sleep(0.1)
	
    # 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