% Lab6. This program demonstrates that interest points can be tracked in a video.
% Matt Boutell


clear all;

% Read in the avi file
%vid = aviread('Checkerboard.avi');
startFrame = 1;
endFrame = 50; %15;
nFrames = endFrame - startFrame + 1;
vid = aviread('Checker92.avi', [startFrame:endFrame]);
%vid = mmread('Canon 025.avi');

% Extract images into a 4D array, x, y, band, t
skip = 1;
imArray = zeros(120/skip, 160/skip, nFrames);
for i = 1:nFrames
%    ar = rgb2gray(vid.frames(i).cdata); % if mmread
     ar = rgb2gray(vid(i).cdata); % if aviread
     imArray(:,:,i) = ar(1:skip:size(ar,1), 1:skip:size(ar,2));
end

% Find interest points in the first frame.
[nr, nc] = size(imArray(:,:,1));

w = 7; % 7x7 patches
h = (w-1)/2;
result = zeros(nr, nc);
for r = 1+h:nr-h
    for c = 1+h:nc-h
        result(r,c) = interest(imArray(r-h:r+h, c-h:c+h, 1));
    end
end

% Threshold the interest points
%figure; imshow(uint8(imArray(:,:,1)));
m = max(max(result));
r2 = result/m;
%figure; imshow(r2)
r3 = r2;
r3(find(r3<0.7)) = 0;
%figure; imshow(r3);

% Extract the interest points
[rAr,cAr] = find(r3>0);
f = [rAr cAr];

% Combine ones that are close to each other.
[rOut, cOut] = clusterPoints(rAr, cAr);

% Color them in the first frame
map = makeColorMap(size(rOut, 1))*255;
%ar = vid.frames(1).cdata; % if mmread
ar = vid(1).cdata; % if aviread
ar = ar(1:skip:size(ar,1), 1:skip:size(ar,2), :);
ar = colorInterestPoints(ar, map, rOut, cOut);
ar = imresize(ar, 2);
mov(1) = im2frame(ar);

% Look for them in the next frame

% The number of rows and columns to search for the match.
sr = h*3; % Look for 21 pixels in ech direction
sc = h*3;

for frIdx = 2:nFrames
    fprintf('Processing frame %d\n', frIdx);
    for ptIdx = 1:size(rOut,1)
        %Grab neighborhood around interest point in previous frame
        if (rOut(ptIdx,1) == 0 & cOut(ptIdx,1)==0) continue; end
        
        r = rOut(ptIdx,1);
        c = cOut(ptIdx,1);
        patch = imArray(r-h:r+h, c-h:c+h, frIdx-1);
        patch = patch - mean(mean(patch));

        % Search for it in this image.
        maxMatch = 0;
        matchI = 0;
        matchJ = 0;
        for i = r-sr:r+sr
            for j = c-sc:c+sc
                % Skip borders
                if (i<1+h | i>nr-h | j < 1+h | j>nc-h) continue; end  
                searchPatch = imArray(i-h:i+h, j-h:j+h, frIdx);
                searchPatch = searchPatch - mean(mean(searchPatch));
                prod = patch .* searchPatch;
                match = sum(sum(prod));
                if (match > maxMatch)
                    maxMatch = match;
                    matchI = i;
                    matchJ = j;
                end
                
            end
        end

        % Check if we found a match.
        % Non-matches will have value (0,0) and be ignored next time
 
            
        %fprintf('Match for (%d,%d) at (%d,%d)\n', r,c,matchI, matchJ);
        if (matchI ~= rOut(ptIdx,1)-sr)
            rOut(ptIdx,1) = matchI;
        end
        
        if (matchJ ~= cOut(ptIdx,1)-sc)
            cOut(ptIdx,1) = matchJ;
        end
    end

    % Color and save new interest points
    ar = vid(frIdx).cdata;
    ar = ar(1:skip:size(ar,1), 1:skip:size(ar,2), :);
    ar = colorInterestPoints(ar, map, rOut, cOut);
    ar = imresize(ar, 2);
    mov(frIdx) = im2frame(ar);
end

%movie2avi(mov,'out.avi','fps', 4); 
movie(mov, -4, 8);