function [x,w,preset]=ParticleFilter(NP,x0range,uc,dt,covrobot,covceil,light,resample,yreal,sen,filename,plotsteps,xreal)

% ParticleFilter      It runst the Particle Filter
%
% [x,w,preset]=ParticleFilter(NP,x0range,uc,dt,covrobot,covceil,light,resample,yreal,sen,filename)
% [x,w,preset]=ParticleFilter(NP,x0range,uc,dt,covrobot,covceil,light,resample,yreal,sen,filename,plotsteps,xreal)
%
% NP: number of particles
% x0range: 3*2 matrix with the initial ranges of each variables (x,y,theta)
% uc: control signals
%     uc(:,:,k): control signal at time k
% covrobot: covariance of the errors associated to the control signals
% covceil: covariance of the errors of the height sensor
% light: information associated with the lighting model
%   light(1): limit
%   light(2): l2
%   light(3): l3
% resample: type of PF
%   resample=0  SIS
%   resample=1  SIR
%   resample=2  Auxiliary PF
% yreal: measurements at all the steps
%   yreal(:,:,k): measurements at time step k (height,light)
% sen: sensors selected to calculate the probability of the measurements
%      given the position of the particles
% filename: name of the file to save intermediate information. If empty,
%           no information is saved
% plotsteps: time steps where plotting is required.
% xreal: real values of the state (only used for drawing)
% x: particles information at last time step
%   x(:,:,k): particle information at time k
% w: weights of the particles at last time step
% preset: time steps where the PF is reset 

% APRENDIZAJE AUTOMÁTICO (G. Pajares y J. M. de la Cruz, Eds.)
% Editorial: RA-MA
% Capítulo 8: Monte Carlo II
% Autores: Eva Besada-Portas, Sergey M. Plis



lx=x0range(1,:);
ly=x0range(2,:);
la=x0range(3,:);

%Initial values of the particles
x=rand(NP,3);
x(:,1)=x(:,1)*(lx(2)-lx(1))+lx(1);
x(:,2)=x(:,2)*(ly(2)-ly(1))+ly(1);
x(:,3)=x(:,3)*(la(2)-la(1))+la(1);

%Initial values of the weights
w=ones(NP,1)/NP;
N=size(yreal,3); %Number of iterations
preset=zeros(N,1); %If the PF is reset in any iteration

if nargin<10
    plotsteps=[];
end

%Deciding when to do plot
doplot=zeros(N,1);
doplot(plotsteps)=1;

k=1;
if sum(doplot) %If we have to do plot at some time step, we need the lights and the height elements
    hf=figure;   
    k2=find(w>max(w)/2);
    h(1)=plot(x(k2,1,k),x(k2,2,k),'*r');
    hold on;
    h(2)=plot(xreal(:,1,k),xreal(:,2,k),'*b','MarkerSize',8);
    h(3)=plot(xreal(:,1,k),xreal(:,2,k),'b','LineWidth',2);
    if doplot(k)==0    %If we dont want to draw the initial values, make them invisible   
        set(h,'Visible','off');
        title(sprintf('Tiempo=%4.1f (s)',(k-1)*dt),'FontSize',12);
    end
    
    Height(1);
    MapLight(1);
    xlabel('X (m)','FontSize',12);
    ylabel('Y (m)','FontSize',12);                
    drawnow
end

%If we want to save the experiment, do it at each time step
if ~isempty(filename)
    eval(sprintf('x%d=x;w%d=w;save(filename,''x%d'',''w%d'',''xreal'',''yreal'',''dt'');clear x%d;clear w%d;',k,k,k,k,k,k));
end

for k=2:N %Time step loop
   
    if resample==2  %Auxiliary reweighting step of the auxiliar  PF
        xmean=ModelRobot(2,x(:,:,k-1),dt,uc(:,:,k-1),covrobot); %2: obtain mean value
        waux=ModelSensor(1,xmean,covceil,light,sen,yreal(:,:,k));%obtain their p(y|mean)
        swaux=sum(waux);
        if swaux==0
            waux(:)=1/NP;
        else
            waux=waux/sum(waux); %normalize it to select NP elements
        end
        index = ResampleResidual(1:NP,waux,NP);
        w=w(index)./waux(index);  %Update the weights   (selected ones)
        x=x(index,:,:);           %Unpate the particles (selected ones)
    end
   
    x(:,:,k)=ModelRobot(0,x(:,:,k-1),dt,uc(:,:,k-1),covrobot); %x at new time step

    wnew=ModelSensor(1,x(:,:,k),covceil,light,sen,yreal(:,:,k)); %likelihood
    w=w.*wnew;
    sw=sum(w);
    if sw==0 %Then we are lost, make all the weights equal
        preset(k)=1; 
        w(:)=1/NP;
    else
        w=w/sw;
    end

   if resample %resampling step of SIR and Auxiliary PF
        index = ResampleResidual(1:NP,w,NP); %select according to the weights
        w(:)=1/NP;                           %all the weights become equal   
        x=x(index,:,:);                      %select the resampled trajectories
    end

    if doplot(k)  %We have to plot the new time step
        figure(hf);
        k2=find(w>max(w)/2);
        set(h(1),'XData',x(k2,1,k),'YData',x(k2,2,k));
        set(h(2),'XData',xreal(:,1,k),'YData',xreal(:,2,k));
        set(h(3),'XData',squeeze(xreal(:,1,1:k)),'YData',squeeze(xreal(:,2,1:k)));
        title(sprintf('Tiempo=%4.1f (s)',(k-1)*dt),'FontSize',12);
        drawnow
    end
    disp(sprintf('Iteration %d',k));
   
    %If we have to save data, added to the file
    if ~isempty(filename) 
        eval(sprintf('x%d=x;w%d=w;save(''-append'',filename,''x%d'',''w%d'');clear x%d;clear w%d;',k,k,k,k,k,k));
    end
    
    %Decide by pressing 's' if you want to stop at the given time step
    if stop_keyboard(k) 
        return;
    end

end
