% This Function Creates a parasitic cost map the feedback only style
% controller from TOGAC27_CD_TransferFunctionDerivAndStability.pdf 
% the cost map is created as found in the Towards Collaborative drilling paper
% 2020 by Aydin et al in the transparency section 5.

% Function Parameters:
%
% Inputs:
%   ma_space - the linear or log space of m_ad values shaped as a vector
%   ba_space - the linear or log space of b_ad values shaped as a vector
%
% Outputs:
%   C_map - a nxm matrix for the given m_ad and b_ad range on the transfer
%   functions for the system kind
% Created by Brevin Banks
% Modified 3/14/2023

function C_map = CostTransMapFeedback(ma_space,ba_space)

i = 0;
j = 0;
warning('off');
fs = 200; % samples in 10 seconds for sampling rate
fc = 5; % cut off frequency for butterworth filter
[b,a] = butter(5,fc/(fs/2)); %butterworth filter coefficients
cost_filt = tf(b,a); %transfer function for the low pass butterworth filter

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)
    m_ad = ma_space(ma_iter);
    i = i+1;
    for ba_iter = 1:length(ba_space)
         b_ad = ba_space(ba_iter);
        j = j+1;
        As = tf([1],[m_ad b_ad]);
        TF = TFFeedback(As);
        [Num,Den]=tfdata(TF);
        dZs = tf(Den,Num);
        
        space = logspace(-2,2); %range to test human impedance interaction. 10^-2 to 10^2
        space = space(1:45); %feasible human range is really  0.01 to ~30 Hz
        C = 0;
        for k = 1:length(space)
            [dZsmag,psZ] = mag_phase(dZs,space(k)); %get the magnitude of dZs
            [Weightfun,psW] = mag_phase(cost_filt,space(k)); %get the magnitude of the butterworth filter
            C = C + Weightfun*log(dZsmag); % Calculate the cost to go
        end
        if C<0
            C = 0;
        end
        C_map(i,j) = C; % build the map value i,j for the calculated cost
        
        x = x+1; % add to the progress bar
        madstr = string(m_ad);
        badstr = string(b_ad);
        bar_msg = "Calculating Parasitic Impedance Sum Map: m_{ad}="+madstr+" b_{ad} ="+badstr; 
        waitbar(x/steps,f.waitbar,bar_msg); % update progress bar
    end
    j = 0;
end
warning('on');


waitbar(1,f.waitbar,"Completed. Please close this window"); % update progress bar
try

delete(f.waitbar)
catch
    return;
end

end