%Homework 5 - Computer Vision - Benjamin Berger
%CISC 689 - University of Delaware - 05/04/03

%===============================================================%
% Part 1 - Particle Filtering
%===============================================================%

% load AVI file
movname = '../../Data/5/dot.avi';
movinfo = aviinfo(movname);

nf=movinfo.NumFrames;
cf=movinfo.Width;
rf=movinfo.Height;

imarray = cell(movinfo.NumFrames);
synth_imarray = cell(movinfo.NumFrames);

% read movie into image array
for t=1:movinfo.NumFrames
   if mod(t,10)==0, readframe=t, end
   % read current input frame and convert to image
   input_frame = aviread(movname, t);
   imarray{t} = im2double(frame2im(input_frame));
end

n = 100;    % n particles
m = 2;      % m parameter hypothetical state 
   
% pick n random weights + normalize them so that sum=1
Pi = random('Uniform',0,1,n,1);
Pi = Pi ./ sum(Pi);

%create n random particles within the image
S(:,1) = random('Uniform',1,cf,n,1);
S(:,2) = random('Uniform',1,rf,n,1);

%create rectangular marker for state estimate:
Rectmark(:,:,:)=zeros(3,3,3);
Rectmark(:,:,2)=ones(3,3);

%% generate the new movie
for i=1:nf
    [S,Pi] = update_PF( S , Pi , imarray{i} , @F , @Xi , @H );
    % plot(S(:,1),S(:,2),'b.')
    Xhat = state_estimate_PF(S,Pi);
    z_flies = imarray{i};
    %mark estimated state with a cyan square:
    z_flies(Xhat(2)-1:Xhat(2)+1, Xhat(1)-1:Xhat(1)+1, : ) = Rectmark;
    Sr=round(S);
    %print cloud of flies (particles) into frame:
    for j =1:n
        if ((Sr(j,1)>=1) & (Sr(j,2)>=1) & (Sr(j,1)<=cf) & (Sr(j,2)<=rf))
            z_flies(Sr(j,2),Sr(j,1),:) = 0.7;
        end;
    end;
    imshow(z_flies);
    synth_imarray{i} = z_flies;
    Xhatsnake(i,:) = Xhat;
end;

zpathhist = imarray{nf};
textfile = fopen('dot_state_estimates.txt','w');
fprintf(textfile,['State estimates of dot location for each frame of dot.avi (frame: x, y):\n']);
for i=1:nf
    %plot all estimated states on last image
    zpathhist( Xhatsnake(i,2), Xhatsnake(i,1),:) = [0 1 0];
    %write location of estimated states in textfile:
    fprintf(textfile,'%i: %i, %i\n',i,Xhatsnake(i,:));
end;
fclose(textfile);
imwrite(im2uint8(zpathhist),'dot_states_plot.png','png');

imwrite(im2uint8(synth_imarray{1}),'dot_trackframe1.png','png');
imwrite(im2uint8(synth_imarray{2}),'dot_trackframe2.png','png');

% we're done! now output an AVI
synthname = 'dot_tracked.avi';
synthmov = avifile(synthname, 'Compression', 'None');

for t=1:nf
   % print what frame we're on
   if mod(t,10)==0, written_frames=t, end
   % convert truecolor image to output frame
   output_frame = im2frame(im2uint8(synth_imarray{t}));
   % write movie frame
   synthmov = addframe(synthmov, output_frame);
end

% finish writing movie
synthmov = close(synthmov);