import cv2
import cv2.cv as cv
import numpy as np
import RPi.GPIO as GPIO
import time

kernel = np.ones((5,5),np.uint8)


cap = cv2.VideoCapture(-1)


cap.set(3,320)
cap.set(4,240)

def nothing(x):
    pass

cv2.namedWindow('HueComp')
cv2.namedWindow('SatComp')
cv2.namedWindow('ValComp')
cv2.namedWindow('closing')
cv2.namedWindow('tracking')


def motor() :
    

    channel = 18


    GPIO.setmode(GPIO.BCM)
    GPIO.setup(channel, GPIO.OUT)


    def motor_on(pin):
        GPIO.output(pin, GPIO.HIGH)  # Turn motor on


    def motor_off(pin):
        GPIO.output(pin, GPIO.LOW)  # Turn motor off


    if __name__ == '__main__':
        try:
            motor_on(channel)
            time.sleep(1)
        
            GPIO.cleanup()
        except KeyboardInterrupt:
            GPIO.cleanup()




cv2.createTrackbar('hmin', 'HueComp',12,179,nothing)
cv2.createTrackbar('hmax', 'HueComp',37,179,nothing)

cv2.createTrackbar('smin', 'SatComp',96,255,nothing)
cv2.createTrackbar('smax', 'SatComp',255,255,nothing)

cv2.createTrackbar('vmin', 'ValComp',186,255,nothing)
cv2.createTrackbar('vmax', 'ValComp',255,255,nothing)


# hmn = 12
# hmx = 37
# smn = 96
# smx = 255
# vmn = 186
# vmx = 255


while(1):

    buzz = 0
    _, frame = cap.read()

    
    hsv = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
    hue,sat,val = cv2.split(hsv)


    hmn = cv2.getTrackbarPos('hmin','HueComp')
    hmx = cv2.getTrackbarPos('hmax','HueComp')
    

    smn = cv2.getTrackbarPos('smin','SatComp')
    smx = cv2.getTrackbarPos('smax','SatComp')


    vmn = cv2.getTrackbarPos('vmin','ValComp')
    vmx = cv2.getTrackbarPos('vmax','ValComp')
    
    

    
    hthresh = cv2.inRange(np.array(hue),np.array(hmn),np.array(hmx))
    sthresh = cv2.inRange(np.array(sat),np.array(smn),np.array(smx))
    vthresh = cv2.inRange(np.array(val),np.array(vmn),np.array(vmx))

   
    tracking = cv2.bitwise_and(hthresh,cv2.bitwise_and(sthresh,vthresh))

    
    dilation = cv2.dilate(tracking,kernel,iterations = 1)
    closing = cv2.morphologyEx(dilation, cv2.MORPH_CLOSE, kernel)
    closing = cv2.GaussianBlur(closing,(5,5),0)

    
    circles = cv2.HoughCircles(closing,cv.CV_HOUGH_GRADIENT,2,120,param1=120,param2=50,minRadius=10,maxRadius=0)
    

    
    if circles is not None:
            for i in circles[0,:]:
                
                if int(round(i[2])) < 30:
                    cv2.circle(frame,(int(round(i[0])),int(round(i[1]))),int(round(i[2])),(0,255,0),5)
                    cv2.circle(frame,(int(round(i[0])),int(round(i[1]))),2,(0,255,0),10)
                    motor()
                    
                
                elif int(round(i[2])) > 35:
                    cv2.circle(frame,(int(round(i[0])),int(round(i[1]))),int(round(i[2])),(0,0,255),5)
                    cv2.circle(frame,(int(round(i[0])),int(round(i[1]))),2,(0,0,255),10)
                    buzz = 1
                    motor()

    
    
    
    cv2.imshow('HueComp',hthresh)
    cv2.imshow('SatComp',sthresh)
    cv2.imshow('ValComp',vthresh)
    cv2.imshow('closing',closing)
    cv2.imshow('tracking',frame)

    k = cv2.waitKey(1) & 0xFF
    if k == 27:
        break

cap.release()

cv2.destroyAllWindows()
