from picamera import PiCamera
from time import sleep
import numpy as np
import cv2
import pigpio

def nothing(*x):
    pass

freq=1000

blue_enable=21
tisch=25
ir_pwm=24
blue_pwm=23
fokus=18
gp=pigpio.pi()
gp.set_mode(blue_enable,pigpio.OUTPUT)
gp.set_mode(tisch,pigpio.OUTPUT)
gp.set_mode(blue_pwm,pigpio.OUTPUT)
gp.set_PWM_range(blue_pwm,4000)
gp.set_mode(ir_pwm,pigpio.OUTPUT)
gp.set_PWM_range(ir_pwm,4000)
gp.set_PWM_frequency(blue_pwm, 1000)
gp.set_PWM_frequency(ir_pwm, 1000)

font = cv2.FONT_HERSHEY_SIMPLEX
cv2.namedWindow('detection',cv2.WINDOW_KEEPRATIO)
cv2.namedWindow('go_fuckyourself',cv2.WINDOW_KEEPRATIO)
cv2.createTrackbar('g','detection',30,179,nothing)
cv2.createTrackbar('d','detection',15,179,nothing)
cv2.createTrackbar('ul','detection',0,255,nothing)
cv2.createTrackbar('ol','detection',255,255,nothing)
cv2.createTrackbar('blue_enable','go_fuckyourself',0,1,nothing)
cv2.createTrackbar('tisch','go_fuckyourself',0,1,nothing)
cv2.createTrackbar('blue_pwm','go_fuckyourself',4000,4000,nothing)
cv2.createTrackbar('ir_pwm','go_fuckyourself',0,4000,nothing)
cv2.createTrackbar('fokus','go_fuckyourself',0,100,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')
old_dt=0
while key != ord("q")&0xFF:
    sleep(0.025)
    blue_on=cv2.getTrackbarPos('blue_enable','go_fuckyourself')
    tisch_on=cv2.getTrackbarPos('tisch','go_fuckyourself')
    blue_dt=cv2.getTrackbarPos('blue_pwm','go_fuckyourself')
    ir_dt=cv2.getTrackbarPos('ir_pwm','go_fuckyourself')
    fokus_dt=cv2.getTrackbarPos('fokus','go_fuckyourself')
    gp.write(blue_enable,blue_on)
    gp.write(tisch,tisch_on)
    gp.set_PWM_dutycycle(blue_pwm, blue_dt)
    gp.set_PWM_dutycycle(ir_pwm, ir_dt)
    if fokus_dt!=old_dt:
        gp.hardware_PWM(fokus,28000,fokus_dt*10000)
        old_dt=fokus_dt
    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('ul','detection')
    v=cv2.getTrackbarPos('ol','detection')

    
    
    imgHSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    imgThreshLow = cv2.inRange(imgHSV, np.array([g, s, s]), np.array([g+d, v, v]))
    res = cv2.bitwise_and(img,img, mask= imgThreshLow)
    cv2.imshow("detection", res)
cv2.destroyAllWindows()
#gp.set_mode(gpio,pigpio.INPUT)
camera.close()
gp.set_mode(blue_enable,pigpio.INPUT)
gp.set_mode(blue_pwm,pigpio.INPUT)
gp.set_mode(ir_pwm,pigpio.INPUT)
gp.set_mode(tisch,pigpio.INPUT)
gp.hardware_PWM(fokus,28000,0)
gp.stop()
cv2.imwrite("capture.png",img)
