clc; clear all; close all;

%% Figure light theme

set(0, "defaultAxesColorOrder", [
        1.0000 0.0000 0.0000;  # red
        0.0000 0.0000 1.0000;  # blue
        1.0000 0.4980 0.0549;  # orange
        0.0000 0.7490 1.0000;  # 9. Sky Blue / Cyan (New)
        0.1725 0.6275 0.1725;  # green
        0.5451 0.7020 0.0000;  # 15. Olive / Acid Green (New - Warm, yellow-green with great contrast)
        0.0471 0.4000 0.1373;  # 13. Forest Green (New - Deep, dark green)
        0.4000 0.7608 0.5020;  # 14. Mint / Sage Green (New - Lighter, fresh green)
        0.6350 0.0780 0.1840;  # 12. Burgundy / Dark Red (New)
        0.0000 0.5882 0.5333;  # 10. Teal (New)
        0.8510 0.6471 0.1255;  # 11. Gold / Mustard (New - High-contrast alternative to yellow)
        0.5490 0.3373 0.2941;  # brown
        0.8902 0.4667 0.7608;  # pink
        0.5804 0.4039 0.7412;  # purple
        0.4980 0.4980 0.4980;  # gray
]);

set(0, "DefaultFigureColor", [1 1 1]);
set(0, "DefaultAxesColor", [1 1 1]);
set(0, "DefaultAxesXColor", [0 0 0]);
set(0, "DefaultAxesYColor", [0 0 0]);
set(0, "DefaultTextColor", [0 0 0]);

set(0, "DefaultAxesGridColor", [0 0 0]);
set(0, 'DefaultAxesGridAlpha', 1.00);

set(0, "DefaultAxesMinorGridColor", [0 0 0]);
set(0, 'DefaultAxesMinorGridAlpha', 0.20);

set(0, "DefaultLineLinewidth", 3.00);
set(0, "DefaultAxesFontSize", 16);
set(0, "DefaultTextFontSize", 16);
set(0, "DefaultAxesLineWidth", 1.00);

set(0, "DefaultAxesXGrid", "on");
set(0, "DefaultAxesYGrid", "on");
set(0, "DefaultAxesZGrid", "on");
set(0, "DefaultAxesXMinorGrid", "on");
set(0, "DefaultAxesYMinorGrid", "on");
set(0, "DefaultAxesXMinorTick", "on");
set(0, "DefaultAxesYMinorTick", "on");

set(0, 'DefaultAxesGridAlpha', 0.50);
set(0, "defaultAxesFontName", "Nimbus Sans");

% exported osc data
filename = "bode.csv";

fid = fopen(filename, "r");

% Skip header
fgetl(fid);

% Read frequency and complex string
data = csvread(filename, 1, 0);

freq    = data(:, 1);
mag_osc = data(:, 2);

mag_osc = squeeze(mag_osc);
mag_osc = 20*log10(mag_osc);
mag_osc = movmean(mag_osc, 3);

% for k = 1:N
%     tokens = regexp(response{k}, ...
%         '\(([-+0-9.eE]+)dB,([-+0-9.eE]+)°\)', ...
%         'tokens');

%     mag_model(k)   = str2double(tokens{1}{1});
% end

figure("visible", "off");
grid on;
set(gcf, "paperunits", "inches");
set(gcf, "papersize", [10 4]);
set(gcf, "paperposition", [0 0 10 4]);
% theorical transfer function

H1 = tf([1 250], [1 500])
H2 = tf([4000^2], [1 4000^2/9000 4000^2])
H = 12*H1*H2
[mag_the, pha_the, freq] = bode(H, freq);

% bode functions returns linear scale y-axis
mag_the = squeeze(mag_the);
pha_the = squeeze(pha_the);
mag_the = 20*log10(mag_the);


% theorical normalized transfer function

H1_norm = tf([1 252.52], [1 505.05])
H2_norm = tf([4014.80^2], [1 4014.80/2.264 4014.80^2])
H_norm = 12*H1_norm*H2_norm
[mag_the_norm, pha_the_norm, freq] = bode(H_norm, freq);

% bode functions returns linear scale y-axis
mag_the_norm = squeeze(mag_the_norm);
pha_the_norm = squeeze(pha_the_norm);
mag_the_norm = 20*log10(mag_the_norm);

%% Magnitude
% ax1 = figure()
hold on;
y1 = semilogx(freq, mag_the, "LineWidth", 3.75);
y2 = semilogx(freq, mag_the_norm, "LineWidth", 3);
y3 = semilogx(freq*2*pi, mag_osc, "LineWidth", 2.25);
ylim([-35 35])
yticks([-20 0 20]);
xlim([1 1e5])
xlabel("FRECUENCIA [rad/s]");
ylabel("MAGNITUD [dB]");
% title("Bode Magnitude");
grid on;

% Remove x tick labels from upper plot
% set(ax1, "XTickLabel", []);

% %% Phase
% ax2 = subplot(2, 1, 2)
% hold on;
% semilogx(freq, pha_the, "LineWidth", 3.75);
% semilogx(freq, pha_the_norm, "LineWidth", 3);
% semilogx(freq*2*pi, phase, "LineWidth", 2.25);
% semilogx(freq*2*pi, phase_model, "LineWidth", 1.5);
% 
% grid on;
% ylim([-225 45])
% yticks([-180 -90 0]);
% xlim([1 1e5])
% xlabel("FRECUENCIA  [rad/s]");
% ylabel("FASE  [grados]");
% title("Bode Phase");
% legend("Normalizado", "Teórica", "Location", "northeast");

% Invisible axes covering the whole figure
axL = axes("Position", [0 0 1 1], ...
           "Visible", "off", ...
           "Units", "normalized");

% Create legend on top axes
lgd = legend([y1 y2 y3], ...
                {"H", "H normalizada", "Placa"}, ...
                "Location", "southwest");

% Move legend
set(lgd, "position", [0.16 0.23 0.10 0.05]);
% set(lgd, "numcolumns", 1);

% Reduce vertical spacing
% set(ax1, "Position", [0.13 0.58 0.80 0.38]);

print("bode.png", "-dpng", "-r500");

freq_osc_rad = freq*2*pi;
mag_osc_interp = interp1(freq_osc_rad, mag_osc, freq);

data = [freq(:), mag_the(:), mag_osc_interp(:)];

fid = fopen("bode_data_exported.csv", "w");
fprintf(fid, "freq_rad_s,mag_the,mag_the_norm,mag_osc\n");
fclose(fid);

dlmwrite("bode_data_exported.csv", data, "-append", ...
    "delimiter", ",", "precision", "%2.1f");