import wiringpi as wp
import Queue
import time
import threading

wp.wiringPiSetup()
AInput = 2
BInput = 0
CInput = 7
DInput = 6

RState = 0
LState = 0



def GetStepsRight():
    global Rsteps
    RlastState = 0
    Rsteps = 0
    wp.pullUpDnControl(AInput, 2)
    wp.pullUpDnControl(BInput, 2)
    while True:
        AState = wp.digitalRead(AInput)
        BState = wp.digitalRead(BInput) << 1
        RState = AState | BState
        if RlastState != RState:
            if RState == 0:
                if RlastState == 2:
                    Rsteps += 1
                elif RlastState == 3:
                    Rsteps -= 1
            elif RState == 2:
                if RlastState == 3:
                    Rsteps += 1
                elif RlastState == 0:
                    Rsteps -= 1
            elif RState == 3:
                if RlastState == 0:
                    Rsteps += 1
                elif RlastState == 2:
                    Rsteps -= 1
            RlastState = RState
            #print(RState)
        time.sleep(0.001)


def GetStepsLeft():
    global Lsteps
    Lsteps = 0
    LlastState =  0
    wp.pullUpDnControl(CInput, 2)
    wp.pullUpDnControl(DInput, 2)
    while True:
        CState = wp.digitalRead(CInput)
        DState = wp.digitalRead(DInput) << 1
        LState = CState | DState
    #print(CState, DState)
        if LlastState != LState:
            if LState == 0:
                if LlastState == 2:
                    Lsteps += 1
                elif LlastState == 3:
                    Lsteps -= 1
            elif LState == 2:
                if LlastState == 3:
                    Lsteps += 1
                elif LlastState == 0:
                    Lsteps -= 1
            elif LState == 3:
                if LlastState == 0:
                    Lsteps += 1
                elif LlastState == 2:
                    Lsteps -= 1
            LlastState = LState
            #print(LState)
            #out_queue.put(Lsteps)
    time.sleep(0.001)