% FilterBodeMagPlotConjPoles.m
%
% By: Neil E Cotter
% 11/13/2020
%
% By: Neil E Cotter
% 2024/12/26  updated to plot conjugate poles
%
% Script file to plot magnitude frequency response |H| in dB vs log omega.
%
% User must set input variables (or defaults below are used):
%
% k = overall gain factor
% pole_array = array of pole locations as complex numbers
% zero_array = array of zero locations as complex numbers
%  (set to [] if no zeros used)
% max_w = max angular frequency to use for plot
%
% Note: Do clc command between plots to avoid crashes when saving .eps

% Define frequency flag values.
Hz  = 0;
rad = 1;

% Define straight line or actual |H| plot flags.
straight_line = 0;
actual_line = 1;

% Set flag for whether to plot frequency in f (Hz) or omega (rad/s).
plot_freq_units_flag = rad;

% Set the frequency axis limits in units as per freq_units_flag.
min_freq = 1;
max_freq = 1e6;

% Set flag for whether to plot |H| as straight line approx or smooth curve.
bode_mode = actual_line;

%freq_units_flag = 1;  % for ω (rad/s).
% NOTE: units for pole and zero values are rad/s.  If using Hz, user must
%   multiply by 2π.


% Set real pole values in rad/s.
%real_pole_array = [100];
real_pole_array = [];
%real_pole_array = 2*pi*[100];
%real_pole_array = [1e3]

% Set real zero values.
%real_zero_array = [];
real_zero_array = [0]
%real_zero_array = 2*pi*[10e3]
%real_zero_array = [1e3]

% For conjugate poles/zeros specify resonant freq and damping factor of
%  (s/ω0)^2 + 2ζ(s/ω0) + 1:
%   Input:
%     omega0 = ω0 = resonant freq
%     zeta = ζ = dampling factor,resonant freq, omega0.

%conj_pole_omega0_array = [];
conj_pole_omega0_array = [1e3];
%Q = 3

%zeta = 1/(2*Q)   % if using Q values.
%conj_pole_zeta_array = [];
conj_pole_zeta_array = [0.25];   

%conj_zero_omega0_array = [1e3];
conj_zero_omega0_array = [];
%Q = 5;
% ζ = 1/(2*Q) if using Q values.
conj_zero_zeta_array = [];
%conj_zero_zeta_array = [1/(2*Q)]   

% Set overall gain factor.
%k = conj_pole_omega0_array.^2/100
k = 500
%k = 1;

%----------  Do not change code below here ----------%

% Verify that number of pole res freqs is same as number of zetas.
if length(conj_pole_omega0_array) ~= length(conj_pole_zeta_array)
  disp('Number of pole omega0 values ~= number of zeta values')
  return
end

% Verify that number of zero res freqs is same as number of zetas.
if length(conj_zero_omega0_array) ~= length(conj_zero_zeta_array)
  disp('Number of zero omega0 values ~= number of zeta values')
  return
end

% Convert conjugate poles res freq and damping to root values.
conj_pole_array = zeros(1,2*length(conj_pole_omega0_array));
for conj_index = 1:length(conj_pole_omega0_array)
  pole_imag_part = sqrt(1 - conj_pole_zeta_array(conj_index)^2);
  conj_pole_array(2*conj_index - 1) = ...
    conj_pole_omega0_array(conj_index) ...
    * (-conj_pole_zeta_array(conj_index) + pole_imag_part*j)
  conj_pole_array(2*conj_index) = ...
    conj_pole_omega0_array(conj_index) ...
    * (-conj_pole_zeta_array(conj_index) - pole_imag_part*j)
end

% Convert conjugate zeros res freq and damping to root values.
conj_zero_array = zeros(1,2*length(conj_zero_omega0_array));
for conj_index = 1:length(conj_zero_omega0_array)
  zero_imag_part = sqrt(1 - conj_zero_zeta_array(conj_index)^2);
  conj_zero_array(2*conj_index - 1) = ...
    conj_zero_omega0_array(conj_index) ...
    * (-conj_zero_zeta_array(conj_index) + zero_imag_part*j);
  conj_zero_array(2*conj_index) = ...
    conj_zero_omega0_array(conj_index) ...
    * (-conj_zero_zeta_array(conj_index) - zero_imag_part*j);
end

num_real_poles = length(real_pole_array);
num_real_zeros = length(real_zero_array);
num_conj_poles = length(conj_pole_array);
num_conj_zeros = length(conj_zero_array);

%log_freq_vec = log10(min_freq):log10(max_freq)/2703:log10(max_freq);
log_freq_vec = log10(min_freq):1/3.03e3:log10(max_freq);

%log_freq_vec = log10(min_freq):0.5:log10(max_freq);
if plot_freq_units_flag == Hz  % freq in Hz
  jw_vec = 1j*2*pi*power(10,log_freq_vec);
else                           % freq in rad/s
  jw_vec = 1j*power(10,log_freq_vec);
end

num_w = length(log_freq_vec);

gain_dB_vec = zeros(1,num_w);

