close all
clear
clc

% File generated by the TransferFunctionCheck example
filename = "test.csv";

% There are some annoying warnings for text parsing and matrix solve
warning('off','MATLAB:rankDeficientMatrix');
warning('off','MATLAB:table:ModifiedAndSavedVarnames');

data = readtable(filename);

lines = readlines(filename);
data_start = find(startsWith(lines,"f(hz),"));
if numel(data_start) ~= 1
    error("Could not parse type")
end
lines = lines(1:data_start-1);
fprintf("%s\n",lines)

% Try and work out types
filters = {};
filter_index = find(startsWith(lines,"LowPassFilterConstDtFloat"));
for i = 1:numel(filter_index)
    filters{end+1} = parse_low_pass(lines, filter_index(i)); %#ok<SAGROW> 
end
filter_index = find(startsWith(lines,"LowPassFilterFloat"));
for i = 1:numel(filter_index)
    filters{end+1} = parse_low_pass(lines, filter_index(i)); %#ok<SAGROW> 
end
filter_index = find(startsWith(lines,"LowPassFilter2pFloat"));
for i = 1:numel(filter_index)
    filters{end+1} = parse_biquad(lines, filter_index(i)); %#ok<SAGROW> 
end
filter_index = find(startsWith(lines,"NotchFilterFloat"));
for i = 1:numel(filter_index)
    filters{end+1} = parse_notch(lines, filter_index(i)); %#ok<SAGROW> 
end

if numel(filters) == 0
    error("Failed to parse filters")
end

% Make sure all have the same sample rate
for i = 2:numel(filters)
    if filters{i}.sample_rate ~= filters{1}.sample_rate
        error("Sample rate mismatch")
    end
end
sample_freq = filters{1}.sample_rate;

% Parse time from headers
for i = numel(data.Properties.VariableDescriptions):-1:2
    time(i-1) = sscanf(data.Properties.VariableDescriptions{i},'t = %f');
end
if any(isnan(time))
    error("Got NAN time")
end

% Parse input frequency and output amplitude
freq = data{:,1};
if any(isnan(freq))
    error("Got NAN freq")
end

output = data{:,2:end};
if any(isnan(output))
    error("Got NAN time")
end

% Brute force the amplitude and phase from the sweep
% This method is currently used in the js filter tool

% Best fit to sin to get amplitude, phase and DC offset
% https://math.stackexchange.com/questions/3926007/least-squares-regression-of-sine-wave
% Expecting output to be of same frequency at input
% Z = a*sin(t*kt + p) + O
% A = a*cos(p)
% B = a*sin(p)
% S = sin(t*kt)
% C = cos(t*kt)

num_sweep = numel(freq);

amplitude = zeros(num_sweep,1);
phase = zeros(num_sweep,1);

% Only fit to last 100 points, better aproximation for steady state
num_best_fit_points = 100;
best_fit_index = false(1,numel(time));
best_fit_index(end-num_best_fit_points+1:end) = true;
for i = 1:num_sweep
 
    X = [sin(time(best_fit_index) * 2*pi*freq(i));
         cos(time(best_fit_index) * 2*pi*freq(i)); 
         zeros(1,num_best_fit_points)];
    Y = output(i,best_fit_index);

    % Z = a*sin(t*kt + p) + O
    Z = Y / X;

    amplitude(i) = sqrt(Z(1)^2 + Z(2)^2);
    phase(i) = atan2d(Z(2), Z(1));
    % DC_offset = Z(3);

end

% Calculate using transfer function
Z = exp(1i*pi*(freq/(sample_freq*0.5)));

for i = numel(filters):-1:1
    % Transfer function of each filter
    H(:,i) = filters{i}.transfer_function(Z);
end

% multiply all transfer functions together
H_all = prod(H,2); 

% Extract amplitude and phase from transfer function
calc_amplitude = abs(H_all);
calc_phase = atan2d(imag(H_all),real(H_all));

% Caculate and print error size
amp_error = abs(amplitude - calc_amplitude);
fprintf('Amplitude error - max: %g mean: %g\n',max(amp_error), mean(amp_error))

phgase_error = abs(phase - calc_phase);
fprintf('Phase error - max: %g mean: %g\n',max(phgase_error), mean(phgase_error))

% Bode plot
figure
tiledlayout(2,1)
nexttile
hold all
plot(freq, amplitude)
plot(freq, calc_amplitude)
ylabel('magnitude')
legend('Brute force','transfer function')

nexttile
hold all
plot(freq, phase)
plot(freq, calc_phase)
xlabel('Frequency (Hz)')
ylabel('phase')

function ret = parse_low_pass(lines, start_index)
    found = startsWith(lines(start_index),"LowPassFilterFloat") || startsWith(lines(start_index),"LowPassFilterConstDtFloat");
    if ~found
        error("Expecting LowPassFilterConstDtFloat or LowPassFilterFloat")
    end
    
    % Next line gives sample rate and cutoff
    temp =  sscanf(lines(start_index+1),'Sample rate: %f Hz, Cutoff: %f Hz');
    ret.sample_rate = temp(1);
    ret.target_freq = temp(2);

    % Assume we know the form
    alpha = sscanf(lines(start_index+3),'a: %f');

    ret.transfer_function = @(z) alpha./(1-(1-alpha)*z.^-1);

end

function ret = parse_biquad(lines, start_index)
    if ~startsWith(lines(start_index),"LowPassFilter2pFloat")
        error("Expecting LowPassFilter2pFloat")
    end
    
    % Next line gives sample rate and cutoff
    temp =  sscanf(lines(start_index+1),'Sample rate: %f Hz, Cutoff: %f Hz');
    ret.sample_rate = temp(1);
    ret.target_freq = temp(2);

    % Assume we know the form
    coefficents = sscanf(lines(start_index+3),'a1: %f, a2: %f, b0: %f, b1: %f, b2: %f');
 
    a1 = coefficents(1);
    a2 = coefficents(2);
    b0 = coefficents(3);
    b1 = coefficents(4);
    b2 = coefficents(5);

    ret.transfer_function = @(z) (b0 + b1*z.^-1 + b2*z.^-2)./(1 + a1*z.^-1 + a2*z.^-2);

end

function ret = parse_notch(lines, start_index)
    if ~startsWith(lines(start_index),"NotchFilterFloat")
        error("Expecting NotchFilterFloat")
    end
    
    % Next line gives sample rate and cutoff
    temp =  sscanf(lines(start_index+1),'Sample rate: %f Hz, Center: %f Hz');
    ret.sample_rate = temp(1);
    ret.target_freq = temp(2);

    % Assume we know the form
    coefficents = sscanf(lines(start_index+3),'a0: %f, a1: %f, a2: %f, b0: %f, b1: %f, b2: %f');
 
    a0 = coefficents(1);
    a1 = coefficents(2);
    a2 = coefficents(3);
    b0 = coefficents(4);
    b1 = coefficents(5);
    b2 = coefficents(6);

    ret.transfer_function = @(z) (b0 + b1*z.^-1 + b2*z.^-2)./(a0 + a1*z.^-1 + a2*z.^-2);

end
