Henrik Klaus Martin

public class MCL {
    int samplescount=0;
    Sample[] samples;
    double xrange = 0.0;

    public MCL(int samplescount, double xrange){
        this.samplescount = samplescount;
        this.samples = new Sample[samplescount];
        this.xrange = xrange;
        for(int i=0; i<samplescount; i++)
            samples[i] = new Sample();
    }

    public void initialise(){
        for(int i=0; i<samplescount; i++){
            samples[i].setX(Math.random()*xrange);
            samples[i].setImportance(1.0/samplescount);
        }
    }

    public double Pmap(double x) {
        //return (x/xrange)*100.0;
        //return 0.0885 * (x/xrange)*255 + 33.439;

                double[] a = {1.0,1,1,2,1,1,1,0,1,2,0,1,1,1,1,1,1,1,1,0,1,1,0,0,0};
        double[] b = {33.0,33,33,30,34,34,34,41,33,24,44,33,33,33,33,33,33,33,33,52,32,32,54,54,54};

        for(int i=0; i<25; i++){
            if(x>=i && x<i+1){return a[i]*x+b[i];}
        }
        return 0.0;
    }

    public double expPosition(){
        double sumX=0.0;
        for(int i=0; i<samplescount; i++){
            sumX += samples[i].getX();
        }
        return sumX/samplescount;
    }

    public void printSamples(){
        for(int i=0; i<samplescount; i++){
            System.out.println(samples[i].getX() + " " + samples[i].getImportance());
        }
    }

    public void update(double deltax, double sensor, double uncertainty){
        Sample[] newsamples = new Sample[samplescount];
        double sumimp=0.0;

        for(int i=0; i<samplescount; i++){
            int k = chooseSample();
            double newx = samples[k].getX() + deltax + (Math.random()*uncertainty-uncertainty/2);

            if(newx > xrange){newx = xrange;}
            if(newx < 0.0){newx = 0.0;}

            double expectedsensor = Pmap(newx);
            double newimportance = Math.pow((100.0-Math.abs(sensor - expectedsensor))/100.0, 2);
            newsamples[i] = new Sample(newx, newimportance);
            sumimp += newimportance;
        }

        for(int i=0; i<samplescount; i++){
            samples[i].setX(newsamples[i].getX());
            samples[i].setImportance(newsamples[i].getImportance()/sumimp);
        }
    }

    public int chooseSample(){
        double r = Math.random();
        double sum=0; int count=0;

        for(int i=0; i<samplescount; i++){
            sum += samples[i].getImportance();
            if(sum >= r){count = i; break;}
        }

        return count;
    }
}