% Compute gain magnitude.
for w_index = 1:length(jw_vec)
  jw = jw_vec(w_index);
  gain_dB_vec(w_index) = 20*log10(k);
  for zero_index = 1:num_real_zeros
    if bode_mode == actual_line
      gain_dB_vec(w_index) = gain_dB_vec(w_index) ...
        +20*log10(abs(jw - real_zero_array(zero_index)));
    else
      if abs(jw) > abs(real_zero_array(zero_index))
        gain_dB_vec(w_index) = gain_dB_vec(w_index) ...
          +20*log10(abs(jw));
      else
        gain_dB_vec(w_index) = gain_dB_vec(w_index) ...
          +20*log10(abs(real_zero_array(zero_index)));
      end
    end      
  end
  for pole_index = 1:num_real_poles
    if bode_mode == actual_line
      gain_dB_vec(w_index) = gain_dB_vec(w_index) ...
        -20*log10(abs(jw - real_pole_array(pole_index)));
    else
      if abs(jw) > abs(real_pole_array(pole_index))
        gain_dB_vec(w_index) = gain_dB_vec(w_index) ...
          -20*log10(abs(jw));
      else
        gain_dB_vec(w_index) = gain_dB_vec(w_index) ...
          -20*log10(abs(real_pole_array(pole_index)));
      end
    end
  end
  for zero_index = 1:num_conj_zeros
    gain_dB_vec(w_index) = gain_dB_vec(w_index) ...
      +20*log10(abs(jw - conj_zero_array(zero_index)));
  end
  for pole_index = 1:num_conj_poles
    gain_dB_vec(w_index) = gain_dB_vec(w_index) ...
      -20*log10(abs(jw - conj_pole_array(pole_index)));
  end
end

% Determine the smalles and highest gain values.
gain_dB_min = min(gain_dB_vec);
gain_dB_max = max(gain_dB_vec);

% Set plot box boundaries.
xmin = log10(min_freq);  % Freq axis starts just left of zero.
xmax = log10(max_freq);
ymin = 20*floor(gain_dB_min/20);
ymax = 20*ceil(gain_dB_max/20);

hold off  % Start a new plot.

% Close figure 1, if it exists.
if ishandle(1)
  close(1)
end

% Create a new figure 1.
%figure(1)
f = figure;
position_array = ...
  75*[1, 1, 1+min(9,(xmax-xmin)), 1+min(9,(ymax-ymin)/20)];
f.Position = position_array;

% Reset all figure properties, except Position, Units, PaperPosition,
%  and PaperUnits.
clf('reset')

hold on

% Set axes limits. % Put the values in an array.
axis_spec = [xmin, xmax, ymin, ymax];
axis(axis_spec);  % Set the axes limits.

% Set defaults of fonts and line sizes for axes and labels.
set(0,'DefaultAxesFontSize',18)
set(0,'DefaultLineLineWidth',2)
set(0,'DefaultTextFontSize',18)

% Draw freq and gain axes.
%loglog([xmin,xmax*0.88],[0,0],'k')
%loglog([xmin,xmin],[ymin,ymin+(ymax-ymin)*0.92],'k')
% Label the horizontal axis.
if plot_freq_units_flag == Hz  % freq in Hz
  text(xmax-0.8,ymin,'(log) $f$ (Hz)','FontUnits','points', ...
    'FontWeight','normal', ...
    'interpreter','latex','FontSize',18, ...
    'FontName','Times')  % Uses LateX string.
else                     % freq in rad/s
  text(xmax-0.8,ymin,'(log) $\omega$ (r/s)','FontUnits','points', ...
    'FontWeight','normal', ...
    'interpreter','latex','FontSize',18, ...
    'FontName','Times')  % Uses LateX string.
end
% Label vertical axis.
text(-0.07,ymax*1.02,'$|H|\; dB$','FontUnits','points', ...
  'FontWeight','normal', ...
  'interpreter','latex','FontSize',18, ...
  'FontName','Times')  % Uses LateX string.

%plot(w_vec,gain_vec,'b-')
loglog(log_freq_vec,gain_dB_vec,'b-')

% Put tick marks at powers of 10 for freq.
xticks_vec = ceil(xmin):floor(xmax);
xticks(xticks_vec)

xticklabels_vec = [];
for decade_index = 1:length(xticks)
  log_freq = xticks_vec(decade_index);

  if log_freq == 0
    cell_tick_str = {'1'};
  elseif log_freq == 1
    cell_tick_str = {'10'};
  elseif log_freq == 2
    cell_tick_str = {'100'};
  elseif log_freq == 3
    cell_tick_str = {'1k'};
  elseif log_freq == 4
    cell_tick_str = {'10k'};
  elseif log_freq == 5
    cell_tick_str = {'100k'};
  elseif log_freq == 6
    cell_tick_str = {'1M'};
  elseif log_freq == 7
    cell_tick_str = {'10M'};
  elseif log_freq == 8
    cell_tick_str = {'100M'};
  elseif log_freq == 9
    cell_tick_str = {'1G'};
  elseif log_freq == 10
    cell_tick_str = {'10G'};
  elseif log_freq == 11
    cell_tick_str = {'100G'};
  end

  xticklabels_vec = [xticklabels_vec, cell_tick_str];
end

xticklabels(xticklabels_vec)

grid on

hold off

