%load SurvTestFrames.mat;
%load SurvTestFrames2.mat;

cf = mod(repmat([1:320],[240 1]) + repmat([1:240]',[1 320]),2);
t = 0.1;

for i=1:size(ag,3)
    imshow(ag(:,:,i));
    title('original')
    pause(0.05)
end

for i=1:size(ag,3)
    ag1(:,:,i) = histeq(ag(:,:,i));
    imshow(ag1(:,:,i));
    title('equalized');
    pause(0.01)
end

BG = median(ag1,3);

besti = []; bestj=[];

BGsample = BG(16+[0:3:209],16+[0:3:289]);
ag2 = [];
for i=1:size(ag,3)
  mses = zeros(31);
  for oi=1:31
    for oj=1:31
        mses(oi,oj) = sum(sum(abs(BGsample - ag1(oi+[0:3:209],oj+[0:3:289],i))));
    end
  end
  [m, ind] = min(mses(:));
  ind = ind-1;
  bestj(i) = floor(ind/31)+1;
  besti(i) = mod(ind,31) + 1;
  disp([i besti(i) bestj(i)])
  ag2(:,:,i) = ag(besti(i)+[0:209],bestj(i)+[0:289],i);
  imshow(ag2(:,:,i));
  title('Correcting for camera movement');
  pause(0.001);
end

BG = median(ag2,3);

FG1 = abs(ag2 - repmat(BG,[1 1 size(ag,3)])) > t;

for i=1:size(ag,3)
    fg = zeros(240,320);
    fg(besti(i)+[0:209],bestj(i)+[0:289]) = FG1(:,:,i);
    imshow([fg   (1-fg).*ag1(:,:,i)+fg.*cf]);
    title(['initial foreground (with noise) - threshold from median is ',num2str(t)])
    pause(0.1);
end

h = ones(1,1,3);
FG1a = imerode(FG1,h);
FG1a = imdilate(FG1a,h);

for i=1:size(ag,3)
    fg = zeros(240,320);
    fg(besti(i)+[0:209],bestj(i)+[0:289]) = FG1a(:,:,i);
    imshow([fg   (1-fg).*ag1(:,:,i)+fg.*cf]);
    title('after closing (temporal)')
    pause(0.1);
end

h = [ 0 1 1 1 0; 1 1 1 1 1; 1 1 1 1 1 ; 1 1 1 1 1; 0 1 1 1 0];

FG1b = imerode(FG1a,h);
FG1b = imdilate(FG1b,h);

for i=1:size(ag,3)
    fg = zeros(240,320);
    fg(besti(i)+[0:209],bestj(i)+[0:289]) = FG1b(:,:,i);
    imshow([fg   (1-fg).*ag1(:,:,i)+fg.*cf]);
    title('after closing (individual frames)')
    pause(0.1);
end

h = strel('disk',10)
FG1c = imdilate(FG1b,h);
FG1c = imerode(FG1c,h);

for i=1:size(ag,3)
    fg = zeros(240,320);
    fg(besti(i)+[0:209],bestj(i)+[0:289]) = FG1c(:,:,i);
    imshow([fg   (1-fg).*ag1(:,:,i)+fg.*cf]);
    title('after opening with a large disk (individual frames)')
    pause(0.1);
end
