clc; clear; close all;

pkg load signal;
pkg load control;

sampleRate      = 48000;
faustDSPtarget  = fullfile(                             ...
                            '..',                       ...
                            '..',                       ...
                            'Faust',                    ...
                            'testBeds',                 ...
                            'wienerHammerstein.dsp'     ...
                            );
workspaceTarget = fullfile(                             ...
                            '..',                       ...
                            '..',                       ...
                            'Faust',                    ...
                            'testBeds',                 ...
                            'wienerHammerstein.mat'     ...
                            );
                            
permission      = 'w';
precision       = '%.18f';
antialFIRTaps   = 16;
windowFcn       = @chebwin;
finalFrequency  = 23000;
fPoints         = 4096;
                            
% Manually define branch IIRs:
branchIIRs      = cell(0);

% 1
[z, p, k] = cheby1(                             ...
                    3,                          ...
                    5,                          ...
                    2 * 30 / sampleRate,        ...
                    'high'                      ...
                    );
                   
branchGain = 1;

branchIIRs{1, 1} = packBranchIIR(z, p, k, branchGain); 

% 2
[z, p, k] = cheby1(                             ...
                    5,                          ...
                    2,                          ...
                    2 * 5000 / sampleRate,      ...
                    'high'                      ...
                    );
                   
branchGain = 10^(-5 / 20);

branchIIRs{2, 1} = packBranchIIR(z, p, k, branchGain);

% 3
[z, p, k] = cheby1(                             ...
                    5,                          ...
                    0.1,                        ...
                    2 * 100 / sampleRate,       ...
                    'high'                      ...
                    );
                   
branchGain = 10^(-3 / 20);

branchIIRs{3, 1} = packBranchIIR(z, p, k, branchGain); 

% 4
[z, p, k] = cheby1(                             ...
                    5,                          ...
                    2,                          ...
                    2 * 2500 / sampleRate,      ...
                    'high'                      ...
                    );
                   
branchGain = 10^(-10 / 20);

branchIIRs{4, 1} = packBranchIIR(z, p, k, branchGain); 

% 5
[z, p, k] = cheby1(                             ...
                    5,                          ...
                    2,                          ...
                    2 * 1500 / sampleRate,      ...
                    'high'                      ...
                    );
                   
branchGain = 10^(-2 / 20);

branchIIRs{5, 1} = packBranchIIR(z, p, k, branchGain);

% 6
[z, p, k] = cheby1(                             ...
                    5,                          ...
                    2,                          ...
                    2 * 200 / sampleRate,       ...
                    'high'                      ...
                    );
                   
branchGain = 10^(-6 / 20);

branchIIRs{6, 1} = packBranchIIR(z, p, k, branchGain);

% 7
[z, p, k] = cheby1(                             ...
                    5,                          ...
                    2,                          ...
                    2 * 50 / sampleRate,        ...
                    'high'                      ...
                    );
                   
branchGain = 10^(-10 / 20);

branchIIRs{7, 1} = packBranchIIR(z, p, k, branchGain);

% 8
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 2000 / sampleRate,                                  ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-7 / 20);

branchIIRs{8, 1} = packBranchIIR(z, p, k, branchGain);

% 9
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 1000 / sampleRate,                                  ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-3 / 20);

branchIIRs{9, 1} = packBranchIIR(z, p, k, branchGain);

% 10
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 500 / sampleRate,                                   ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-11 / 20);

branchIIRs{10, 1} = packBranchIIR(z, p, k, branchGain);

% 11
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 20 / sampleRate,                                    ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-10 / 20);

branchIIRs{11, 1} = packBranchIIR(z, p, k, branchGain);

% 12
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 440 / sampleRate,                                   ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-13 / 20);

branchIIRs{12, 1} = packBranchIIR(z, p, k, branchGain);

% 13
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 7000 / sampleRate,                                  ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-12 / 20);

branchIIRs{13, 1} = packBranchIIR(z, p, k, branchGain);

% 14
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 20 / sampleRate,                                    ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-12 / 20);

branchIIRs{14, 1} = packBranchIIR(z, p, k, branchGain);

% 15
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 10 / sampleRate,                                    ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-8 / 20);

branchIIRs{15, 1} = packBranchIIR(z, p, k, branchGain);

% 16
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 40 / sampleRate,                                    ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-3 / 20);

branchIIRs{16, 1} = packBranchIIR(z, p, k, branchGain);

% 17
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 50 / sampleRate,                                    ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-9 / 20);

branchIIRs{17, 1} = packBranchIIR(z, p, k, branchGain);

% 18
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 70 / sampleRate,                                    ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-15 / 20);

branchIIRs{18, 1} = packBranchIIR(z, p, k, branchGain);

% 19
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 60 / sampleRate,                                    ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-16 / 20);

branchIIRs{19, 1} = packBranchIIR(z, p, k, branchGain);

% 20
[z, p, k] = cheby1(                                                         ...
                    3,                                                      ...
                    1,                                                      ...
                    2 * 50 / sampleRate,                                    ...
                    'high'                                                  ...
                    );
                   
branchGain = 10^(-2 / 20);

branchIIRs{20, 1} = packBranchIIR(z, p, k, branchGain);

%% Create Faust DSP
sosData         = cell(size(branchIIRs, 1), 1);
antialFIRs      = cell(size(branchIIRs, 1), 1);
H               = zeros(size(branchIIRs, 1), fPoints);

for b = 1:size(branchIIRs, 1)
    
    H(b, :)         = freqz(branchIIRs{b, 1}.b, branchIIRs{b, 1}.a, fPoints);
    
    sosItem         = struct();
    sosItem.SOS     = branchIIRs{b, 1}.S;
    sosItem.G       = branchIIRs{b, 1}.G;
    sosData{b, 1}   = sosItem;
    
    antialFIRs{b, 1} = fir1(                                        ...
                       antialFIRTaps - 1,                           ...
                       (2.0 * finalFrequency) / (b * sampleRate),   ...
                       'lowpass',                                   ...
                       windowFcn(antialFIRTaps)                     ...
                       );
    
endfor

save(workspaceTarget);
writeFaustDSP(faustDSPtarget, permission, precision, antialFIRs, sosData, 1.0, 0.0);

f = (0:(fPoints - 1)) * sampleRate / (2 * fPoints);
figure;
subplot(2, 1, 1);
semilogx(f, 20.0 * log10(abs(H)));
xlim([f(2) f(end)]);
ylim([-120 10 * ceil(max(20.0 * log10(abs(H(:)))) / 10)]);
grid on;
xlabel('Frequency [Hz]');
ylabel('Kernels IIR [rad]');
subplot(2, 1, 2);
semilogx(f, angle(H));
xlim([f(2) f(end)]);
grid on;
xlabel('Frequency [Hz]');
ylabel('Kernels IIR [rad]');