%%
run sumcon2mdlconst

%% Set Act Gain
%% 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','F1','F0','IP'};
DOFs = {{'L','P','Y'},{'L','T','V','R','P','Y'},{'GAS'},{'GAS'},{'GAS'},{'L','T','Y'}};
Sensors = {{'OpLev'},{'OSEM'},{'LVDT'},{'LVDT'},{'LVDT'},{'LVDT','GAS'}};

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

%% MODEL IMPORT
%optic = input('which optic? ');
optic = 'BS';
load(strcat(optic,'mdl.mat'));  % Import ss model
DefaultParameters;    % Import default servo filters
%load(strcat(optic,'mdl_0params.mat'));
mdlfile='typeBsimctrl';   % typeB ver.180313
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);
    sensor = sensormap(stage);
    if length(sensor) >=2
        sensor = sensor(1);
    end
    mon_port = strcat(sensor,'_',actDOFmap(stage),stage);
    
    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(:,20));
        currentGain = eval(char(valname(k)));
        NewGain = currentGain/DCgain;
        assignin('base',char(valname(k)),NewGain);
        
    end
    
end

save(strcat(optic,'mdl_0params.mat'));

%% Check TFs

clear all;                     % Clear workspace
close all;                     % Close plot windows
addpath('D:\OneDrive\Documents\phys\src\DttData');
addpath('../../utility');      % Add path to utilities
addpath('servofilter');        % Add path to servo
addpath('measurement')
g = 9.81;                      % Gravity constant
freq=logspace(-2,2.5,1001);

%% Load parameter
%optic = input('which optic? ');
optic = 'BS';
load(strcat(optic,'mdl.mat'));
load(strcat(optic,'mdl_0params.mat'));
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);

%%
for i = 1:numel(Stages)
    stage = char(Stages(i));
    act_port = strcat('act',actDOFmap(stage),stage);
    sensor = sensormap(stage);
    if length(sensor) >=2
        sensor = sensor(1);
    end
    mon_port = strcat(sensor,'_',actDOFmap(stage),stage);
    
    %for k = 1:numel(act_port)
    %    bodesusplotopt(sysc0,act_port(k),mon_port(k),freq);
    %    
    %end
    
end

%% Set Real Gain
%% Add path
clear all;                     % Clear workspace
close all;                     % Close plot windows
addpath('D:\OneDrive\Documents\phys\src\DttData');
addpath('../../utility');      % Add path to utilities
addpath('servofilter');        % Add path to servo
addpath('measurement')
g = 9.81;                      % Gravity constant
freq=logspace(-2,2.5,1001);

%% Load parameter

%optic = input('which optic? ');
optic = 'BS';
load(strcat(optic,'mdl.mat'));
load(strcat(optic,'mdl_0params.mat'));
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);

%% Mapping stages and DOFs in simulink

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

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

%% GAS
for i=3:5
    stage = char(Stages(i));
    actDOF = 'GAS';
    act_port = strcat('act',actDOF,stage);
    mon_port = strcat(sensormap(stage),'_',actDOF,stage);
    filename = strcat('./measurement/',optic,'_TF_',stage,actDOF,'.xml');

    data = DttData(char(filename));
    chA = strcat('K1:VIS-',optic,'_',stage,'_TEST_',actDOF,'_EXC');
    chB = strcat('K1:VIS-',optic,'_',stage,'_DAMP_',actDOF,'_IN1');
    [fr,tf] = transferFunction(data, char(chA), char(chB));
    [fr,coh] = coherence(data, char(chA), char(chB));
    
    [mag,phs]=bodesus(sysc0,act_port,mon_port,fr);

    A = [];
    for i = 1:length(fr)
        if coh(i) > 0.95 && fr(i)<0.2
           A = [A, mag(i)/abs(tf(i))]; 
        end
    end
    Aav = mean(A);
    Asig = cov(A);


    titlearg=['Transfer Function',' from ',char(act_port),' to ',char(mon_port)];
    fig=figure;
    subplot(5,1,[1 2 3])
    loglog(fr,mag./Aav,'LineWidth',2)
    ylim([1E-6 1E+2]);
    grid on
    hold on
    loglog(fr,abs(tf),'r.', 'LineWidth',2)
    title(titlearg,'FontSize',12,'FontWeight','bold','FontName','Times New Roman',...
        'interpreter','none')
    ylabel('Magnitude[um or urad/cnt]','FontSize',12,'FontWeight','bold','FontName','Times New Roman')
    set(gca,'FontSize',12,'FontName','Times New Roman')
    legend('simulation','Measured','Location','southwest')

    subplot(5,1,[4 5])
    semilogx(fr,phs,'LineWidth',2)
    grid on
    ylim([-180 180])
    hold on
    semilogx(fr,180/pi*angle(tf),'r.', 'LineWidth',2)
    ylabel('Phase [deg]','FontSize',12,'FontWeight','bold','FontName','Times New Roman')
    xlabel('Frequency [Hz]','FontSize',12,'FontWeight','bold','FontName','Times New Roman')
    set(gca,'FontSize',12,'FontName','Times New Roman')
    set(gca,'YTick',-180:90:180)
    set(fig,'Color','white')

    %figfile = strcat('./fig/',optic,'_TF_',stage,actDOF,'.png');
    %saveas(fig,char(figfile));


    valname = strcat('gain_act_',actDOF,stage);
    B = eval(char(valname)); %[N/m]
    Eff = B/Aav/1E+6; %[N/cnt]
    Effsig = B/1E+6*(Asig^2/Aav^2);

    assignin('base',char(valname),eval(char(valname))/Aav);

    X = ['Actuation efficiency of ', char(act_port),': ', num2str(Eff),'(+/- ',num2str(Effsig),') [N/cnt]'];
    disp(X)   
    
end