from rplidar import RPLidar
import RPi.GPIO as GPIO

def CheckStatus(FrontArray, LeftArray, RightArray):
    FrontObstacle = 0
    LeftObstacle = 0
    RightObstacle = 0
    for x in FrontArray:
        if (x[0] >5) and (x[2] <400):
            FrontObstacle =1
    for y in LeftArray:
        if (y[0] >5) and (y[2] <400):
                LeftObstacle = 1
    for z in RightArray:
        if (z[0] >5) and (z[2] <400):
            RightObstacle = 1
    if (FrontObstacle == 0):
        GoForward()
    elif (LeftObstacle==0):
        GoLeft()
    elif (RightObstacle==0):
        GoRight()
    else:
        Stop()

def GoForward():
    print('Going Forward')
    pwm.ChangeDutyCycle(30)
    GPIO.output(Motor2E,GPIO.LOW)

def GoLeft():
    print('Going Left')
    pwm.ChangeDutyCycle(30)
    GPIO.output(Motor2A,GPIO.LOW)
    GPIO.output(Motor2B,GPIO.HIGH)
    GPIO.output(Motor2E,GPIO.HIGH)

def GoRight():
    print('Going Right')
    pwm.ChangeDutyCycle(30)
    GPIO.output(Motor2A,GPIO.HIGH)
    GPIO.output(Motor2B,GPIO.LOW)
    GPIO.output(Motor2E,GPIO.HIGH)

def Stop():
    print('Stopping')
    pwm.ChangeDutyCycle(0)
    GPIO.output(Motor2E,GPIO.LOW)

GPIO.setmode(GPIO.BOARD)
Motor1A = 16
Motor1B = 18
Motor1E = 12
Motor2A = 21
Motor2B = 23
Motor2E = 19
GPIO.setup(Motor1A,GPIO.OUT)
GPIO.setup(Motor1B,GPIO.OUT)
GPIO.setup(Motor1E,GPIO.OUT)
GPIO.setup(Motor2A,GPIO.OUT)
GPIO.setup(Motor2B,GPIO.OUT)
GPIO.setup(Motor2E,GPIO.OUT)
pwm=GPIO.PWM(Motor1E, 100)
pwm.start(30)

PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
i=0
left=[];
right=[];
front=[];
try:
    print('Recording measurments... Press Crl+C to stop.')
    for measurment in lidar.iter_measurments():
        #line = '\t'.join(str(v) for v in measurment)
        isNewMeasurment = measurment[0]
        signalHealth = measurment[1]
        degrees = int(round(measurment[2],0))
        distance = int(round(measurment[3],0))
        #print(str(isNewMeasurment) + str(signalHealth) + str(degrees) + str(distance) + '\n')
        if (degrees>325) or (degrees<35):
            front.append([signalHealth, degrees, distance])
        elif (degrees>270) and (degrees<325):
            left.append([signalHealth, degrees, distance])
        elif (degrees>35) and (degrees<90):
            right.append([signalHealth, degrees, distance])
        if (degrees==357) or (degrees==358) or (degrees==359):
            CheckStatus(front, left, right)
            left[:] = []
            right[:] = []
            front[:] = []

except KeyboardInterrupt:
    print('Stoping.')

lidar.stop()
lidar.stop_motor()
lidar.disconnect()
GPIO.output(Motor1E,GPIO.LOW)
GPIO.cleanup()