% This function will create a stability map for any lines of stability and
% ma space or ba space. The result is a figure with the given lines of
% stability plotted. If a cost map is provided, it will underlay the map to
% display in color where the stable region is found. If exactly 4 lines of
% stability are provide, the default legend highlighting relevant impedance
% cases from Aydin et al will identified like figure 6 in the Towards
% collaborative drilling paper. The output lines are fit using a 6th order
% polynomial, therefore the exact bounds near the lines are not perfectly
% accurate, and strange curving behavior may be observed. This analysis is
% mostly qualitative. In reality, straight edges where stability may start or stop
% may be present where oscilating curves are shown in these figures due to
% the over fitting.
% Input:
%   ma_space - the values of admittance masses used in finding the lines of
%   stability
%   ba_space - the values of admittance damping values used in finding the lines of
%   stability
%   Stability_lines_cell_array - a cell array of lines of stability for
%   given stability tests. Just an array of numbers that increase with ma
%   and ba
%   Cost_map - a length(ma)xlength(ba) 2D arrray from CostTrans"".m
%   functions

% Created by Brevin Banks 
% Modified 3/16/2023
function StabMapper(ma_space,ba_space,Stability_lines_cell_array,Cost_Map)

% Create a new figure
figure
hold on
% if no cost map is provided don't plot one
if nargin<4
    Cost_Map = 0;
else % grab the cost map and plot it
    M=max(max(Cost_Map));
    imagesc([ba_space(1) ba_space(end)],[ma_space(1) ma_space(end)],Cost_Map) % scale the axis appropriately
    caxis([0 M]) % Identify color bar weight scale
    set(gca,'YDir','normal') % flip the Y axis for the upside down mass values
    colorbar % Add a color bar for reference
end
% initialize the fit functions for the lines of stability
f = zeros(length(Stability_lines_cell_array),length(ma_space));

% fit a 6th order polynomial for each line of stability
for k = 1:length(Stability_lines_cell_array)
    Lineofstab = Stability_lines_cell_array{k};
    p = polyfit(Lineofstab(1,:),Lineofstab(2,:),6);
    f(k,:) = polyval(p,ma_space);
end

% check which polynomial fits bound the further stability case
for mxlook = 1:length(ma_space)
    fpotentials = [];
    for k =1:length(Stability_lines_cell_array)
        fpotentials = [fpotentials f(k,mxlook)];
    end
    fextreme(mxlook) = max(fpotentials);
end

% Place white over the unstable area of the cost map
ypatch = [ma_space ma_space(end) 0];
xpatch = [fextreme 0 0];
patch(xpatch,ypatch,[1 1 1],'HandleVisibility','off')

% plot the lines of stability
for plot_of_four=1:length(Stability_lines_cell_array)
    switch plot_of_four
        case 1
            plot(f(plot_of_four,:),ma_space,'Color',[0.4940 0.1840 0.5560],'Linestyle',':','Linewidth',2)
        case 2
            plot(f(plot_of_four,:),ma_space,'Linestyle','--','Linewidth',2)
        case 3
            plot(f(plot_of_four,:),ma_space,'Linestyle','-.','Linewidth',2)
        case 4
            plot(f(plot_of_four,:),ma_space,'Color',[0 0.4470 0.7410],'Linewidth',2)
            
        otherwise
            plot(f(plot_of_four,:),ma_space)
    end
end

% add info data to the figure
xlim([0 ba_space(end)])
ylim([0 ma_space(end)])
xlabel('b_{a} [Ns/m]')
ylabel('m_{a} [kg]')

% if four lines of stability are given, assume the legend
if length(Stability_lines_cell_array)==4
    legend('m_{h} = 5 kg, b_{h} = 41 Ns/m','m_{h} = 5 kg, b_{h} = 0 Ns/m','m_{h} = 0 kg, b_{h} = 41 Ns/m','m_{h} = 0 kg, b_{h} = 0 Ns/m','location','northwest')
else
    legend
end

end