%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Object tracking using Particle Filter
%Project CIS 601
%Jogen Shah and Nandini Easwar
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% We read the avi file and load frames one by one into I(:,:,:,:)
% thus I(:,:,:,1) is the first frame 

movieObj = aviread('movie_final.avi');
noOfFrameImages = size(movieObj,2);         %no. of frames in the avi video
for k =1:1:noOfFrameImages
    image = movieObj(k).cdata;
    I(:,:,:,k)=image;
end
% calculating reference histogram from a reference image 
reference = imread('reference.jpg');

[ref_count,ref_bin] = imhist(reference);
double(ref_count);

% Finding reference histogram in image
% initialisation
N = 500;                        % No. of particles
n_x = 2;                        % we track location (x,y) hence we use 2 dimensions
x1= 45;                         %Initialisation for location of object in 1st frame
y1= 45;

d = zeros(N,noOfFrameImages);   %distance matrix
rect=zeros(1,4);
w = zeros(N,noOfFrameImages);   %weights
x_new = zeros(n_x,N,noOfFrameImages);

x = zeros(n_x,N,noOfFrameImages);       % x is state vector . it has values of location (x,y) for N particles for all frames
state = zeros(n_x,noOfFrameImages);     % This is the matrix where we store the predicted location of object in video

state(:,1) = [x1;y1];                   %matrix which stores the final states
% For 1st frame initialise N particles
for i = 1:N
    x(1,i,1) = x1 + 50 * rand(1) - 50 *rand(1); 
    x(2,i,1) = y1 + 50 * rand(1) - 50 *rand(1);
    
end
% Using first-order auto-regressive dynamics to predict next states
% Model is : x(t+1) = A * x(t)

    A = zeros(n_x,n_x);         % A is first order dynamics model constant
    A = [1 0;0.7 0.7];          
    B = rand(n_x);              % B is second order dynamics model constant
    
    new_c=zeros(255,1);
% for each frame from 2 to noofframeimages 
for j = 2:noOfFrameImages
    %for each particle
    for i = 1:N
        if (j==2) x(:,i,j) = A * x(:,i,j-1);
        else x(:,i,j) = rand(n_x) * x(:,i,j-1) + rand(n_x) * x(:,i,j-2);    %using the 2nd order dynamics eqn
        end 
        rect = [(x(1,i,j)-15),(x(2,i,j)-15),30,30];
        [count,binnumber] = imhist(imcrop(I(:,:,:,j),rect));
        
        double(count);
%         for k = 1:255
%             new_c(rnd(binnumber(k)))=count(binnumber(k));
%         end
        for k = 1:255
            d(i,j) = d(i,j) + (double(count(k)) - double(ref_count(k)))^2; %calculating the distance
        end
        d(i,j) = sqrt(d(i,j));                  % Sequential Importance Sampling
        w(i,j) = d(i,j);                        %assigning weights
    end
    
    [small_value,small_index] = min(d(:,j));    % find the next state of object
    
    state(1,j) = round(x(1,small_index,j));     %calculating final states
    state(2,j) = round(x(2,small_index,j));
    
  %  hold on
   % I((state(1,j),(state(2,j),:,k) = [255 0 0];
    
    one  = ones(N,1);   
    w(:,j) = w(:,j)./sum(w(:,j));               % Calculate the normalised weights for each particle
    w(:,j) = one(:,1) - w(:,j);
    
    % Resampling
    for i = 1:N
    x(1,i,j) = state(1,j) + 50 * rand(1) - 50 *rand(1); 
    x(2,i,j) = state(2,j) + 50 * rand(1) - 50 *rand(1);
    end
    
%     outindex = multinomialR(1:N,w(:,j));
%      for m = 1:N
%          x_new(:,m,j) = x(:,outindex(m),j);
%      end
%      x(:,:,j) = x_new(:,:,j);
%   
end    

for p = 1:noOfFrameImages                       %displaying frame by frme, with arrow indicating the predicted new state
    figure(p)
    imshow(I(:,:,:,p));
    hold on;
    text(state(1,p),state(2,p),'\leftarrow ','FontSize',18);
    hold off;
    pause(0.5);
end
    
% aviFileName = 'movie.avi';
% 
% framesPerSec = 25;
% aviobj = avifile(aviFileName,'fps',framesPerSec,'COMPRESSION','NONE');
% for k = 1:noOfFrameImages 
%     aviobj = addframe(aviobj,I(:,:,:,k));
%     hold on;
%     text(state(1,k),state(2,k),' \leftarrow ','FontSize',18);
%     hold off;
% 
% end
% aviobj = close(aviobj);
