import RPi.GPIO as GPIO
import time
import serial

ser = serial.Serial("/dev/ttyAMA0", 115200) #ttysomething
GPIO.setmode (GPIO.BCM)

ControlPin = [21,20,16,12]                   #pins for LiDAR stepper motor

cc_seq = [ [1,0,0,0],                          #sequence of magnets to spin motor
        [1,1,0,0],
        [0,1,0,0],
        [0,1,1,0],
        [0,0,1,0],
        [0,0,1,1],
        [0,0,0,1],
        [1,0,0,1] ]

c_seq = [ [1,0,0,1],
          [0,0,0,1],
          [0,0,1,1],
          [0,0,1,0],
          [0,1,1,0],
          [0,1,0,0],
          [1,1,0,0],
          [1,0,0,0], ]

def LiScan():
    for pin in ControlPin:
        GPIO.setup (pin, GPIO.OUT)          #set pins to GPIO Out
        GPIO.output (pin, 0)                #default output = 0

    dataPoints = 0
    for i in range (256):                   #half revolution (512 steps for 360*)
        for halfstep in range (8):          #each sequence of halfstep
            for pin in range (4):           #each magnet
                GPIO.output (ControlPin[pin], cc_seq[halfstep][pin])
                                            #output to wanted sequence
                count = ser.in_waiting      #how many bytes of data in through LiDAR
                time.sleep (0.00015)        #time for armature to set (smaller is quicker, less reliable)
                if count > 8:               #9 bytes in buffer is full
                    recv = ser.read(9)      #read bytes, put into recv
                    ser.reset_input_buffer()
                                          #reset the buffer

                    if recv[0] == 0x59 and recv[1] == 0x59:
                                            #proper if bits 0 and 1 are 89
                        distance = recv[2] + recv[3] * 256
                        strength = recv[4] + recv[5] * 256
                        print('(', distance, ',', strength, ')')
                                            #can skip the print if don't need
                        dataPoints=dataPoints+1
                        ser.reset_input_buffer()
                                            #reset the buffer

    for i in range (256):                   #same as before, but reverse sequence
        for halfstep in range (8):
            for pin in range (4):
                GPIO.output (ControlPin[pin], c_seq[halfstep][pin])
                time.sleep(0.00015)

    GPIO.cleanup()                          #reset GPIO pins to end
    #return distance
    print("Number of Data Points: ",dataPoints)

if __name__ == '__main__':
    try:
        if ser.is_open == False:
            ser.open()
        LiScan()
    except KeyboardInterrupt:   # Ctrl+C
        if ser != None:
            ser.close()