import rplidar
from rplidar import RPLidar
from threading import Thread
import algorithms

class LidarCache():
    '''
    A class that retrieves scans from the lidar,
    runs grouping algorithms between scans and 
    keeps a copy of the group data.
    '''
    run = True
    tracking_group_number = -1
    currentGroups = None
    groupsChanged = []
    
    def __init__(self, measurements=100):
        self.lidar = RPLidar('/dev/ttyUSB0')
        self.measurements = measurements
        print('Info: ' + self.lidar.get_info())
        print('Health: ' + self.lidar.get_health())

    def start_cache(self):
        self.thread = Thread(target=self.do_scanning)
        self.thread.start()

    def do_scanning(self):
        '''
        Performs a scan for the given number of iterations.
        '''
        for i, scan in enumerate(self.lidar.iter_scans(min_len=self.measurements)):
            print('%d: Got %d measurments' % (i, len(scan)))
            if(not self.run):
                break

            # Now process the groups. 
            if self.currentGroups is not None:
                self.currentGroups = algorithms.assign_groups(self.currentGroups, algorithms.calc_groups(scan))
            else:
                self.currentGroups = algorithms.calc_groups(scan)

    def fireGroupsChanged(self):
        pass

    def stop_scanning(self):
        self.run = False