import pigpio
from time import sleep
import cv2
from PiRapidStream import PiRapidStream


freq=1000

blue_enable=21
tisch=25
ir_pwm=24
blue_pwm=23
fokus=18
gp=pigpio.pi()
def nothing(*x):
    pass
cv2.namedWindow("go_fuckyourself",cv2.WINDOW_AUTOSIZE)
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)
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)

vs = PiRapidStream(resolution=(320,240),fps=30,form='yuv',exp=40000,gain=1.15)
vs.startcapt()
key=cv2.waitKey(1)
old_dt=0
while key!=ord('q'):
    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
    src=vs.frame
    cv2.imshow("go_fuckyourself",src)
    key=cv2.waitKey(1)

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()
vs.stopcapt()
cv2.destroyAllWindows()
