%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%
%%%             ****** Purpose of this MATLAB module ******
%%%
%%% The main purpose is to study the Harmonic occillator by extracting
%%% its motion from a video of the oscillator.
%%%
%%% This code is largely based on the algorithm discussed at:
%%%
%%%   https://www.mathworks.com/company/newsletters/articles/tracking
%%%   -objects-acquiring-and-analyzing-image-sequences-in-matlab.html
%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Step 0: INPUT DATA 
% (to obtain the best performance, a few other parameters can be adjusted
% but one needs to read the code)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;

% path of the video file
videofile_path = 'pendulum1.mp4';

% first and last frame needed 
%frame_start = 200;
%frame_end = 1000;

% the following four parameters define a rectanglar region in the middle
% of the frames, which covers all the trajectory of the oscillator
X = 200;
Y = 650;
cutWidth = 700;
cutHeight= 400;


%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Step 1: INITIAL PROCESSING 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Read the video file
v = VideoReader(videofile_path);

% read the frames from v_start to v_end if needed
%v_frames = read(v,[frame_start,frame_end]);
v_frames = read(v);

% Turn the frames from RGB to gray
frame_num = size(v_frames, 4);
for k = frame_num:-1:1
    gray_frames(:, :, k) = rgb2gray(v_frames(:, :, :, k));
end

% cut the frames if needed; discard proper outer erea of the frames
for k = frame_num:-1:1
    frames_resized(:,:, k) = gray_frames(Y:Y+cutHeight, X:X+cutWidth, k);
end

% calculate frame differences between (k+fd)-th and k-th
% fd can be adjusted
fd=2;
for k = frame_num-fd:-1:1
    frames_diff(:, :, k) = imabsdiff(frames_resized(:, :, k), frames_resized(:, :, k+fd));
end

% calculate a threshold, to turn gray frames to black_and_white
thresh = graythresh(frames_diff);
bw = (frames_diff >= thresh * 255); 

%%%% Optional: visualize processed frames 
%frame_id = 30;
%imtool(v_frames(:,:,:,frame_id))
%imtool(gray_frames(:,:,frame_id))
%imtool(frames_diff(:,:,frame_id))
%imtool(bw(:,:, 35))

implay(im2uint8(bw));
R = input('Enter to continue or press 1 to stop (if the resized frames are not proper): ');
if R == 1
    return
end

%%%% Opitional: converting bw into a movie
%VidObj = VideoWriter('movie1.avi', 'Uncompressed AVI'); %set your file name and video compression
%VidObj.FrameRate = v.FrameRate; %set your frame rate
%open(VidObj);
%for f = 1:size(bw, 3)
%    writeVideo(VidObj, im2uint8(bw(:, :, f)));
%end
%close(VidObj);


%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Step 2: EXTRACT THE OSCILLATION 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% find bottoms of the rod
areacontrol = 500; % only consider area bigger than 100 as an object
bottoms = zeros(frame_num-fd, 2);
for k = 1:1:frame_num-fd
    L = bwlabel(bw(:, :, k)); % this function labels connected areas
    s = regionprops(L, 'area', 'extrema'); % area and center coordinate of L-th area
    area_vector = [s.Area];
    [tmp1, idx1] = max(area_vector);
    area_vector(1,idx1)=0;
    [tmp2, idx2] = max(area_vector);
    if size(tmp2,2)~=0 && tmp2 > areacontrol
        bottoms(k, 1) = (s(idx1).Extrema(5,1) + s(idx1).Extrema(6,1) + s(idx2).Extrema(5,1)+ s(idx2).Extrema(6,1))/4; % the 5 and 6 rows are bottom-left and bottom-right points
        bottoms(k, 2) = (s(idx1).Extrema(5,2) + s(idx1).Extrema(6,2) + s(idx2).Extrema(5,2)+ s(idx2).Extrema(6,2))/4;
    elseif size (tmp1,2) ~= 0  && tmp1 > areacontrol
        bottoms(k, 1) = (s(idx1).Extrema(5,1) + s(idx1).Extrema(6,1))/2;
        bottoms(k, 2) = (s(idx1).Extrema(5,2) + s(idx1).Extrema(6,2))/2;
    else 
        bottoms(k, :) = bottoms(k-1,:);
    end
