%% Set parameters
fs = 200000;              % Sampling rate (Hz) (max 200000)
T  = 120;                  % Acquisition duration (s)
Ns = fs * T;              % Total number of samples

device = 'Dev1';                 % Configure DAQ in NI MAX
input_channel = 'ai1';           % Signals you want to measure should feed into DAQ here
output_channel = 'ai2';          % NOTE: DO NOT USE OSCOPE PROBES IN DAQ BOARD

% Create DAQ object
d = daq("ni");
d.Rate = fs;

addinput(d, device, input_channel, "Voltage");
addinput(d, device, output_channel, "Voltage");

%% Acquire data
disp('Acquiring data...');
data = read(d, Ns, "OutputFormat", "Matrix");
disp('Done.');

% Split channels
t_sample = (0:Ns-1)'/fs;
x1 = data(:,1);
x2 = data(:,2);

%% Plot the signals
figure(1);

tiledlayout(2,1)

ax1=nexttile;
plot(t_sample, x1)
ylabel('Voltage (V)'); xlabel('Time (s)');
title('Input Voltage V_{in}')

ax2=nexttile;
plot(t_sample, x2)
ylabel('Voltage (V)'); xlabel('Time (s)');
title('Output Voltage V_{out}')

%% Compute the short-time Fourier transform
delta_f = 100;                                                              % Frequency resolution
N_windows = fs/delta_f;                                                     % Number of windows determined by frequency resolution
myFFTLength = 2^ceil(log2(N_windows));                                      % Choose FFT length as factor of 2 greater than N_windows
myWindow = hann(N_windows,"periodic");                                      % Sets bin size: fs/N_windows
[s1,f,t]=stft(x1,fs,FFTLength=myFFTLength,Window=myWindow,FrequencyRange='onesided');                 % Sets interpolated bin size: fs/FFTLength
[s2,~,~]=stft(x2,fs,FFTLength=myFFTLength,Window=myWindow,FrequencyRange='onesided');


% Normalize data by maximum value of input signal
% norm_factor = max(s1, [], 'all');
% s1_norm = s1 / norm_factor;
% s2_norm = s2 / norm_factor;

%% Plot the power spectra of your signals
startFreq = 1000;
stopFreq = 100000;
xbounds = [startFreq, stopFreq];                                            % Sets boundaries on x-axis
ybounds = [-25 5];                                                          % Sets boundaries on y-axis

% Power spectra
P1 = abs(s1).^2;
P2 = abs(s2).^2;

% Normalization and conversion to dB
ref = max(P1, [], "all");
P1dB = 10*log10(P1/ref + eps);
P2dB = 10*log10(P2/ref + eps);


figure(2);
clf

frame_skip = 100;
for t_ind = 1:frame_skip:numel(t)                                           % Plot every frame_skip-th curve on top of each other

subplot(2,1,1)                                                              % Plot decibels vs frequency on semilog plot 
semilogx(f,P1dB(:,t_ind),'r')    
xlim(xbounds); ylim(ybounds);
ylabel('Normalized Power (dB)'); xlabel('Frequency (Hz)')
title('Input Power Spectra')
hold on;
pause(0.01)

subplot(2,1,2)
semilogx(f,P2dB(:,t_ind),'r') 
xlim(xbounds); ylim(ybounds);
ylabel('Normalized Power (dB)'); xlabel('Frequency (Hz)')
title('Output Power Spectra')
hold on;
pause(0.001)

end

% Guiding Bode plot magnitude
[mag_dB, f_ctr] = bodeMagHilbert(x1, x2, fs, startFreq, stopFreq);
figure(2)
subplot(2,1,2)
semilogx(f_ctr,mag_dB,"Color",[0 0 0 0.3], "LineWidth",2)


%% Save data to a .mat file
filename = 'Signal_Data.mat';                                               % Set name of file to save
save(filename, 'fs','T','Ns','t_sample','x1','x2', ...
    'N_windows','FFTLength','s1', 's2', 'f','t_ind', '-v7.3');                  % Set variables to be saved

fprintf('Variables saved successfully to %s\n', filename);                  % Might take a while if saving big variables

%% Open saved data in .mat file
load('Signal_Data.mat');

%% Functions
function [mag_dB, f_ctr] = bodeMagHilbert(x1, x2, fs, f_start, f_stop)
% Estimate magnitude response from a chirp using Hilbert demodulation

% Remove DC offset from input and output
x1w = x1(:) - mean(x1(:));
x2w = x2(:) - mean(x2(:));

N = numel(x1w);

% Extract instantaneous phase of the input chirp
analytic_x1 = hilbert(x1w);
phi = unwrap(angle(analytic_x1));

% Estimate instantaneous frequency from phase derivative
f_inst = nan(N,1);
f_inst(2:end) = diff(phi) * fs/(2*pi);

% Smooth frequency estimate to reduce noise
smooth_len = max(5, round(0.05*fs));
f_smooth = movmedian(f_inst, smooth_len, "omitnan");

% Build complex reference from chirp phase
ref = exp(-1j*phi);

% Demodulate input and output signals
z1 = x1w .* ref;
z2 = x2w .* ref;

% Low-pass filter to extract complex envelopes
envBW = 20;
[b,a] = butter(4, envBW/(fs/2), "low");
A1 = filtfilt(b,a,z1);
A2 = filtfilt(b,a,z2);

% Compute magnitude ratio versus time
R_t = abs(A2) ./ abs(A1);

% Select valid time samples
good = isfinite(f_smooth);
amp_thresh = 0.05 * median(abs(A1), "omitnan");
good = good & (abs(A1) > amp_thresh);
good = good & (f_smooth >= f_start) & (f_smooth <= f_stop);

% Extract valid frequency and magnitude data
f_use = f_smooth(good);
R_use = R_t(good);

% Bin magnitude data on a logarithmic frequency axis
Nbins = 200;
edges = logspace(log10(f_start), log10(f_stop), Nbins+1);
f_ctr = sqrt(edges(1:end-1).*edges(2:end)).';

R_bin = nan(Nbins,1);
for k = 1:Nbins
    idx = (f_use >= edges(k)) & (f_use < edges(k+1));
    if any(idx)
        R_bin(k) = mean(R_use(idx), "omitnan");
    end
end

% Convert magnitude to decibels
mag_dB = 20*log10(R_bin);
end
