import math


class Group:

    def __init__(self, number, points=[]):
        self._points = points
        self._number = number
        self._minX = None
        self._maxX = None
        self._minY = None
        self._maxY = None

    def add_point(self, point):
        self._points.append(point)
        self._update_min_max(point)

    def get_points(self):
        return self._points

    @property
    def number(self):
        return self._number

    @number.setter
    def number(self, number):
        self._number = number

    def _update_min_max(self, new_point):
        """
        Updates the in and max points for this group.
        This is to determine when assigning groups whether the
        same group is selected.
        """
        converted_point = convert_lidar_to_cartesian(new_point)

        if self._minX is None or self._minX > converted_point[0]:
            self._minX = converted_point[0]

        if self._maxX is None or self._maxX < converted_point[0]:
            self._maxX = converted_point[0]

        if self._minY is None or self._minY > converted_point[1]:
            self._minY = converted_point[1]

        if self._maxY is None or self._maxY < converted_point[1]:
            self._maxY = converted_point[1]

    def get_minX(self):
        return self._minY

    def get_maxX(self):
        return self._maxY

    def get_minY(self):
        return self._minY

    def get_maxY(self):
        return self._maxY


def convert_lidar_to_cartesian(new_point):
    x = new_point[2] * math.sin(new_point[1])
    y = new_point[2] * math.cos(new_point[1])
    return (x, y)


def calc_groups(scan):
    """
    Calculates groups of points from a lidar scan. The scan should
    already be sorted. 

    Parameters
    ----------

    scan: Iterable
        The lidar scan data to get groups of.
        Should be of format: (quality, angle, distance)

    Returns
    -------
    list
        List of groups that were found.
    """
    prevPoint = None
    currentGroup = None
    allGroups = []
    currentGroupNumber = 0

    # assume the list is already sorted.
    for point in scan:
        if prevPoint is None:
            prevPoint = point
            continue

        # Distances are in mm.
        # within 1cm makes a group. Will need to play around with this.
        if (point[2] - prevPoint[2]) ** 2 < 10 ** 2:
            if currentGroup is None:
                currentGroup = Group(currentGroupNumber)
                allGroups.append(currentGroup)
            currentGroup.add_point(point)
        else:
            if currentGroup is not None:
                currentGroupNumber += 1
                currentGroup = None

        prevPoint = point

    return allGroups


def find_centre(group):
    return ((group.get_maxX() + group.get_minX()) / 2, (group.get_maxY() + group.get_minY()) / 2)


def assign_groups(prev_groups, new_groups):
    """
    Assigns group numbers to a new scan based on the groups of an old scan. 
    """
    for group in prev_groups:
        old_centre = find_centre(prev_groups)
        for new_group in new_groups:
            new_centre = find_centre(new_group)
            # They are considered the same if the new group and old group centres are within 5cm.
            if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 50 ** 2:
                new_group.set_number(group.get_number())

    return new_groups


def updateCarVelocity(oldGroup, newGroup):
    """
    Return a vector indicating how the tracked group has changed, which can
    be used to then update the steering/throttle of the car (or other vehicle that 
    may be used)
    """
    pass