%%% example of a particle filter
%%% CS148 -- Building Intelligent Robots
%%% Author: Chad Jenkins  (cjenkins@cs.brown.edu)
%%%         Yuri Malitsky (ynm@cs.brown.edu)
%%% Modified: February 21, 2009
%%%
%%% call as function with 2-5 arguments:
%%% diffusion_strength: strength of the uncertainty of movement (0-4)
%%% step_by_step: set to 1 to stop after every step (0,1)
%%% truth_movement_range: set the true start and end location of the robot's motion (optional) ([1.1, 1.9] by default)
%%% truth_odometry: set the odometry step size (optional) (-.01 by default) 
%%% truth_doors: set the positions of the doors in the world (optional) ([[1.15; 1.22] [1.55; 1.62] [1.67; 1.74]] by default)
%%%
%%%
%%% Internal Variables:
%%% nx: all posible positions of the robot
%%% bin: probability that the robot is at agiven position
%%% precision: number of sample points
%%% odometry: amount the robot moves after every step
%%%
%%% based on Thrun's tutorial on Probabilistic Robotics

%based on Thrun's tutorial on Probabilistic Robotics

%assume sensed odometry is true odometry

function [nul] = particlefilter_hallway( diffusion_strength, step_by_step, truth_movement_range, truth_odometry, truth_doors )

hold off

welcome = 'Hello World'

% initialization: particle hypotheses, uniform probabilities, doors,
%    odometry step size
precision = 500;
x = 1:(1/precision):2; %1:0.005:2;
nx = 1:(1/precision):2;


% set the environment as the positions of the doors along the hallway
if (~exist('diffusion_strength'))
   diffusion_strength = 1;
end

% set the environment as the positions of the doors along the hallway
if (~exist('step_by_step'))
   step_by_step = 0;
end

% set the environment as the positions of the doors along the hallway
if (exist('truth_doors'))
   doors = truth_doors;
else
   doors = [[1.15; 1.22] [1.55; 1.62] [1.67; 1.74]]; %default environment
end

% set the true start and end location of the robot's motion
if (exist('truth_movement_range'))
   movement_range = truth_movement_range;
   x = movement_range(1):(1/precision):movement_range(2); %1:0.005:2;
   nx = movement_range(1):(1/precision):movement_range(2);
else
   movement_range = [1.9 1.1]; %default robot movement range
end

% set odometry step size
if (exist('truth_odometry'))
   odometry = truth_odometry;
else
   odometry = -0.01; % default odometry step
end

% probabilities
px = ones(size(x));
px = px / sum(px);

bin = ones(size(x));
bin = bin / sum(bin);


% Scan from hallway from right to left
for truex = movement_range(1,1):odometry:movement_range(1,2)
   % sensing from true position along hallway
   sense_door = [0, 0, 0];
   for i = 1:size(doors,2)
       if ((truex-0.02 >= doors(1,i)) & (truex-0.02 <= doors(2,i)))
           sense_door(1) = 1;
       end

       if ((truex >= doors(1,i)) & (truex <= doors(2,i)))
           sense_door(2) = 1;
       end

       if ((truex+0.02 >= doors(1,i)) & (truex+0.02 <= doors(2,i)))
           sense_door(3) = 1;
       end
   end

   % resample particles with replacement by dynamics update
   old_x = x;
   importancebin = cumsum(px);
   importancebin = importancebin/max(importancebin);
   random_samples = rand(size(importancebin));
   for i = 1:length(random_samples)
       % find the next bin
       binassign = min(find(importancebin >= random_samples(1,i)));

       % new sample = old pos + drift + diffusion
       x(1,i) = old_x(1,binassign) + odometry + diffusion_strength*odometry*(2*rand-1.0);
       b(1,i) = binassign;

       if x(1,i) < 1
           x(1,i) = 1;
       end

       if x(1,i) > 2
           x(1,i) = 2;
       end

       % the last 5 samples are random
       if (i > length(random_samples)-5) % add some uniformly random particles
          x(1,i) = 1.5+1.0*(rand-0.5);
          b(1,i) = binassign;
       end
   end

   % evaluate likelihood of each particle
   temp_px = px(1,b);
   for i = 1:length(x)
       px(1,i) = hallway_likelihood(sense_door, x(1,i), doors);
   end

   % sum up the scores for each bin
   temp_bin = bin(1,b);
   bin = zeros(size(x));
   for i = 1:length(x)
       pos = round((x(1,i)-1)*precision) + 1;
       bin(1, pos) = bin(1, pos) + px(1,i);
   end
   bin(1,1) = 0;
   bin(1,precision+1) = 0;

   % normalize distribution over the bins
   bin = bin / sum(bin);
   draw_world( doors, truex, sense_door, nx, temp_bin, bin );

   if step_by_step > 0
       input('Step');
   end
