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);