import RPi.GPIO as GPIO
import time
from AlphaBot2 import AlphaBot2

Ab = AlphaBot2()

TRIG = 22
ECHO = 27

IR = 17
PWM = 50
n=0

dist =  0

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

# IR remote input
GPIO.setup(IR,GPIO.IN)

# Ultrasonic sensor
GPIO.setup(TRIG,GPIO.OUT,initial=GPIO.LOW)
GPIO.setup(ECHO,GPIO.IN)

def Distance():
    GPIO.output(TRIG,GPIO.HIGH)
    time.sleep(0.000015)
    GPIO.output(TRIG,GPIO.LOW)
    while not GPIO.input(ECHO):
        pass
    t1 = time.time()
    while GPIO.input(ECHO):
        pass
    t2 = time.time()
    return (t2-t1)*34000/2

def getkey():
    if GPIO.input(IR) == 0:
        count = 0
        while GPIO.input(IR) == 0 and count < 200:  #9ms
            count += 1
            time.sleep(0.00006)
        if(count < 10):
            return;
        count = 0
        while GPIO.input(IR) == 1 and count < 80:  #4.5ms
            count += 1
            time.sleep(0.00006)

        idx = 0
        cnt = 0
        data = [0,0,0,0]
        for i in range(0,32):
            count = 0
            while GPIO.input(IR) == 0 and count < 15:    #0.56ms
                count += 1
                time.sleep(0.00006)
                
            count = 0
            while GPIO.input(IR) == 1 and count < 40:   #0: 0.56mx
                count += 1                               #1: 1.69ms
                time.sleep(0.00006)
                
            if count > 7:
                data[idx] |= 1<<cnt
            if cnt == 7:
                cnt = 0
                idx += 1
            else:
                cnt += 1
#       print data
        if data[0]+data[1] == 0xFF and data[2]+data[3] == 0xFF:  #check
            return data[2]
        else:
            print("repeat")
            return "repeat"

            
print('IRremote Test Start ...')
Ab.stop()
try:
    while True:
        key = getkey()
        dist = Distance()            
        if(key != None):
            n = 0   
            if key == 0x18:
                if dist < 20:
                    Ab.stop()
                    print("obstabcle")
                else:
                    Ab.forward()
                    print("forward")
            if key == 0x08:
                Ab.left()
                print("left")
            if key == 0x1c:
                Ab.stop()
                print("stop")
            if key == 0x5a:
                Ab.right()
                print("right")
            if key == 0x52:
                Ab.backward()       
                print("backward")
            if key == 0x15:
                if(PWM + 10 < 101):
                    PWM = PWM + 10
                    Ab.setPWMA(PWM)
                    Ab.setPWMB(PWM)
                    print(PWM)
            if key == 0x07:
                if(PWM - 10 > -1):
                    PWM = PWM - 10
                    Ab.setPWMA(PWM)
                    Ab.setPWMB(PWM)
                    print(PWM)
        else:
            if dist < 20:
                n = 0
                Ab.stop()
                print("run into obstabcle")
            else:     
                n += 1
                if n > 20000:
                    n = 0
                    Ab.stop()   
except KeyboardInterrupt:
    GPIO.cleanup();