import io
import cv2
import numpy as np
import os
from picamera import PiCamera
import time
from threading import Thread
###################################################################################################

def nothing(x):
    pass

w=480
h=480
fw=(w+31)//32*32
fh=(h+15)//16*16
Y=None
camera=PiCamera()
camera.resolution = (w, h)
camera.framerate = 40
camera.exposure_mode='off'
camera.awb_mode='off'
camera.awb_gains=1
camera.shutter_speed=15000
stream_done=False
cv2.namedWindow("detection", cv2.WINDOW_AUTOSIZE)
cv2.createTrackbar('param1','detection',80,255,nothing)
cv2.createTrackbar('param2','detection',40,255,nothing)
cv2.createTrackbar('accu','detection',125,255,nothing)
cv2.createTrackbar('blur','detection',0,255,nothing)
cv2.createTrackbar('minR','detection',8,255,nothing)
cv2.createTrackbar('maxR','detection',45,255,nothing)
cv2.createTrackbar('gain','detection',100,500,nothing)
cv2.createTrackbar('speed','detection',15000,100000,nothing)
t=None


def outputs():
    global Y
    stream = io.BytesIO()
    while not stream_done:
        # This returns the stream for the camera to capture to
        yield stream
       
        stream.seek(0)
        Y=np.fromstring(stream.getvalue(),dtype=np.uint8,count=fw*fh).\
           reshape((fh,fw))
        # Finally, reset the stream for the next capture
        stream.seek(0)
        stream.truncate()

def capt():
    camera.capture_sequence(outputs(), 'yuv', use_video_port=True)
    pass
    
def startcapt():
    global t
    t = Thread(target=capt, args=())
    t.daemon = True
    t.start()

       

###################################################################################################
if __name__ == "__main__":
    startcapt()
    time.sleep(0.5)

    while cv2.waitKey(1) != ord("q"):
        gain=cv2.getTrackbarPos('gain','detection')
        speed=cv2.getTrackbarPos('speed','detection')
        p1=cv2.getTrackbarPos('param1','detection')
        p2=cv2.getTrackbarPos('param2','detection')
        accu=cv2.getTrackbarPos('accu','detection')
        blur=cv2.getTrackbarPos('blur','detection')
        minR=cv2.getTrackbarPos('minR','detection')
        maxR=cv2.getTrackbarPos('maxR','detection')
        camera.awb_gains=gain/100.0
        camera.shutter_speed=speed
        frame=Y
        grau=cv2.medianBlur(frame,3)
        circles = cv2.HoughCircles(grau, cv2.HOUGH_GRADIENT, dp=(accu/100.0)+1, minDist=30,param1=p1,param2=p2,maxRadius=maxR,minRadius=minR)
        if circles is not None:
            circles=np.round(circles[0,:]).astype("int")
            for (x,y,r) in circles:
                cv2.circle(frame,(x,y),r,(0,255,0),4)
                cv2.rectangle(frame,(x-5,y-5),(x+5,y+5),(0,128,255),-1)
                #cv2.line(frame,(400,304),(x,y),(0,0,255),3)
        cv2.imshow("detection", frame)              

    stream_done=True
    time.sleep(0.5)
    camera.close()
    cv2.destroyAllWindows()
    t.join()