end

end

function [lhood] = hallway_likelihood(sense_door, belief_position, door_intervals)
   lhood = 0;

   believe_door = 0;
   for i = 1:size(door_intervals,2)
       if ((belief_position >= door_intervals(1,i)) & (belief_position <= door_intervals(2,i)))
           believe_door = 1;
       end
   end

   if (sense_door == believe_door)
       lhood = 10;
   end

   c_case = sense_door(1)*power(2,0) + sense_door(2)*power(2,1) + sense_door(3)*power(2,2);
   for i = 1:size(door_intervals,2)
       switch( c_case )
           case 1  % 1 0 0
               if ((belief_position >= door_intervals(2,i)-0.03) & (belief_position <= door_intervals(2,i)+0.06))
                   lhood = 10;
               end
           case 3  % 1 1 0
               if ((belief_position >= door_intervals(2,i)-0.06) & (belief_position <= door_intervals(2,i)+0.03))
                   lhood = 10;
               end
           case 4  % 0 0 1
               if ((belief_position >= door_intervals(1,i)-0.06) & (belief_position <= door_intervals(1,i)+0.03))
                   lhood = 10;
               end
           case 6  % 0 1 1
               if ((belief_position >= door_intervals(1,i)-0.03) & (belief_position <= door_intervals(1,i)+0.06))
                   lhood = 10;
               end
       end
   end
end

function [] = draw_world( doors, truex, sense_door, x, temp_px, px )
   % draw bounding frame
   hold off;
   plot(1,1,'w.');
   hold on;
   line([0.5 2.5],[1 1],'Color', [0 0 0]);

   % draw doors
   for i = 1:size(doors,2)
       fill([doors(1,i) doors(1,i) doors(2,i) doors(2,i)],[1 1.3 1.3 1], 'r');
       fill([doors(2,i)-.015 doors(2,i)-.015 doors(2,i)-0.005 doors(2,i)-0.005],[1.14 1.16 1.16 1.14], 'w');
   end

   % draw true robot state
   fill([truex-0.03  truex-0.03  truex+0.03  truex+0.03], [0.7  0.80 0.80 0.7],  'g');
   fill([truex-0.027 truex-0.027 truex-0.007 truex-0.007],[0.68 0.72 0.72 0.68], 'b');
   fill([truex+0.027 truex+0.027 truex+0.007 truex+0.007],[0.68 0.72 0.72 0.68], 'b');

   % draw the beams of the sensors
   if (sense_door(1))
       plot([truex-0.02 truex-0.02], [0.8 1.15], ':','LineWidth',2,'Color',[0.5 0 0]);
   else
       plot([truex-0.02 truex-0.02], [0.8 1.15], ':','LineWidth',2,'Color',[0.5 0.5 0.5]);
   end

   if (sense_door(2))
       plot([truex truex], [0.8 1.15], ':','LineWidth',2,'Color',[0.5 0 0]);
   else
       plot([truex truex], [0.8 1.15], ':','LineWidth',2,'Color',[0.5 0.5 0.5]);
   end

   if (sense_door(3))
       plot([truex+0.02 truex+0.02], [0.8 1.15], ':','LineWidth',2,'Color',[0.5 0 0]);
   else
       plot([truex+0.02 truex+0.02], [0.8 1.15], ':','LineWidth',2,'Color',[0.5 0.5 0.5]);
   end

   scale = max(px);
   for i = 1:length(x)
       plot([x(1,i) x(1,i)],[0 0.5*px(1,i)/scale], 'r');
   end

   % estimate robot's position as mean of particle distribution
   %meanx = 0;
   %for i = 1:length(x)
   %   meanx = meanx + x(1,i)*px(1,i);
   %end

   % draw robot's estimated position
   %fill([meanx-0.03  meanx-0.03  meanx+0.03  meanx+0.03], [1.4  1.55 1.55 1.4],  'g');
   %fill([meanx-0.027 meanx-0.027 meanx-0.007 meanx-0.007],[1.38 1.42 1.42 1.38], 'b');
   %fill([meanx+0.027 meanx+0.027 meanx+0.007 meanx+0.007],[1.38 1.42 1.42 1.38], 'b');

   % refresh display
   refresh;
   drawnow;
end