import rplidar
from rplidar import RPLidar
from threading import Thread
from tracking import algorithms
import tracking.lidar_tracker_pb2 as tracker_pb
import zmq
import Messaging.message_factory as mf
import Messaging.messages as messages


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 = []
    port = None

    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):
        if self.port is None:
            print('ERROR: Port has not been set!')
            return
        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._mFactory = mf.getZmqPubSubStreamer(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._mFactory.send_message_topic("lidar_map", messages.ProtoMessage(
            message=tracker_pb.PointScan(points=[]).SerializeToString()))

    def stop_scanning(self):
        self.run = False