import sys
import signal
import wiringpi as wp
import encoder as enc
import threading as th
import time
import Queue

pwmpinA = 26 #A
pwmpinB = 24 #B
pwmB = 23 #Signalo A generatorius
pwmA = 1 #Signalo B generatorius
wp.wiringPiSetup()
daugiklis = input("metrai")
atstumas = 174*daugiklis
atstumasR = 174*daugiklis

freqDiv = 1200
def movingMotor(pwm, pwmPin, on, speed):
    if on:
        wp.pinMode(pwmPin, 2)
        wp.pwmWrite(pwm, speed)
    else:
        wp.pinMode(pwmPin, 1)
        wp.pwmWrite(pwm, 0)

def signal_handler(sig, frame):
    movingMotor(pwmpinA, pwmA, False, 0)
    movingMotor(pwmpinB, pwmB, False, 0)
    print("Exit")
    sys.exit(0)

signal.signal(signal.SIGINT, signal_handler)
left_queue = Queue.Queue()
right_queue = Queue.Queue()

left = th.Thread(target = enc.GetStepsLeft)
left.daemon = True
right = th.Thread(target = enc.GetStepsRight)
right.daemon = True
left.start()
right.start()
left_value = 0
right_value = 0
cmd = 1
#for i in  range(10):
 #   movingMotor(pwmpinA,pwmA,True,57*i)
  #  movingMotor(pwmpinB,pwmB,True,60*i)
   # time.sleep(0.1)
while left_value < atstumas or right_value *-1< atstumasR:
    #cmd = int(input("input "))
    if cmd == 1:
        on = True


        movingMotor(pwmpinA, pwmA, on, 732) #Right  Wheel
                #time.sleep(0.005)
        movingMotor(pwmpinB, pwmB, on, 740) #Left  Wheel
    elif cmd == 0:
        on = False
        movingMotor(pwmpinA, pwmA, on, 0)
        movingMotor(pwmpinB, pwmB, on, 0)
    left_value = enc.Lsteps
    right_value = enc.Rsteps
    #print(praslydimas)
    print(enc.Lsteps , right_value *-1)
    time.sleep(0.05)
movingMotor(pwmpinA, pwmA, False, 0)
movingMotor(pwmpinB, pwmB, False, 0)