from picamera import PiCamera
from time import sleep
import numpy as np
import cv2
import pigpio
freq=1000
dt=3700
gpio=23
def nothing(*x):
    pass

gp=pigpio.pi()
gp.set_mode(gpio,pigpio.OUTPUT)
gp.set_PWM_range(gpio,4000)
gp.set_PWM_dutycycle(gpio, dt)
gp.set_PWM_frequency(gpio, freq)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.namedWindow('detection',cv2.WINDOW_KEEPRATIO)
cv2.createTrackbar('g','detection',30,179,nothing)
cv2.createTrackbar('d','detection',15,179,nothing)
cv2.createTrackbar('s','detection',70,255,nothing)
cv2.createTrackbar('v','detection',70,255,nothing)
resolution=(2000,2000)
w=resolution[0]
h=resolution[1]
fw=(w+31)//32*32
fh=(h+15)//16*16
gain=1.0
iso=800
image = np.empty((fh*fw*3,), dtype=np.uint8)
img = image.reshape((fh, fw, 3))
camera = PiCamera(resolution=(fw,fh), framerate=0.1)
#camera.CAPTURE_TIMEOUT = 60
camera.awb_mode='off'
camera.exposure_mode = 'off'
camera.shutter_speed = 10*1000000
camera.awb_gains=gain
camera.iso = iso
key=cv2.waitKey(1)
#img=cv2.imread('dark.png')

while key != ord("q")&0xFF:
    key=cv2.waitKey(1)

    if(key == ord("c")):
        print('capturing...')
        camera.capture(image, format='bgr')
        #camera.capture('dark.png')
        
        print('...done.')
        img = image.reshape((fh, fw, 3))

    #cv2.putText(output,'es ist fumpf uhr fumpfundfumpfzig',(10,40), font,0.5,(255,255,255),1,cv2.LINE_AA)
    
    g=cv2.getTrackbarPos('g','detection')
    d=cv2.getTrackbarPos('d','detection')
    s=cv2.getTrackbarPos('s','detection')
    v=cv2.getTrackbarPos('v','detection')

    
    
    imgHSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    imgThreshLow = cv2.inRange(imgHSV, np.array([g, s, v]), np.array([g+d, 255, 255]))
    res = cv2.bitwise_and(img,img, mask= imgThreshLow)
    cv2.imshow("detection", res)
cv2.destroyAllWindows()
#gp.set_mode(gpio,pigpio.INPUT)
camera.close()
