#!/usr/bin/env python3

import math
import random

import mapp
import geom

class Robot:
    
    def __init__(self, mapp, num_particles):
        """
        Initialize the robot with a map.
        Inputs:
            mapp: a Map object on which the robot will move.
        """
        
        self.d_sigma = 0.05 # Uncertainty for distances.
        self.a_sigma = 0.05 # Uncertainty for angles.
        self.size = 0.2 # Size of the robot in meters.
        
        self.ang = 0
        self.coor = (0, 0)
        
        self.alp_slow = 0.1
        self.alp_fast = 0.5
        self.w_slow = 0.1
        self.w_fast = 0.1
        self.w_random = 0
        
        self.alp_dist = 0.3
        self.w_dist = 10
        
        self.num_particles = num_particles
        self.particles = []
        
        self.mapp = mapp
        
        # Draw num_particles random particles inside the map.
        for i in range(self.num_particles):
            self.particles.append((self.random_particle(), 0))
    
    def random_particle(self):
        close = True
        while close:
            x = random.random() * self.mapp.width
            y = random.random() * self.mapp.height
            close = self.mapp.closest_wall((x, y)) < self.size
        ang = random.random() * 2*math.pi
        return (ang, (x, y))
    
    def put(self, ang, coor):
        """
        Put the robot on a place on the map.
        Inputs:
            ang: The orientation of the robot in radians.
            x: The x-coordinate of the robot in meters.
            y: the y-coordinate of the robot in meters.
        """
        
        self.ang = ang
        self.coor = coor
    
    def intersects(self, position, wall):
        """
        Checks if the wall intersects the robot at a given position.
        Inputs:
            state: A tuple with the robot coordinates: (x, y).
            wall: A tuple with the wall's begin and end points:
                ((x1, y1), (x2, y2))
        Output:
            True if the wall intersects the robot, False otherwise.
        """
        
        return geom.dist_point_line(position, wall) < self.size
    
    def motion_model(self, u, state=None, exact=False):
        """
        Calculate the next state for a given state and control.
        Inputs:
            u: A tuple of the form (angle, distance) describing the
                desired movement.
            state: A tuple of the form (angle, (x_coordinate,
                y_coordinate)) describing the current state.
        Output:
            A tuple of the form (angle, (x, y)).
        """
        
        # If no state is given, use the current state of the robot.
        if state is None:
            ang = self.ang
            coor = self.coor
        else:
            ang = state[0]
            coor = state[1]
        
        # Calculate the angle and distance under which to move.
        if exact:
            ang += u[0]
            dist = u[1]
        else:
            ang += random.gauss(u[0], self.a_sigma)
            dist = random.gauss(u[1], u[1] * self.d_sigma)
        
        while ang > 2*math.pi:
            ang -= 2*math.pi
        while ang < -2*math.pi:
            ang += 2*math.pi
        
        # Calculate a step size of at most 0.1, so that the destination
        # will be exactly reached.
        steps = int(math.ceil(dist / 0.1))
        x_step = dist / steps * math.cos(ang)
        y_step = dist / steps * math.sin(ang)
        
        # Take small steps until the destination is reached, or the
        # robot collides with a wall.
        step = 0
        intersect = False
        while step < steps and not intersect:
            
            # Calculate the position after an incremented number of
            # steps.
            step += 1
            position = (
                coor[0] + step * x_step,
                coor[1] + step * y_step
            )
            
            # Check if the robot collides with any of the walls. If so,
            # make sure we exit the while-loop.
            for wall in self.mapp.walls:
                if self.intersects(position, wall):
                    intersect = True
                    step -= 1
                    break
        
        # Calculate the final position of the robot and return this.
        x = coor[0] + step * x_step
        y = coor[1] + step * y_step
        
        return (intersect, (ang, (x, y)))
    
    def move(self, ang, dist, exact=False):
        """
    
    Move the robot according to the motion model and update the
        particles.
        Inputs:
            ang: The angle over which to rotate the robot.
            dist: The distance ove
r which to move the robot.
        Output:
            True if the particles approximate the robot pose good
            enough.
        """
        
        u = (ang, dist)
        
        # Move the robot.
        _, new_state = self.motion_model(u, exact=exact)
        self.ang, self.coor = new_state
        self.measurement = self.measure()
        
        # Initialize the temporary particle list with a dummy particle.
        # Elements are of the form ((ang, (x, y)), weight)
        temp = [((0, (0, 0)), 0)]
        for particle in self.particles:
            _, new_part = self.motion_model(u, particle[0])
            weight = self.measurement_model(new_part, particle[1])
            
            temp.append((new_part, temp[-1][1] + weight, weight))
        
        # Remove the dummy particle and empty the particle list.
        temp.pop(0)
        self.particles = []
        rand_particles = []
        total_weight = temp[-1][1]
        self.set_weights(temp)
        
        # Add num_particles new particles to the list, according to the
        # cumulative distribution stored in temp[i][1].
        
        for i in range(self.num_particles):
            if random.random() < self.w_random:
                rand_particles.append((self.random_particle(), 0))
            else:
                selector = random.random() * total_weight
                
                # Find the largest temporary particle whose cumulative
                # weight is smaller than the random selector.
                k = 0
                while temp[k][1] < selector:
                    k += 1
                self.particles.append((temp[k][0], temp[k][2]))
        
        # See if the non-random particles are close enough yet.
        self.w_dist += self.alp_dist * (self.particles_distance() - self.w_dist)
        self.particles.extend(rand_particles)
        
        return self.w_dist < 0.5
    
    def particles_distance(self):
        """
        Calculate the average distance of the best portion of the
        particles to the actual robot position.
        """
        
        avg_num = len(self.particles)//3
        distances = []
        distances = [geom.dist_points(self.coor, p[0][1]) for p in self.particles]
        return sum(sorted(distances)[:avg_num])/avg_num
    
    def print(self):
        """
        Print info on the location of the robot.
        """
        
        print('angle: ' + str(round(self.ang, 2)) +
            ', coordinates: ('+str(round(self.coor[0], 2)) +
            ', ' + str(round(self.coor[1], 2)) + ')')
    
    def draw(self):
        """
        Draw a map with the robot and current particles on it.
        Output:
            An Image file, from the PIL module.
        """
        
        return self.mapp.draw(
            robot=self.coor,
            particles=[p[0] for p in self.particles]
        )


class Robot1(Robot):
    
    half_measures = 25  # Half of the number of measurements (the total
                        # number must be even to simplify calculations.)
    min_range = -10   # The minimal and
    max_range = 10  # maximal measuring distance.
    hit_sigma = 0.3 # See Thrun p. 172.
    
    def __init(self, mapp, num_particles):
        self.measurement = []
        super(Robot, self).__init__(mapp, num_particles)
    
    def set_weights(self, particles):
        """
        Update the moving averages used to determine the number of
        random particles that wil