% ----------------------------------------------------------------------
% 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_params.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
% i=1;
% 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,'_OLDAMP_',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,'LineWidth',2)
%     ylim([1E-6 1E+2]);
%     grid on
%     hold on
%     loglog(fr,Aav*abs(tf),'r.', 'LineWidth',2)
%     title(titlearg,'FontSize',12,'FontWeight','bold','FontName','Times New Roman',...
%         'interpreter','none')
%     ylabel('Magnitude','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);
% 
%     X = ['Actuation efficiency of ', char(act_port(k)),': ', num2str(Eff),'(+/- ',num2str(Effsig),') [N/cnt]'];
%     disp(X)
% end

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,'LineWidth',2)
ylim([1E-6 1E+2]);
grid on
hold on
loglog(fr,Aav*abs(tf),'r.', 'LineWidth',2)
title(titlearg,'FontSize',12,'FontWeight','bold','FontName','Times New Roman',...
    'interpreter','none')
ylabel('Magnitude','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);

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,'LineWidth',2)
ylim([1E-6 1E+2]);
grid on
hold on
loglog(fr,Aav*abs(tf),'r.', 'LineWidth',2)
title(titlearg,'FontSize',12,'FontWeight','bold','FontName','Times New Roman',...
    'interpreter','none')
ylabel('Magnitude','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);

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,'LineWidth',2)
ylim([1E-6 1E+2]);
grid on
hold on
loglog(fr,Aav*abs(tf),'r.', 'LineWidth',2)
title(titlearg,'FontSize',12,'FontWeight','bold','FontName','Times New Roman',...
    'interpreter','none')
ylabel('Magnitude','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);

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,'LineWidth',2)
        ylim([1E-6 1E+2]);
        grid on
        hold on
        loglog(fr,Aav*abs(tf),'r.', 'LineWidth',2)
        title(titlearg,'FontSize',12,'FontWeight','bold','FontName','Times New Roman',...
            'interpreter','none')
        ylabel('Magnitude','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);
        
        X = ['Actuation efficiency of ', char(act_port(k)),': ', num2str(Eff),'(+/- ',num2str(Effsig),') [N/cnt]'];
        disp(X)
    end
end