function output = sub_cycle_sim(f0,Q,k,F0,Fe,df,tau,beta,Fs,ntaps,fc,n_pts,PLOT)
%
% This script solves the equation for a damped-driven harmonic oscillator
% with the parameters given below. This script applies a STRETCHED
% EXPONENTIAL electrostatic force at t = 3*Q*T at 0-phase.
%
%
% Based on the following works:
%
% [1] Giridharagopal, R. et al. Nano letters 12.2 (2012): 893-898.
% [2] Karatay, D. U., et al. Review of Scientific Instruments 87.5 (2016): 053702.
%
% Analysis is done as follows:
%  
% Window the data and apply a blackman window-function, then apply a FIR
% filter, then Hilbert transform to get the phase and instantaneous
% frequency, which is plotted in the last line.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%           INPUTS
%
%
% Cantilever parameters:
%
% f0    = resonance frequency (Hz)
% Q     = Q-factor 
% k     = spring constant (N/m)

% Force parameters:
%
% F0    = drive force (N)
% Fe    = electrostatic force (N)
% df    = maximum frequency shift (Hz)
% tau   = relaxation time-constant (s)
% beta  = stretching factor (0 < beta < 1)

% Filter/simulation parameters:
%
% Fs    = sample rate (samples/s)
% ntaps = # coefficients for FIR filter (MUST BE ODD)
% fc    = FIR filter cutoff frequency (Hz)
% n_pts = # of points around the pulse to analyze/plot
% PLOT  = plot result (BOOLEAN)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

syms y(t) t

W0 = f0*2*pi;       % Ang. freq.
T = 1/f0;           % Period
m = k/W0^2;
dW = df*2*pi;

ts = 1/Fs;
tTrig = Q*T*3;                  % Trigger time at 3*Q periods into the sim.
tTotal = tTrig*2;               % Total sim time

t0 = 0:ts:tTrig;                % Ring-up time array
t1 = t0(end):ts:tTotal;        % Pulse-applied time array

FdFunc = @(t) F0*sin(W0*t);                   % Drive force

% This function makes the 2nd order diff eq. into a system of 1st orders:
[V0] = odeToVectorField(diff(diff(y)) + W0/Q*diff(y) + W0^2*y == FdFunc(t)/m);
M0 = matlabFunction(V0,'vars', {'t','Y'});   % Make it a Matlab function
sol0 = ode45(M0,[t0(1),t0(end)],[0 1.9e-9]);      % Solve it, y(0) = 0, y'(0) = 1

% Exponential function:
expParams = [tau];
C = @(params,t) (1-exp(-((t-t1(1))/params(1)).^beta));

% Frequency change(t) after exponential pulse is applied:
dWFunc = @(t) W0 - dW * C(expParams,t);

% This is the actual system response with an exponential pulse applied at t1(1):
[V1] = odeToVectorField(diff(diff(y)) + dWFunc(t)/Q*diff(y) + dWFunc(t)^2*y == Fe*C(expParams,t)/m + FdFunc(t)/m);
M1 = matlabFunction(V1,'vars', {'t','Y'});                   %Make it a Matlab function
sol1 = ode45(M1,[t1(1),t1(end)],[deval(sol0,t0(end))]);     %Solve it


y0 = deval(sol0,t0,1);
y1 = deval(sol1,t1,1);
tf = horzcat(t0,t1(2:end));
yf = horzcat(y0,y1(2:end));


Nf = 1/ts/2; % Nyquist frequency

% Number of samples to use around the trigger pulse (same number before+after):
% Sample number when trigger occured:
st = length(y0);

bwindow = blackman(length(yf(length(t0)-n_pts/2:length(t0)+n_pts/2-1)));

windowedSig = yf(length(t0)-n_pts/2:length(t0)+n_pts/2-1).*bwindow';        % Windowed and filtered signal
twindowed = tf(length(t0)-n_pts/2:length(t0)+n_pts/2-1);                % Time for windowed signal

FILT = fir1(ntaps,[(f0-fc/2)/Nf,(f0+fc/2)/Nf],'bandpass',blackman(ntaps+1));
filteredData = conv(windowedSig,FILT,'same');

% Take the hilbert transform of the signal:
z = hilbert(filteredData);
% Get the phase:
phi = unwrap(angle(z));

% Linear fit to the phase, can be used to subtract the resonance before
% filtering/differentiating:
phaseFit = polyfit(twindowed(end/4:end/3)-twindowed(1),phi(end/4:end/3),1);
yData = (twindowed-twindowed(1))*phaseFit(1) + phaseFit(2);

filtPhi = sgolayfilt(phi-yData,1,5);

Fs = 1/(t0(2)-t0(1));
instFreq = Fs./(2*pi)*diff(filtPhi);

% Correct the time, which is shifted by (ntaps - 1)/2:
tShift = twindowed((ntaps-1)/2:end-(ntaps-1)/2-1)-t0(end);
instFreq = instFreq(1:end-(ntaps-2));

if(PLOT)
    plot(tShift,instFreq);
    set(gca,'xlim',[0,0.3e-3])
end

output = vertcat(tShift,instFreq);