% TOGAC47_RGC_RealGalenSysStability.m

% This file analyzes the stablity and transparency of the real identified
% galen system in the mock OR. Currently the Rotation axis are not yet
% identified.


% Modified 05-4-2023
% Created by Brevin Banks

%%% TF choices from RealIdentification.m
% Xest
% Yest
% Ztf


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




for tf_num=0:2
s = tf('s');
switch (tf_num)
    case 0
        % Has a .115s estimated time delay
        TF = 9.883e04/(s^2 + 12.47*s + 1.034e05);
        Tfchoice = "Xtf";
    case 1
        % Has a 0.0599 s estimated time delay
        TF = 1.607e07/(s^2 + 1.971e05*s + 1.655e07);
        Tfchoice = "Ytf";
    case 2
        % Has a 0.015 s estimated time delay
        TF = 2.075e04/(s^2 + 749.4*s + 2.098e04);
        Tfchoice = "Ztf";
    case 3
        % TF = 
        Tfchoice = "Wxtf";
    case 4
        % TF = 
        Tfchoice = "Wytf";
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,700,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 "+Tfchoice+" Stab Map W/ Cost. UpperBound")

numforstring = tf_num+1;
Filename = string("RealHighImpMapJoint"+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,50,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 "+Tfchoice+" Stab Map W/ Cost. LowerBound")
numforstring = tf_num+1;
Filename = string("RealLowImpMapJoint"+string(numforstring));
datatosave = {fextreme,ma_spacelow};
save(Filename,"datatosave")
end