% TOGAC39_AMBF_CartesianChirpIdentifiedSystemStab.m

% The following file takes the identified transfer function from the AMBF
% for the galen robot. These are based on the chirp inputs from 0.1 Hz to 5
% Hz This will produce 2 figures. A stability map
% with a cost map for the upper impedance cases (including the environment)
% and lower cases without the environement. Both upper and lower include
% human impedance cases.

% Created by Brevin Banks
% Modified 4/24/2023

%%% Most optimal TF choices from TOGAC38_AMBF_CartesianSystemIdent.m
% Xest
% Yest
% Ztf
% Wxtf
% Wytf

%%%HOW TO MEASURE IMPEDANCE TORQUE?!!!

% for tf_num=1:5
% tf_num = 4;
for tf_num=0:4
s = tf('s');
switch (tf_num)
    case 0
        TF = 132.8/(s + 145.8);
    case 1
        TF = 131.8/(s + 132.4);
    case 2
        TF = 1.973e06/(s^2 + 1.558e04*s + 1.975e06);
    case 3
        TF = 2.542e05/(s^2 + 153.1*s + 2.561e05);
    case 4
        TF = 1.587e04/(s^2 + 230.7*s + 1.584e04);
end

% Create the space of ma and ba values to test the admittance controller on
ma_spaceup = linspace(0.1,200,30);
ba_spaceup = linspace(0.1,350,30);



% select from a list of end case impedance values outlined in the Aydin et
% al paper. Then analyze the stability of the TF from Aydin et al with the
% given ma space, ba space, and impedance
Zeq = Impedance_select(1);
[stable_map1, line_of_stability1up] = AnyTFStabMap(ma_spaceup,ba_spaceup,TF,Zeq)
Zeq = Impedance_select(2);
[stable_map2, line_of_stability2up] = AnyTFStabMap(ma_spaceup,ba_spaceup,TF,Zeq)
Zeq = Impedance_select(3);
[stable_map3, line_of_stability3up] = AnyTFStabMap(ma_spaceup,ba_spaceup,TF,Zeq)
Zeq = Impedance_select(4);
[stable_map4, line_of_stability4up] = AnyTFStabMap(ma_spaceup,ba_spaceup,TF,Zeq)

% extract the cost map for the Aydin controller using the Aydin et al
% method in section 5 Transparency.
CostTFup = CostTransMapAnyTF(ma_spaceup,ba_spaceup,TF);
% Stab mapper takes in Lines in the form of a cell array
Lines={line_of_stability1up,line_of_stability2up,line_of_stability3up,line_of_stability4up};
% Plot the stability plots and cost map in one figure
fextreme = StabMapperNoPoly(ma_spaceup,ba_spaceup,Lines,CostTFup);
title('Identified Wytf Stab Map W/ Cost. UpperBound')

numforstring = tf_num+1;
Filename = string("HighImpMapJoint"+string(numforstring));
datatosave = {fextreme,ma_spaceup};
save(Filename,"datatosave")

%%% Lower bound impedances

% Create the space of ma and ba values to test the admittance controller on
ma_spacelow = linspace(0.1,25,30);
ba_spacelow = linspace(0.1,10,30);

Zeq = Impedance_select(5);
[stable_map1, line_of_stability1low] = AnyTFStabMap(ma_spacelow,ba_spacelow,TF,Zeq)
Zeq = Impedance_select(6);
[stable_map2, line_of_stability2low] = AnyTFStabMap(ma_spacelow,ba_spacelow,TF,Zeq)
Zeq = Impedance_select(7);
[stable_map3, line_of_stability3low] = AnyTFStabMap(ma_spacelow,ba_spacelow,TF,Zeq)
Zeq = Impedance_select(8);
[stable_map4, line_of_stability4low] = AnyTFStabMap(ma_spacelow,ba_spacelow,TF,Zeq)

% extract the cost map for the Aydin controller using the Aydin et al
% method in section 5 Transparency.
CostTFlow = CostTransMapAnyTF(ma_spacelow,ba_spacelow,TF);
% Stab mapper takes in Lines in the form of a cell array
Lines={line_of_stability1low,line_of_stability2low,line_of_stability3low,line_of_stability4low};
% Plot the stability plots and cost map in one figure
fextreme = StabMapperNoPoly(ma_spacelow,ba_spacelow,Lines,CostTFlow);
% end
title('Identified Wytf Stab Map W/ Cost. LowerBound')
numforstring = tf_num+1;
Filename = string("LowImpMapJoint"+string(numforstring));
datatosave = {fextreme,ma_spacelow};
save(Filename,"datatosave")
end