% these are typical run parameters.
% localize(run25, 33.33, 2.75, 2.25, mapx, mapy, .01, .08, pi/60, 10, .5, 1000, 2)
% localize(run25, 33.33, 2.75, 2.25, mapx, mapy, .01, .08, .03, 8, .01, 1000, 2)
%
% mapx = [ 0 0 -12.5 -23 -23 0]/0.0675;
% mapy = [ 0 12 12 4.75 0 0]/0.0675;
%
% These are the map coordinates (roughly) used in all my test data runs.


function [sx, sy, sh] = localize(datums, wb, by, bw, mapx, mapy, xe, ye, he, bt, lt, numparticles, debug)
% datums is the datafile, wb is the wheelbase (in lego rotator units)  1 ru = 0.0675 inch
% by is the distance from the center to the bumper, in inches
% bw is the width of the bumper, in inches
% mapx and mapy are xy coordinates of the map
% xe is the fraction of the lateral offset to use in the error
% ye is the fraction of the forward offset "
% he is the error in heading, in radians
% bt is the tolerance to use in determining whether a bumper has hit a wall
% (~ the radius of the lego wheels on the bumper)
% lt is the tolerance to use in determining whether a point is in the map
% debug: 0 for no intermittent debugging plots, 1 for lots, 2 for only
% after each hit

[d1, d2, dir1, dir2, bu1, bu2] = parsedatalogmore(datums);
[dxd, dyd, thetad] = curvedpath(d1,d2,wb);
[dxdir, dydir, thetadir] = curvedpath(dir1, -dir2, wb);   % pass the negative because they're spinning in opposite directions when turning
[dxb, dyb, thetab] = curvedpath(-bu1,-bu2,wb);             % both negative cus we're backing up

particlesx = min(mapx) + (max(mapx) - min(mapx))*random('unif',0,1,1,numparticles);
particlesy = min(mapy) + (max(mapy) - min(mapy))*random('unif',0,1,1,numparticles);
particlesh = 2*pi*random('norm',0,1,1,numparticles);
dxa = [dxdir';dxd';dxb'];
dxa = dxa(:);                           % create an array of all the dx's
dya = [dydir';dyd';dyb'];
dya = dya(:);                           % create an array of all the dy's
dha = [thetadir';thetad';thetab'];
dha = dha(:);                           % create an array of all the heading changes
bpa = [];
checkhit = -1;
hitcounter = 0;
for i = 1:length(dxa),
    i
    checkhit
    dha(i)
    if debug == 1,
        plot(mapx,mapy,'g', particlesx,particlesy,'rx');
        pause;
        close(gcf);
    end;
    
    ipis = []; % invalid particle indexes
    vpis = [];
    if checkhit == 1,
        for k = 1:numparticles,
            bpa = getbumperpoints(particlesx(k),particlesy(k),particlesh(k),by,bw,1);        
            if (pointinside(mapx, mapy, particlesx(k),particlesy(k), lt) && pointsinside(mapx,mapy,bpa(:,1),bpa(:,2),lt) && onperimeter(bpa(:,1), bpa(:,2),mapx,mapy,bt)),
                vpis = [vpis k];
            else
                ipis = [ipis k];
            end;
        end;
        checkhit = -2;
    else           
        for k = 1:numparticles,
            if pointinside(mapx, mapy, particlesx(k),particlesy(k), lt),
                vpis = [vpis k];
            else
                ipis = [ipis k];
            end;
        end;
    end
    if (debug == 2) & (checkhit == -2),
        hitcounter = hitcounter + 1
        plot(mapx,mapy,'g', particlesx(vpis),particlesy(vpis),'rx');
        pause;
        close(gcf);     
    end;
    if (checkhit == -2),
        imcheckinhit = 1
        tyt = [];
        tkt = [];
        %for n = 1:length(ipis),
        %    tyt = [tyt; getbumperpoints(particlesx(ipis(n)),particlesy(ipis(n)),particlesh(ipis(n)), by,bw,1)];
            
        %end;
        for p = 1:length(vpis),
            tkt = [tkt; getbumperpoints(particlesx(vpis(p)),particlesy(vpis(p)),particlesh(vpis(p)), by,bw,1)];
        end;
        plot(mapx,mapy,'g', particlesx(ipis),particlesy(ipis),'rx', particlesx(vpis),particlesy(vpis),'bx', tkt(:,1),tkt(:,2),'bo');
        %axis([-300 50 -50 300]);
        pause;
        close(gcf);
    end;  
    pc = numparticles;
    if length(vpis) == 0,
        didnotconverge = 1
        break;
    end;
    if i == length(dxa),
        plot(mapx,mapy,'g', particlesx(vpis),particlesy(vpis),'rx');
        pause;
    end;
    pratio = 1 + length(ipis)/length(vpis);
    npx = [];  npy = [];  nph = [];
    dya(i)
    for m = 1:length(vpis),
        if floor(pratio) * (length(vpis) - m + 1) >= pc,
            pdec = floor(pratio);
        else
            pdec = ceil(pratio);
        end;
        pc = pc - pdec;
        tt = getsmear(particlesx(vpis(m)),particlesy(vpis(m)), dxa(i), dya(i), dha(i), particlesh(vpis(m)),xe,ye,he,pdec);
        npx = [npx tt(:,1)'];
        npy = [npy tt(:,2)'];
        nph = [nph tt(:,3)'];
        
    end;
    checkhit = checkhit+1;
    particlesx = npx;
    particlesy = npy;
    particlesh = nph;
    if debug == 1,
        plot(mapx,mapy,'g', particlesx,particlesy,'rx');
        pause;
        close(gcf);
    end;
end;

plot(mapx,mapy,'g', particlesx,particlesy,'rx');