from threading import Thread
from tracking import algorithms
import tracking.lidar_tracker_pb2 as tracker_pb
import zmq
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.
    """

    def __init__(self, lidar, measurements=100):
        self.lidar = lidar
        self.measurements = measurements
        print('Info: ' + self.lidar.get_info())
        print('Health: ' + self.lidar.get_health())
        self.run = True
        self.tracking_group_number = -1
        self.currentGroups = None

    def start_cache(self, sender):
        self.thread = Thread(target=self.do_scanning, args=[sender])
        self.thread.start()

    def do_scanning(self, sender):
        """Performs scans whilst cache is running, and will pass calculated groups data to the sender.

        Parameters
        ----------
        sender: 
            Any class given in messaging.message_factory. This acts as a listener.

        """
        # Create the 0MQ socket first. This should not be passed between threads.
        self._mFactory = sender

        # Batch over scans, so we don't need to do our own batching to determine groups
        # TODO: Implement custom batching, as iter_scans can be unreliable
        for scan in self.lidar.iter_scans(min_len=self.measurements):
            print('Got %d measurments' % (len(scan)))
            if len(scan) < self.measurements:
                # Poor scan, likely since it was the first scan.
                continue

            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)

            self.fireGroupsChanged()

    def fireGroupsChanged(self):
        # Send the updated groups to 0MQ socket.
        # Rename this to be a generic listener method, rather than an explicit 'send' (even though it can be treated as such already)
        pointScan = tracker_pb.PointScan()
        for group in self.currentGroups:
            for point in group.get_points():
                pointScan.points.append(tracker_pb.Point(
                    angle=point[1], distance=point[2], group_number=group.number))

        self._mFactory.send_message_topic(
            "lidar_map", messages.ProtoMessage(message=pointScan.SerializeToString()))

    def stop_scanning(self):
        self.run = False