function track_baseball
%  ENGN0040 Homework 4 2014: Program to track motion of a baseball
%
    clear all;
    close all;
    fps         = 30000;       % camera frame rate (frames/sec)
    dt          = 1/fps;       % Time between successive frames
    baseball_diameter = 7.468;   % physical spacing between scale marks, cm 

    csvfilename = 'test1.csv';

%   Read the video and convert the video frames to grayscale images 
    video = VideoReader('baseball.avi');
    k = 0;
    while hasFrame(video)
        k = k + 1;
        image_list(:,:,k) = rgb2gray(readFrame(video));
    end  

    time = 0:dt:(k-1)/fps;  % Vector of time values  
 
%% Analysis
%   Ask the user to identify the ball diameter in the first image    
    diameter_pixels = calibrate_camera(image_list(:,:,1));
    pixpercm =  diameter_pixels/baseball_diameter;

    [xposition,yposition] = tracker(image_list,diameter_pixels);
%
%   Plot a graph of horizontal position-v-time
    posfigure = figure;
    axes1 = axes('Parent',posfigure,'YGrid','on','XGrid','on','FontSize',14);
    box(axes1,'on');
    hold(axes1,'on');
    ylabel('Position (cm)');
    xlabel('Time (seconds)');
    title('Mass position -v- time');  
    plot(time,xposition/pixpercm);
    
    

   
    % Save the position-v-time data to a csv file
    csvwrite(csvfilename,[time',xposition'/pixpercm]);

end

function diameter = calibrate_camera(refImage)
%   Function to determine # pixels corresponding to the ball diameter
%   User must select the scale marks

    figure;
    imshow(refImage);
    hold on;
    title('Click on 2 points to set ball diameter in image')
    disp('Click on 2 points to set ball diameter in image')
    [xtick_space,ytick_space] = ginput(2);
    diameter   = sqrt( (xtick_space(2)-xtick_space(1)).^2 + (ytick_space(2)-ytick_space(1)).^2 );
    close all;
end
%%
function [x,y] = tracker(image_list,diameter)
% Function to track a point in a sequence of images using MATLAB
% built-in image correclation functions
%
% image_list        =  list of grayscale images
% diameter          =  ball diameter, in pixels
%
% x - Vector specifying x position of a reference point in each image
% y - Vector specifying y position of a reference point in each image
% x,y are measured from the top left corner of the image, in pixels.
% x is horizontal, y is vertical.

    %% Load the correlation image
    [M,N,n_frames]    = size(image_list);
%
    [xc,yc,x0,y0] = corr_template_select(image_list(:,:,1));
    x(1) = x0;
    y(1) = y0;
%   Crop the refrence template from the first image    
    crop_rect         = [max(1,floor(xc(1))) , max(1,floor(yc(1))),min(N,ceil(xc(2))) , min(M,ceil(yc(2)))];
    Icrp              = image_list(crop_rect(2):crop_rect(4),crop_rect(1):crop_rect(3),1);   % Cropped reference image

    %% Track reference image in all subsequent images
    for k=2:n_frames
 % Crop a region of the current image that is expected to include the reference image
        crop_rect2 = [max(1,floor(x(k-1)-diameter/4)) , max(1,floor(y(k-1)-diameter/4)),min(N,ceil(x(k-1)+diameter/4)) , min(M,ceil(y(k-1)+diameter/4))];
        Icrp2      = image_list(crop_rect2(2):crop_rect2(4),crop_rect2(1):crop_rect2(3),k); %Cropped displaced image

% Find the reference image Icrp in the larger current image Icrp2    
        C          = normxcorr2(Icrp,Icrp2);       % Correlation matrix 
        [zpixhold,xpixhold] = find(C == max(C(:)));
%   The coords of the top left corner of the ref image in the cropped current image
        voffset = zpixhold-size(Icrp,1);
        hoffset = xpixhold-size(Icrp,2);                    
%   Coords of top left corner in the full current image      
        voffset = voffset + crop_rect2(2)-1;
        hoffset = hoffset + crop_rect2(1)-1;
%   New coords of the reference point being tracked            
        x0new = hoffset + x0 - crop_rect(1);
        y0new = voffset + y0 - crop_rect(2);
%   Improve the resolution of the correlation using cpcorr
        adjustedpoints = cpcorr([x0new,y0new],[x0,y0],image_list(:,:,k),image_list(:,:,1));
%             
        x(k) = adjustedpoints(1);
        y(k) = adjustedpoints(2);
%       Display an image showing the reference point in the current image
        hold off
        imshow(image_list(:,:,k))
        hold on
        plot(x(k),y(k),'gx','markersize',12,'linew',2)
        pause(0.05);
    end
 
end
%%
function [xc,yc,xi,yi] = corr_template_select(I)
%  Function to ask the user to select a sub-image and reference
%  point to use for image correlation
%
%  I  Image to be displayed to the user
%   xc,yc  upper left, lower right coordinates of rectangle
%   xi,yi  coordinates of referemce point
    figure(1);
    clf;
    imshow(I);
    hold on;

    title('Select upper left, then lower right corner of correlation template');
    disp('Select upper left, then lower right corner of correlation template');
    fprintf('\n')
    [xc,yc] = ginput(2);
    plot([xc(1),xc(2),xc(2),xc(1),xc(1)],[yc(1),yc(1),yc(2),yc(2),yc(1)],'r-','linew',2)

    title('Select a reference point inside template rectangle');
    fprintf('\n')
    [xi,yi] = ginput(1);
    plot(xi,yi,'bx','linew',2,'markersize',10)

    close;
end
