/*
 * This method returns the range reading from the sensor mounted on top of robot.
 * It uses x and y as the actual position of the robot/particle and then creates Vx and Vy as virtual x and y.
 * These virtual x and y loop from the current position till some obstruction is there and tell us distance till there.
 */

private int calculateRange(double x, double y, double Vx, double Vy, int counter, int loop_counter)
{
    while(robotIsWithinBoundary(Vx, Vy))
    {
        int pace = 2;
        Vx += pace* Math.sin(Math.toRadians(robot_orientation));
        Vy += pace* Math.cos(Math.toRadians(robot_orientation));
        counter++;
        Line2D line1 = new Line2D.Double(x,y,Vx,Vy);
        if(line1.intersects(obst1))
        {
            //System.out.println("Distance to obst1:"+counter);
            loop_counter++;
            break;
        }
        if(line1.intersects(obst2))
        {
            //System.out.println("Distance to obst2:"+counter);
            loop_counter++;
            break;
        }

    }
    return counter;
}

/*
 * This method tells us whether the robot/particle is within boundary or not.
 */
private boolean robotIsWithinBoundary(double x, double y)
{
    boolean verdict = true;
    if(x>680||x<0)
    {
        verdict = false;
    }
    if(y<0||y>450)
    {
        verdict = false;
    }
    return verdict;
}   /*
 * This method returns the range reading from the sensor mounted on top of robot.
 * It uses x and y as the actual position of the robot/particle and then creates Vx and Vy as virtual x and y.
 * These virtual x and y loop from the current position till some obstruction is there and tell us distance till there.
 */
private int calculateRange(double x, double y, double Vx, double Vy, int counter, int loop_counter)
{
    while(robotIsWithinBoundary(Vx, Vy))
    {
        int pace = 2;
        Vx += pace* Math.sin(Math.toRadians(robot_orientation));
        Vy += pace* Math.cos(Math.toRadians(robot_orientation));
        counter++;
        Line2D line1 = new Line2D.Double(x,y,Vx,Vy);
        if(line1.intersects(obst1))
        {
            //System.out.println("Distance to obst1:"+counter);
            loop_counter++;
            break;
        }
        if(line1.intersects(obst2))
        {
            //System.out.println("Distance to obst2:"+counter);
            loop_counter++;
            break;
        }

    }
    return counter;
}

/*
 * This method tells us whether the robot/particle is within boundary or not.
 */
private boolean robotIsWithinBoundary(double x, double y)
{
    boolean verdict = true;
    if(x>680||x<0)
    {
        verdict = false;
    }
    if(y<0||y>450)
    {
        verdict = false;
    }
    return verdict;
}

/*
 * This method calculates the importance weights for the particles based on the robot_range which is
 * the reading of the range sensor for the robot.
 */
private double measurementProbability(int index)
{
    double probability=1;
    double particle_x_position=particleListX.get(index);
    double particle_y_position=particleListY.get(index);
    double particle_Vx=particle_x_position;
    double particle_Vy=particle_y_position;
    int range_counter=0;
    int loop_counter=0;

    int distance = calculateRange(particle_x_position, particle_x_position, particle_Vx, particle_Vy ,range_counter, loop_counter);
    probability *= calculateGaussianDistance(distance, senseNoise, robot_range);
    //System.out.println(probability);
    return probability;
}

private double calculateGaussianDistance(double mu, double sigma, double x )
{
    double gDistance=Math.exp(-(((Math.pow((mu - x),2))/(Math.pow(sigma,2)) / 2.0) / (Math.sqrt(2.0 * Math.PI * (Math.pow(sigma,2))))));
    return gDistance;
}

sample += randomDouble(0, maxWeight);
    while(sample > particleListProbability.get(index))
    {
        sample -= particleListProbability.get(index);
        index = (index +1) % n;
    }
    return index;
}