from picamera.array import PiRGBArray
from picamera import PiCamera
import cv2
import numpy as np
import time
from imutils.video.pivideostream import PiVideoStream
from imutils.video import FPS

RED_REJECT = 0.5
BLUE_REJECT = 0.5
THRESHOLD = 30

MAX_VERTICAL_SEPARATION = 100
MAX_HORIZONTAL_SEPARATION = 100

#img = cv2.imread("C:\\Users\\carlosj\\Documents\\FRC\\FRC2017\\2017VisionExample\\Vision Images\\LED Boiler\\1ftH5ftD2Angle0Brightness_challenge.jpg")
#cap = cv2.VideoCapture(0)
#if not cap.isOpened():
#    cap.open()
#camera = PiCamera()
#camera.resolution = (320, 240)
#camera.framerate=32
#rawCap = PiRGBArray(camera, size=(320,240))
#time.sleep(0.1)
#stream = camera.capture_continuous(rawCap, format="bgr",
#	use_video_port=True)

vs = PiVideoStream(framerate=60)
vs.start()
vs.camera.awb_mode='off'
vs.camera.awb_gains = (0.9, 3)
vs.camera.shutter_speed=7000
time.sleep(1.0)
fps = FPS().start()

try:
    while True:
        #ret, rawimg = cap.read()
        #camera.capture(rawCap, format="bgr")
	rawimg = vs.read()
	#gains= vs.camera.awb_gains
	#print float(gains[0]), float(gains[1])
	#cv2.imshow('rawimage', rawimg)

        b,g,r = cv2.split(rawimg)
        summed = cv2.addWeighted(b,BLUE_REJECT, r, RED_REJECT,0)

        diff=cv2.subtract(g, summed)
        #cv2.imshow('diff', diff)
        retval,thresholded = cv2.threshold(diff, THRESHOLD, 255, cv2.THRESH_BINARY)
        #cv2.imshow('thresholded', thresholded)
        params = cv2.SimpleBlobDetector_Params()
        params.blobColor=255
        params.filterByColor=True
        params.minThreshold=128
        params.maxThreshold=128
        params.filterByCircularity=False
        params.filterByConvexity=False
        params.filterByInertia=False
        params.filterByArea=True
        params.minArea=100
        params.maxArea = 5000

        detector = cv2.SimpleBlobDetector(params)

        keypoints = detector.detect(thresholded)
        validPairs = []
        keypoints = set(keypoints)
        for i,keypoint1 in enumerate(keypoints):
            sublist = keypoints[i+1:]
            for keypoint2 in sublist:
                if (abs(keypoint1.pt[0]-keypoint2.pt[0]) < MAX_HORIZONTAL_SEPARATION) \
                   and (abs(keypoint1.pt[1]-keypoint2.pt[1]) < MAX_VERTICAL_SEPARATION):
                    validPairs.append((keypoint1, keypoint2))
        
	ock = False
        if len(validPairs) == 1:
            lock = True

        #All blobs in valid pairs
        pairBlobs = set()
        for pair in validPairs:
            pairBlobs.add(pair[0])
            pairBlobs.add(pair[1])

        #Draw valid blobs
        nonPairedBlobs = set(keypoints).difference(pairBlobs)
        #img_with_keypoints = cv2.drawKeypoints(rawimg, list(nonPairedBlobs), np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)

        #Draw valid pairs
        #if lock:
        #    img_with_keypoints = cv2.drawKeypoints(img_with_keypoints, list(pairBlobs), np.array([]), (255,0,0), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
        #else:
        #    img_with_keypoints = cv2.drawKeypoints(img_with_keypoints, list(pairBlobs), np.array([]), (0,127,127), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)

        #cv2.imshow('final', img_with_keypoints)
        #thisTime = time.clock()
	#interval = thisTime-oldTime
	#if interval > 0:
#		print 1./interval
	#oldTime = thisTime
	fps.update()
	#rawCap.truncate(0)
	if cv2.waitKey(1) & 0xFF == ord('q'):
            break
except:
    #cap.release()
    fps.stop()
    print fps.fps()
    raise

#cap.release()
fps.stop()
print fps.fps()
cv2.destroyAllWindows()
vs.stop()