end

%%% plot average bottoms
t = [1:frame_num-fd]/v.FrameRate;
fig = figure( 'Name', 'Average bottoms' );
abs = sqrt(times(bottoms(:,1),bottoms(:,1)) + times(bottoms(:,1),bottoms(:,1)));
subplot(2, 1, 1)
    px = plot(t, bottoms(:,1), '-s');
    ylabel('x');
    set(gca,'FontSize',12);
subplot(2, 1, 2)
    py = plot(t, bottoms(:,2), '-s');
    ylabel('y');
    set(gca,'FontSize',12);

xlabel('time (s)')
px.MarkerSize = 2; 
py.MarkerSize = 2; 

%set(fig,'PaperSize',[8 6]); %set the paper size to what you want  
%print(fig,'filename','-dpdf') % then print it

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Step 3: Fitting (only fit x(t))
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


tdata = [1:frame_num-fd]/v.FrameRate;
xdata = transpose(bottoms(:,1));
[fitresult, gof] = createFit(tdata, xdata);

disp(fitresult)
disp(['The oscillation period is T = 2pi/b1 = ', num2str(2*pi/fitresult.b1), ' seconds.', 'The damping coefficient is w =', num2str(fitresult.w), '.'])

 %%% Define a fuction that does the fitting job 
 
function [fitresult, gof] = createFit(t, x)
%  Input for fiting:
%      X Input : t
%      Y Output: x
%  Output:
%      fitresult : a fit object representing the fit.
%      gof : structure with goodness-of fit info.

% Preparation
x_mean = mean(x);
x_max = max(x);
x_min = min(x);
x_offset = x - x_mean;

num = 3; % try to find 3 crossings
crossing_t = zeros(num,1); % find num crossings
count = 0;
for k=1:1:size(t,2)-1
    if x_offset(k) < 0 && x_offset(k+1)>0
        count = count + 1;
        if count <= num
            crossing_t(count) = k;
        else break;
        end
    end
end

w0 = 0.01;
a0 = (x_max - x_min)/2.0;
b0 = 2* pi * (num-1) /(crossing_t(num)-crossing_t(1))/t(1,1);
c0 = b0 * (crossing_t(1)+ (crossing_t(2)-crossing_t(1))/4.0) * t(1,1);
d0 = x_mean;

[xData, yData] = prepareCurveData(t, x);

% Set up fittype and options.
ft = fittype('a1*exp(-w*x)*cos(b1*x - c1) + d1',...
    'independent',{'x'},...
    'coefficients',{'a1','b1','c1','d1','w'});
opts = fitoptions('Method', 'NonlinearLeastSquares' );
opts.Display = 'Off';
opts.Lower = [-Inf 0 -Inf];
opts.StartPoint = [a0 b0 c0 d0 w0];

% Fit model to data.
[fitresult, gof] = fit(xData, yData, ft, opts);

% Plot fit with data.
p = 2*pi/fitresult.b1;
fig = figure( 'Name', 'Fitting' );
h = plot(fitresult, xData, yData, '-s');
leg = legend(h, 'x-coordinate vs. t', ['fitting curve (period = ' num2str(p) ' )'], 'Location', 'NorthEast', 'Interpreter', 'none');
% Label axes
xlabel( 'time (s)', 'Interpreter', 'none' );
ylabel( 'x', 'Interpreter', 'none' );
h(1).MarkerSize = 6;
leg.FontSize = 16;
set(gca,'FontSize',14)
grid on

%set(fig,'PaperSize',[7.3 5.8]); %set the paper size to what you want  
%print(fig,'filename','-dpdf') % then print it

end
