from machine import Pin,PWM
import utime
trig = 27
state_value = 0
IR_Pin  = Pin(19,Pin.IN,Pin.PULL_UP)
QTI_L=Pin(39,Pin.IN,Pin.PULL_UP)
QTI_R=Pin(36,Pin.IN,Pin.PULL_UP)
button = Pin(18, Pin.IN)
N=0
def getDistance(trig):
    trig = Pin(27,Pin.OUT)
    trig.value(0)
    utime.sleep_us(2)
    trig.value(1)
    utime.sleep_us(10)
    trig.value(0)
    trig = Pin(27,Pin.IN)
    while trig.value() == 0:
        start = utime.ticks_us()
    while trig.value() == 1:
        end = utime.ticks_us()
    d = (end - start) * 0.0343 / 2 
    return d
def motor(L12,L13,R14,R15):
    PWM(Pin(12)).duty(L12) 
    PWM(Pin(13)).duty(L13) 
    PWM(Pin(14)).duty(R14) 
    PWM(Pin(15)).duty(R15)
    
def Advance():
    distance = getDistance(trig)
    if distance < 20:
        motor(800,0,800,0)
    else:
        motor(0,800,0,800)
            
def Tracking():
    QTI_MAX = QTI_L.value() * 2 + QTI_R.value()
    if QTI_MAX == 0:
        motor(800,0,800,0)
    elif QTI_MAX == 1:
        motor(0,800,0,400)
    elif QTI_MAX == 2:
        motor(0,400,0,500)
    elif QTI_MAX == 3:
        motor(0,1000,0,1000)

def IR_remote_control(key_val):
    global state_value
    if(key_val==0x45):
        state_value = 1
    elif(key_val==0x46):
        state_value = 2
    elif(key_val==0x47):
        state_value = 3
    if state_value == 0:
        motor(0,0,0,0)
    
def Remote_Move(key_val):
    if(key_val==0x18):
        motor(0,1023,0,1023) # Go forward
    elif(key_val==0x08):
        motor(1023,0,0,1023) # Turn left
    elif(key_val==0x5a):
        motor(0,1023,1023,0) # Turn right
    elif(key_val==0x52):
        motor(1023,0,1023,0) # Go back
    else:
        motor(0,0,0,0) # Stop
            
if __name__ == '__main__':
    while button.value() == 1:
        pass
    else:
        while True:
            if IR_Pin.value() == 0:
                count = 0
                while IR_Pin.value() == 0 and count < 200:
                    count += 1
                    utime.sleep_us(60)
                count = 0
                while IR_Pin.value() == 1 and count < 80:
                    count += 1
                    utime.sleep_us(60)
                idx = 0
                cnt = 0
                data = [0,0,0,0]
                for i in range(0,32):
                    count = 0
                    while IR_Pin.value() == 0 and count < 15:
                        count += 1
                        utime.sleep_us(60)
                    count = 0
                    while IR_Pin.value() == 1 and count < 40:
                        count += 1
                        utime.sleep_us(60)
                    if count > 8:
                        data[idx] |= 1<<cnt
                    if cnt == 7:
                        cnt = 0
                        idx += 1
                    else:
                        cnt += 1
                if data[0]+data[1] == 0xFF and data[2]+data[3] == 0xFF:
    #                 print("Retrieve key: 0x%02x" %data[2])
                    N=data[2]
                IR_remote_control(N)
                if state_value == 1:
                    Remote_Move(N)
            if IR_Pin.value() == 1:
                motor(0,0,0,0)     # Stop
                state_value = state_value
            if state_value == 2:
                Advance()
            elif state_value == 3:
                Tracking()

