from rplidar import RPLidar
import pyautogui
from pyautogui import press, typewrite, hotkey
import keyboard
import math
import numpy
import curses

lidar = RPLidar('/dev/ttyUSB0')
 
info = lidar.get_info()
ang = 0;
dist = 10000;
viewAngle = 180
rangeLimit = 600
angleLimitRatio = 7
depthLimit = 10
dispWidth = 1920 / 2


spotAng = []
spotDist = []
spotNumber = []

targets = []

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

def addTarget(x, y):
    # array is x and y and than the active state and set flag
    global targets
    target = numpy.array([x,y,0,0])
    targets.append(target)
    
addTarget(330, 200)    
addTarget(100, 600)    
addTarget(-870, 1050)   

stdscr = curses.initscr()
curses.noecho()
curses.cbreak()

stdscr.addstr(0, 0,"LiDAR MultiSpot Tool. Timescale Technologies (c)2021")
stdscr.addstr(1, 0,"----------------------------------------------------")

#print(info)
 
#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: #Only "front" facing data
            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])
                     
                     if angDiff < angleLimitRatio: #if difference is less than 3 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]) * 3
        spotY[spots] = round(spotY[spots] / spotNumber[spots]) * 3
        # check spotTable for matches
        for target in range(len(targets)):
            if abs(targets[target][0] - spotX[spots]) < 100 and abs(targets[target][1] - spotY[spots]) < 100:
                if targets[target][2] < 3:
                    targets[target][2] += 2 # set target to active.
                    #do action
                    if targets[target][2] > 2 and targets[target][3] == 0:
                        tData = "Target:" + str(target) + " = On "
                        stdscr.addstr(7 + target, 0, tData)
                        press(chr(65 + target))
                        targets[target][3] = 1

    for target in range(len(targets)):
        #Check status of all targets.
        if targets[target][2] > 0:
            targets[target][2] -= 1 # reduce activity.
            if targets[target][2] == 0 and targets[target][3] == 1:
                tData = "Target:" + str(target) + " = Off"
                stdscr.addstr(7 + target, 0, tData)
                press(chr(97 + target))
                targets[target][3] = 0
                
    if (len(spotAng) > 0):
         coords= "x = " + str(round(spotX[0])) + " : y = " + str(round(spotY[0])) + "    "
         spotData = "Points :" + str(spotNumber[0]) + "   "
         groupData = "Groups : " + str(group + 1)
         stdscr.addstr(3, 0, coords)
         stdscr.addstr(4, 0, spotData)
         stdscr.addstr(5, 0, groupData)
             
         
    stdscr.refresh()
         #if dist < 750:
            #pyautogui.moveTo(560 + spotX[0] , spotY[0] - 200 )
    if keyboard.is_pressed("q"):
        curses.nocbreak()
        stdscr.keypad(False)
        curses.echo()
        curses.endwin()
        
        lidar.stop()
        lidar.stop_motor()
        lidar.disconnect()
        quit()

lidar.stop()
lidar.stop_motor()
lidar.disconnect()