from rplidar import RPLidar
import math
import numpy

lidar = RPLidar('/dev/ttyUSB0')
# lidar = RPLidar('COM1') # for windows

PI = math.pi
TAU = 2*PI

def getAngDiff(x, y):
    a = (x - y) % TAU
    b = (y - x) % TAU
    return -a if a < b else b
 
#info = lidar.get_info()
ang = 0;
dist = 10000;
viewAngle = 180
rangeLimit = 600
depthLimit = 10
dispWidth = 1920 / 2

spotAng = []
spotDist = []
spotNumber = []
averageX =  0
averageY = 0
counter = 0

#print(info)
print("")
print("LiDAR Spot Finder by Timescale Technologies (c) 2021")
print("----------------------------------------------------")
#health = lidar.get_health()
#print(health) 
for i, scan in enumerate(lidar.iter_scans(scan_type='express', min_len=100, max_buf_meas=False)):
    group = 0
    spotAng = []
    spotX = []
    spotY = []
    spotDist = []
    spotNumber = []
    pointcount = 0
    for x in range(len(scan)):
        if scan[x][1] > 270 or scan[x][1]< 90:
    
              if scan[x][2] < rangeLimit:
                # point in within scanning distance.
                ang = scan[x][1]
                dist = scan[x][2]
                if len(spotAng) -1 != group:
                     #first point detected, Add point to first array
                     spotAng.append(ang)
                     spotDist.append(dist)
                     spotNumber.append(1)
                     calcAng = round(ang - 270)
                     x = math.cos(math.radians(calcAng)) * dist
                     y = math.sin(math.radians(calcAng)) * dist
                     spotX.append(x)
                     spotY.append(y)
        
                else :
                     # still in same group.. Check for angle
                     angDiff = getAngDiff(ang,spotAng[group])
                     #print(angDiff)
                     if angDiff < 7: #if difference is less than x degrees
                         if abs(dist - spotDist[group]) < 40:
                             # distance is alo within the same area.
                             spotAng[group] = ang
                             spotNumber[group] += 1
                             spotDist[group] = dist
                             calcAng = round(ang - 270)
                             x = math.cos(math.radians(calcAng)) * dist
                             y = math.sin(math.radians(calcAng)) * dist
                             spotX[group] += x
                             spotY[group] += y

                         else:
                             group +=1
                     else:
                         #out of bounce
                         group += 1
    # Done. Now do the group processing.
    #print(len(spotNumber))
    for spots in range(len(spotNumber)):
        spotX[spots] = round(spotX[spots] / spotNumber[spots]) 
        spotY[spots] = round(spotY[spots] / spotNumber[spots])
    if (len(spotAng) > 0):
        print("x = " + str(round(spotX[0])) + " : y = " + str(round(spotY[0])))
        averageX += spotX[0]
        averageY += spotY[0]
        counter += 1
        if counter == 10:
            averageX = averageX / 10
            averageY = averageY / 10
            print("---------------------")
            print("x = " + str(round(averageX)) + " : y = " + str(round(averageY)))
            lidar.stop()
            lidar.stop_motor()
            lidar.disconnect()
            quit()