% ----------------------------------------------------------------------
% Type-Bp for KAGRA
% Coded by A Shoda on 2017/10/27
% ----------------------------------------------------------------------
% Get the actuation efficiency [cnt/m] & [N/cnt]
% Note: gain_act_** is [N/m] obtained from the simulation
% ----------------------------------------------------------------------
%% 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? ');
load(strcat(optic,'susmdl.mat'));
load(strcat(optic,'susmdl_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);

%% get cnt/m from the measured data

%% For TM

stage = 'TM';
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
filename = strcat('./measurement/PR3/TF_',stage,actDOFmap(stage),'.xml');

k=1;
data = DttData(char(filename(k)));
chA = strcat('K1:VIS-',optic,'_',stage,'_TEST_L_EXC');
chB = strcat('K1:VIS-',optic,'_',stage,'_OPLEV_LEN_YAW_OUT');
[fr,tf] = transferFunction(data, char(chA), char(chB));
[fr,coh] = coherence(data, char(chA), char(chB));

[mag,phs]=bodesus(sysc0,act_port(k),mon_port(k),fr);

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


titlearg=['Transfer Function',' from ',char(act_port(k)),' to ',char(mon_port(k))];
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')

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

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

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

%%
k=2;
data = DttData(char(filename(k)));
chA = strcat('K1:VIS-',optic,'_',stage,'_TEST_P_EXC');
chB = strcat('K1:VIS-',optic,'_',stage,'_OPLEV_TILT_PIT_OUT');
[fr,tf] = transferFunction(data, char(chA), char(chB));
[fr,coh] = coherence(data, char(chA), char(chB));

[mag,phs]=bodesus(sysc0,act_port(k),mon_port(k),fr);

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


titlearg=['Transfer Function',' from ',char(act_port(k)),' to ',char(mon_port(k))];
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')

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

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

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


%%
k=3;
data = DttData(char(filename(k)));
chA = strcat('K1:VIS-',optic,'_',stage,'_TEST_Y_EXC');
chB = strcat('K1:VIS-',optic,'_',stage,'_OPLEV_TILT_YAW_OUT');
[fr,tf] = transferFunction(data, char(chA), char(chB));
[fr,coh] = coherence(data, char(chA), char(chB));

[mag,phs]=bodesus(sysc0,act_port(k),mon_port(k),fr);

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


titlearg=['Transfer Function',' from ',char(act_port(k)),' to ',char(mon_port(k))];
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')

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

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

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

%% For IM and above

for i = 2: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
    filename = strcat('./measurement/PR3/TF_',stage,actDOFmap(stage),'.xml');
    
    for k = 1:numel(act_port)
        data = DttData(char(filename(k)));
        chA = strcat('K1:VIS-',optic,'_',stage,'_TEST_',actDOFmap(stage),'_EXC');
        chB = strcat('K1:VIS-',optic,'_',stage,'_DAMP_',actDOFmap(stage),'_IN1');
        [fr,tf] = transferFunction(data, char(chA(k)), char(chB(k)));
        [fr,coh] = coherence(data, char(chA(k)), char(chB(k)));

        [mag,phs]=bodesus(sysc0,act_port(k),mon_port(k),fr);

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

        titlearg=['Transfer Function',' from ',char(act_port(k)),' to ',char(mon_port(k))];
        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')
        
        valname = strcat('gain_act_',actDOFmap(stage),stage);
        B = eval(char(valname(k))); %[N/m]
        Eff = B/Aav/1E+6; %[N/cnt]
        Effsig = B/1E+6*(Asig^2/Aav^2);
        
        assignin('base',char(valname(k)),eval(char(valname(k)))/Aav);
        
        X = ['Actuation efficiency of ', char(act_port(k)),': ', num2str(Eff),'(+/- ',num2str(Effsig),') [N/cnt]'];
        disp(X)
    end
end


%%
save('PR3susmdl_realparams.mat')