import cv2
from Autofokus import Autofokus
from PiRapidStream import PiRapidStream
from TheSquaire import TheSquaire
from time import sleep
import pigpio
import serial
freq=1000
blue_enable=21
tisch=25
ir_pwm=24
blue_pwm=23
def nothing(*x):
    pass
cv2.namedWindow("preview",cv2.WINDOW_AUTOSIZE)
cv2.createTrackbar('ir_pwm','preview',0,4000,nothing)
cv2.createTrackbar('fokus','preview',0,100,nothing)

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)
gp.write(tisch,1)
vs = PiRapidStream(resolution=(576,512),fps=30,form='yuv',exp=30000,gain=1.15)
vs.startcapt()
af=Autofokus()
af.start_focus([vs],[gp])
key=cv2.waitKey(1)
old_ir_dt=0
old_fokus=0
x,y,z=0,0,0
ser = serial.Serial("/dev/ttyACM0", 115200, timeout = 1)
print(ser.readall());
ser.write("G28\n")
while key!=ord('q')&0xFF:
    ir_dt=cv2.getTrackbarPos('ir_pwm','preview')
    fokus_dt=cv2.getTrackbarPos('fokus','preview')
    src=vs.frame
    cv2.imshow("preview",vs.frame)
    
    if old_ir_dt!=ir_dt:
        gp.set_PWM_dutycycle(ir_pwm, ir_dt)
        old_ir_dt=ir_dt
    if fokus_dt!=old_fokus:
        gp.hardware_PWM(18,28000,fokus_dt*10000)
        old_dt=fokus_dt
    key=cv2.waitKey(1)
    if key==ord('g'):
        af.global_search()
        cv2.setTrackbarPos('fokus','preview',af.best_duty)
    if key==ord('w'):
        x=x+5
        ser.write("G1 X {} F 3000\n".format(x))
    if key==ord('s'):
        x=x-5
        ser.write("G1 X {} F 3000\n".format(x))
        
    if key==ord('a'):
        y=y-5
        ser.write("G1 Y {} F 3000\n".format(y))
    if key==ord('d'):
        y=y+5
        ser.write("G1 Y {} F 3000\n".format(y))
    if key==ord('y'):
        z=z+5
        ser.write("G1 Z {} F 777\n".format(z))
    if key==ord('x'):
        z=z-5
        ser.write("G1 Z {} F 777\n".format(z))
    if key==ord('p'):
        #print af.findsharpness_post(af.findsharpness_prior(src))
        cv2.imwrite("shit.png",src)

af.stop_focus()
vs.stopcapt()
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.stop()
ser.close()
