import cv2
import numpy as np
import serial
from PiRapidStream import PiRapidStream
from time import sleep
from multiprocessing.dummy import Pool
from TheSquaire import TheSquaire
import pigpio

###################################################################################################

freq=1000
blue_enable=21
tisch=25
ir_pwm=24
blue_pwm=23

def nothing(x):
    pass

def main():
    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, 500)
    gp.set_PWM_dutycycle(ir_pwm, 4000)
    #default res (576,512)
    vs = PiRapidStream(resolution=(576,512),fps=30,form='yuv',exp=30000,gain=1.15)
    vs.startcapt()
    cv2.namedWindow("detection", cv2.WINDOW_AUTOSIZE)
    minr=85
    maxr=88
    #upc=200
    cv2.createTrackbar('minr','detection',81,255,nothing)
    cv2.createTrackbar('maxr','detection',86,255,nothing)
    cv2.createTrackbar('accres','detection',190,1000,nothing)
    cv2.createTrackbar('upcanny','detection',200,255,nothing)
    cv2.createTrackbar('thres','detection',80,255,nothing)
    p=Pool(processes=4)
    ts=TheSquaire()
    
    ser = serial.Serial("/dev/ttyACM0", 115200, timeout = 1)
    print(ser.readall());
    key=cv2.waitKey(1)
    while key != ord("q") and vs.stream_done==False:            
        img = vs.frame            
        grau=cv2.medianBlur(img,1)
        minr=cv2.getTrackbarPos('minr','detection')
        maxr=cv2.getTrackbarPos('maxr','detection')
        accres=cv2.getTrackbarPos('accres','detection')/100.0
        upc=cv2.getTrackbarPos('upcanny','detection')
        th=cv2.getTrackbarPos('thres','detection')
        key=cv2.waitKey(1)
        if key==ord('w'):
            print "forward"
            ser.write("G0 X -10 F 4000\n")
        
        if key==ord('a'):
            print "left"
            ser.write("G0 Y -10 F 4000\n")
            
        circ=ts.kreise(x=(grau,minr,maxr,accres,upc,th))
        ts.zeichne_kreise(result=circ,dst=[img])
        #result=p.map(ts.kreise,[(grau,38,43,2.3,60,120),(grau,30,33,1,80,40),(grau,19,23,1.4,60,30),(grau,8,14,1.9,80,40)],chunksize=1)
        last_command=[0,0]
        if circ is not None:
            command=np.multiply((circ[0][0:2]-[int(576/2),int(512/2)-79]),250/576)
            diff=np.subtract(last_command,command)
            last_command=command
            if np.abs(diff[0])<4:
                command[0]=0
            if np.abs(diff[1])<4:
                command[1]=0
            
            #print((circ[0][0:2]-[int(576/2),int(512/2)])*250/576);
            #ser.write("G1 X {} Y {} F 4000\n".format(-command[1],command[0]))
            length=np.sqrt(np.power(command[1],2)+np.power(command[0],2))
            time=length*60.0/4000
            sleep(time)
            
            #ser.write("G0 X {} Y {}\n")
            print("G0 X {} Y {}\n".format(-command[1],command[0]))

            #print(ser.readline())
                
       
        cv2.imshow('detection',img)


    cv2.destroyAllWindows()
    p.close()
    p.join()
    vs.stopcapt()
    ser.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.stop()

    print "quit"

    return

###################################################################################################
if __name__ == "__main__":
    main()
