import rplidar
from rplidar import RPLidar
from threading import Thread
from persontracking import algorithms
import zmq

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, port, measurements=100):
        self.port = port
        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.
        '''

        # Create the 0MQ socket first. This should not be passed between threads.
        self._socket = self._create_socket()
        self._socket.bind("tcp://*:" + str(self.port))

        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):
        # Send the updated groups to 0MQ socket.
        # self._socket.send_multipart(["lidar_map", ...])
        pass

    def stop_scanning(self):
        self.run = False

    def _create_socket(self):
        return zmq.Context.instance().socket(zmq.PUB)