"""Module containing simulated robot sensors"""
import math

import numpy as np


def add_noise(distance, angle, sigma):
    _mean = np.array([distance, angle])
    # Noise for distance or angle are not correlated
    covariance = np.diag(sigma ** 2)
    new_distance, new_angle = np.random.multivariate_normal(_mean, covariance)
    # Don't want negative values
    new_distance = max(new_distance, 0)
    new_angle = max(new_angle, 0)
    return [new_distance, new_angle]


class Lidar2DSensor:

    def __init__(self, senor_range, environment_map, uncertainty, speed=4, colors=None):
        """
        :param range: range of the sensor
        :param map: the map
        :param uncertainty: uncertainty of sensor measurements
        :param speed: revolutions per second
        """
        self.range = senor_range
        self.map = environment_map
        self._uncertainty = uncertainty
        self.speed = speed
        # The sensor noise represented as a distance and an angle
        self.sigma = np.array([self._uncertainty[0], self._uncertainty[1]])
        # TODO make this a value of the robot.
        self.position = (0, 0)
        self.map_width, self.map_height = self.map.get_size()
        self.sensor_point_cloud = []
        self.colors = colors

    def calc_distance(self, obstacle_position):
        """Calculates the distance from robot position to obstacle.

        Uses the Euclidean distance calculation.
            distance = sqrt((Xb - Xa)^2 + (Yb-Ya)^2)
        """
        px = (obstacle_position[0] - self.position[0]) ** 2
        py = (obstacle_position[1] - self.position[1]) ** 2
        return math.sqrt(px + py)

    def sense_obstacles(self):
        data = []
        x1, y1 = self.position[0], self.position[1]

        # 360 degrees == 2Pi radians
        # np.linspace's use allows up to set the number of values which affects the simulated map resolution.
        for angle in np.linspace(0, 2 * math.pi, 60, False):
            # Calculate the end point of the lidar range's line segment in the 2D plane
            x2, y2 = (x1 + self.range * math.cos(angle), y1 - self.range * math.sin(angle))
            # Sample the line segment and detext if the sampled point is "black" when overlayed on the simulated map
            for i in range(0, 100):
                # TODO break this out to a function
                u = i / 100
                # TODO add comment explaining this calculation
                x = int(x2 * u + x1 * (1 - u))
                y = int(y2 * u + y1 * (1 - u))
                # TODO an optimization could be performed to not sample any further
                # if the point has fallen off the map.
                # Check if the sampled point is on the map
                if 0 < x < self.map_width and 0 < y < self.map_height:
                    map_point_color = self.map.get_at((x, y))
                    if (map_point_color[0], map_point_color[1], map_point_color[2]) == (0, 0, 0):
                        # if black add some noise
                        distance = self.calc_distance((x, y))
                        noised_distance = add_noise(distance, angle, self.sigma)
                        # TODO there is definitely a better datastructure to use. Maybe a named tuple?
                        noised_distance.append(self.position)
                        data.append(noised_distance)
                        # if obstacle found stop further sampling this line
                        break
                else:
                    # line fell off of map break this loop iteration
                    break
        # TODO OMG I hate this guys code
        # Callers of this function need to check truthiness not in the return  statement
        # if len(data)>0:
        #     return data
        # else:
        #     return False
        return data