function track_crash
%  ENGN0040 Homework 4 2018: Program to track motion of a collision between
%  billiard balls
%
    clear all;
    close all;
    fps         = 3000;       % camera frame rate (frames/sec)
    dt          = 1/fps;       % Time between successive frames
    marker_spacing = 5.715;   % physical spacing between scale marks, cm 

    csvfilename = 'test4.csv';

%   Read the video and convert the video frames to grayscale images 
    video = VideoReader('billiard_impact.avi');
    k = 0;

    while hasFrame(video)
        k = k + 1;
        image_list(:,:,k) = rgb2gray(readFrame(video));
    end
    size(image_list)   
    time = 0:dt:(k-1)/fps;  % Vector of time values  
 
%% Analysis
%   Ask the user to identify the ball diameter in the first image    
    spacing_pixels = calibrate_camera(image_list(:,:,1));
    pixpercm =  spacing_pixels/marker_spacing;

    [xposition1,yposition1] = tracker(image_list,spacing_pixels/2);
    [xposition2,yposition2] = tracker(image_list,spacing_pixels/2);
    %
%   Laplace smoothing
    for n=1:3
       for i=2:k-1
           xposition1(i) = 0.5*(xposition1(i-1)+xposition1(i+1));
           yposition1(i) = 0.5*(yposition1(i-1)+yposition1(i+1));
           xposition2(i) = 0.5*(xposition2(i-1)+xposition2(i+1));
           yposition2(i) = 0.5*(yposition2(i-1)+yposition2(i+1)); 
       end
    end
    if (yposition2(1)>yposition1(1))
        junitv = [xposition2(end)-xposition2(1),yposition2(end)-yposition2(1)];
        junitv = junitv/sqrt(dot(junitv,junitv));
        iunitv = [junitv(2),-junitv(1)];
    else
        junitv = [xposition1(end)-xposition1(1),yposition1(end)-yposition1(1)];
        junitv = junitv/sqrt(dot(junitv,junitv));
        iunitv = [junitv(2),-junitv(1)];
    end
%   Basis change
    for i=1:length(xposition1)
       xp1(i) = dot(iunitv,[xposition1(i),yposition1(i)]);
       yp1(i) = dot(junitv,[xposition1(i),yposition1(i)]);
    end
    for i=1:length(xposition2)
       xp2(i) = dot(iunitv,[xposition2(i),yposition2(i)]);
       yp2(i) = dot(junitv,[xposition2(i),yposition2(i)]); 
    end    
    %    plot(time,xposition/pixpercm);
    posfigure = figure;
    axes1 = axes('Parent',posfigure,'YGrid','on','XGrid','on','FontSize',14);
    box(axes1,'on');
    hold(axes1,'on');
    ylabel('Position (cm)');
    xlabel('Position (cm)');
    title('Marker position -v- time'); 
    plot(xp1/pixpercm,yp1/pixpercm);
    plot(xp2/pixpercm,yp2/pixpercm);  
    % Save the position-v-time data to a csv file
    csvwrite(csvfilename,[time',xp1'/pixpercm,yp1'/pixpercm,xp2'/pixpercm,yp2'/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 diametrically opposed points on a ball')
    disp('Click on 2 diametrically opposed points on a ball')
    [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,spacing)
% 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)-spacing)) , max(1,floor(y(k-1)-spacing)),min(N,ceil(x(k-1)+spacing)) , min(M,ceil(y(k-1)+spacing))];
        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
    y = N-y;
 
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
