% This function will analyze the stability of the controller found in Aydin
% et al. The towards collaborative drilling paper 2020.
% Given the transfer functions derived in the paper and used in TFAydin the
% stability is analyzed and found for given impedance bounds.
% Input:
%   ma_space - the values of admittance masses to test
%   ba_space - the values of admittance damping to test
%   Zeq - the transfer function for the desired applied impedance
% Output:
%   stable_map - an length(ma_space)xlength(ba_space) map of 0s and 1s
%   representing stable and unstable cases (0 is unstable)
%   line_of_stability - A line that shows where the 0s change to 1s

% Created by Brevin Banks 
% Modified 3/16/2023
function [stable_map, line_of_stability] = AydinStabMap(ma_space,ba_space,Zeq)
stable_map = zeros(length(ma_space),length(ba_space));
line_of_stability = [0;0];

steps = length(ma_space)*length(ba_space); % Calculate necessary iterations to complet the task for the progress bar
x = 0; % initialize progress bar start point
f.waitbar = waitbar(0,'Loading..'); % create progress bar
for ma_iter = 1:length(ma_space)
    line_of_stability(1,ma_iter) = ma_space(ma_iter);
    for ba_iter = 1:length(ba_space)
        m_ad = ma_space(ma_iter);
        b_ad = ba_space(ba_iter);
        As = tf([1],[m_ad b_ad]);
        stable_result = Check_stable(minreal(TFAydin(As,Zeq,'closed')),'pole');
        if stable_result == 0
            line_of_stability(2,ma_iter) = b_ad;
        end
        stable_map(ma_iter,ba_iter) = stable_result;
        x = x+1; % add to the progress bar
        madstr = string(m_ad);
        badstr = string(b_ad);
        bar_msg = "Calculating Stability Map: m_{ad}="+madstr+" b_{ad} ="+badstr; 
        waitbar(x/steps,f.waitbar,bar_msg); % update progress bar
    end
end
waitbar(1,f.waitbar,"Completed. Please close this window"); % update progress bar
try

delete(f.waitbar)
catch
    return;
end
end