% msd_fd.m
%
% Used to plot the solution of a mass-spring-damper system using
% a backward finite difference approach.
%
% Gary P. Scavone
% McGill University, 2004.

% Simulation Parameters

fs = 44100;      % sample rate
T = 1/fs;        % sample period
N = 1000;        % iterations

% System Parameters

m = 0.5;          % mass in kg
k = 5000000;      % spring constant (N/m)
w0 = sqrt(k/m);   % natural angular frequency of system
f0 = w0/(2*pi);   % natural frequency of system (no damping)
mu = 0.5;         % damping constant
x0 = 0.01;        % initial displacement
v0 = 0.1;         % initial velocity

% System Equation: m(x'') + mu(x') + kx = 0, where x' = dx/dt

% Using the Laplace Transform:
%
% m(s^2*X(s) - s*x0 - v0) + mu(s*X(s) - x0) + k X(s) = 0
%
% X(s) = (m*s*x0 + m*v0 + mu*x0) / (m*s^2 + mu*s + k)

% Now do s-plane to z-plane mapping for the backward finite difference: s =
% (1-z^-1)/T and s^2 = (1 - 2z^-1 + z^-2)/T^2. Note that the constant
% initial condition terms in the numerator should be scaled by the sample
% rate (by a factor of 1/T) when converted from continuous to discrete
% time. Another way to think about this is a continuous delta function
% needs to be scaled by 1/T to maintain the same "integrated area" over
% time and to properly bandlimit it: delta(t) -> delta[n]/T:
%
% X(z) = (b0 + b1*z^-1) / (a0 + a1*z^-1 + a2*z^-2),
%
% where:
b0 = m*x0*T + m*v0*T^2 + mu*x0*T^2;
b1 = -m*x0*T;
a0 = m + mu*T + k*T^2;
a1 = -(2*m + mu*T);
a2 = m;

% Now find the difference equation via the inverse z-Transform. 
%
% x[n] = b0/(T*a0)*delta[n] + b1/(T*a0)*delta[n-1] - a1/a0*x[n-1] - a2/a0*x[n-2]

% Displacement Initialization
x = zeros(1,N);
xm1 = 0;       % x[n-1]
xm2 = xm1;     % x[n-2]

% Now do simulation:
for n = 1:N
  
  x(n) = -xm1*a1 - xm2*a2;
  if n == 1
    x(n) = x(n) + b0/T;
  end
  if n == 2
      x(n) = x(n) + b1/T;
  end
  x(n) = x(n) / a0;
  xm2 = xm1;
  xm1 = x(n);
  
end

plot(x)
grid
xlabel('Time Steps (samples)');
ylabel('Mass Displacement (m)');
title('Finite Difference');

hold on

% Alternatively, we can take the time-domain differential equation and make
% the backward finite difference substitutions directly in the time domain,
% solving for x[n] in terms of x[n-1] and x[n-2]. The only issue is that we
% will not have explicit reference to the initial displacement and velocity
% terms. However, we can initialize x[n-1] as x0 and then solve the
% first-order backward finite difference approximation to find x[n-2]:
%
% v0 = (xm1 - xm2)/T => so, xm2 = xm1 - v0*T;

T2 = T^2;
a0 = m / T2 + mu / T + k;
a1 = (2 * m) / T2 + mu / T;
a2 = m / T2;

% Initialize the state variables
xm1 = x0;
xm2 = xm1 - v0*T;

% Run the iteration
for n = 1:N
  x(n) = xm1*a1 - xm2*a2;
  x(n) = x(n) / a0;
  xm2 = xm1;
  xm1 = x(n);
end

plot(x, '*')
hold off
