% ----------------------------------------------------------------------
% Type-Bp for KAGRA
% Coded by A Shoda on 2017/10/27
% ----------------------------------------------------------------------
% This code is for adjusting the gain_act numbers
% ----------------------------------------------------------------------

%% PRELIMINARY
clear all;                     % Clear workspace
close all;                     % Close plot windows
addpath('../../../../utility');      % Add path to utilities
addpath('servofilter');        % Add path to servo
g = 9.81;                      % Gravity constant
freq=logspace(-2,2.5,1001);


%% Mapping stages and DOFs in simulink

Stages = {'TM','IM','BF','SF'};
DOFs = {{'L','P','Y'},{'L','T','V','R','P','Y'},{'L','T','V','R','P','Y','GAS'},{'GAS'}};
Sensors = {'OpLev','OSEM','LVDT','LVDT'};

actDOFmap = containers.Map(Stages,DOFs);
sensormap = containers.Map(Stages,Sensors);

%% MODEL IMPORT
optic = input('which optic? ');
load(strcat(optic,'susmdl.mat'));  % Import ss model
DefaultParameters;    % Import default servo filters
mdlfile='typeBpsimctrl_171030';   % typeBp ver.170704
st     =linmod(mdlfile);          % Linearize simulink model
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc0  =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% test

for i = 1:numel(Stages)
    stage = char(Stages(i));
    act_port = strcat('act',actDOFmap(stage),stage);
    mon_port = strcat(sensormap(stage),'_',actDOFmap(stage),stage);
    if length(act_port) ~= length(mon_port)
        print('ERROR: SIZE OF THE DICTIONARY MISMATCH')
    end
    
    for k = 1:numel(act_port)
        valname = strcat('gain_act_',actDOFmap(stage),stage);
        [mag,~]=bodesus(sysc0,act_port(k),mon_port(k),freq);
        DCgain = mean(mag(:,30));
        currentGain = eval(char(valname(k)));
        NewGain = currentGain/DCgain;
        assignin('base',char(valname(k)),NewGain);
        
    end
    
end

st     =linmod(mdlfile);          % Linearize simulink model
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc0  =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);
save('PR3susmdl_0params.mat')

%% Check TFs

for i = 1:numel(Stages)
    stage = char(Stages(i));
    act_port = strcat('act',actDOFmap(stage),stage);
    mon_port = strcat(sensormap(stage),'_',actDOFmap(stage),stage);
    if length(act_port) ~= length(mon_port)
        print('ERROR: SIZE OF THE DICTIONARY MISMATCH')
    end
    
    for k = 1:numel(act_port)
        bodesusplotopt(sysc0,act_port(k),mon_port(k),freq);
        
    end
    
end