import random
import numpy as np
import random_numbers as rn

class Particle(object):
    """Data structure for storing particle information (state and weight)"""
    def __init__(self, x=0.0, y=0.0, theta=0.0, weight=0.0):
        self.x = x
        self.y = y
        self.theta = np.mod(theta, 2.0*np.pi)
        self.weight = weight

    def getX(self):
        return self.x
        
    def getY(self):
        return self.y
        
    def getTheta(self):
        return self.theta
        
    def getWeight(self):
        return self.weight

    def setX(self, val):
        self.x = val

    def setY(self, val):
        self.y = val

    def setTheta(self, val):
        self.theta = np.mod(val, 2.0*np.pi)

    def setWeight(self, val):
        self.weight = val

    def copy(self):
        return Particle(self.x, self.y, self.theta, self.weight)


def estimate_pose(particles_list):
    """Estimate the pose from particles by computing the average position and orientation over all particles. 
    This is not done using the particle weights, but just the sample distribution."""
    x_values = [i.getX() for i in particles_list]
    y_values = [i.getY() for i in particles_list]
    cos_values = [np.cos(i.getTheta()) for i in particles_list]
    sin_values = [np.sin(i.getTheta()) for i in particles_list]

    x_sum = sum(x_values)
    y_sum = sum(y_values)
    cos_sum = sum(cos_values)
    sin_sum = sum(sin_values)

    flen = len(particles_list)
    if flen != 0:
        x = x_sum / flen
        y = y_sum / flen
        theta = np.arctan2(sin_sum/flen, cos_sum/flen)
    else:
        x = x_sum
        y = y_sum
        theta = 0.0
    return Particle(x, y, theta)
     
     
def move_particle(particle, delta_x, delta_y, delta_theta):
    """Move the particle by (delta_x, delta_y, delta_theta)"""
    particle.setX(particle.getX() + delta_x)
    particle.setY(particle.getY() + delta_y)
    particle.setTheta(particle.getTheta() + delta_theta)


def add_uncertainty(particles_list, sigma, sigma_theta):
    """Add some noise to each particle in the list. Sigma and sigma_theta is the noise
    variances for position and angle noise."""
    for particle in particles_list:
        particle.x += rn.randn(0.0, sigma)
        particle.y += rn.randn(0.0, sigma)
        particle.theta = np.mod(particle.theta + rn.randn(0.0, sigma_theta), 2.0 * np.pi) 


def add_uncertainty_von_mises(particles_list, sigma, theta_kappa):
    """Add some noise to each particle in the list. Sigma and theta_kappa is the noise
    variances for position and angle noise."""
    for particle in particles_list:
        particle.x += rn.randn(0.0, sigma)
        particle.y += rn.randn(0.0, sigma)
        particle.theta = np.mod(rn.rand_von_mises(particle.theta, theta_kappa), 2.0 * np.pi) - np.pi

def resample(particle_list):
    weights = [p.weight for p in particle_list]
    if sum(weights) == 0:
        weights = [1/len(particle_list) for _ in particle_list]
    new_particles = random.choices(particle_list, weights, k=len(particle_list))
    particle_list = [p.copy() for p in new_particles]
    return particle_list