%%
% Calculate actuation efficiency using IM to TM transfer functions

%% 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 data
load('BSmdl_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 IM pitch

data = DttData('C:\Users\ayaka\Dropbox (�l)\Shoda\BS\180510\TF_IMP-TMP.xml');
chA = strcat('K1:VIS-BS_IM_TEST_P_EXC');
chB = strcat('K1:VIS-BS_TM_OPLEV_PIT_DIAG_DQ');
[fr,tf] = transferFunction(data, chA, chB);
[fr,coh] = coherence(data, chA, chB);

[mag,phs]=bodesus(sysc0,'actPIM','OpLev_PTM',fr);

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


titlearg='Transfer Function from PIM to PTM';
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/BS_TF_IMP2TMP.png');
saveas(fig,char(figfile(k)));

B = gain_act_PIM; %[N/m]
Eff = B/Aav/1E+6; %[N/cnt]
Effsig = B/1E+6*(Asig^2/Aav^2);

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

%% IM YAW

data = DttData('C:\Users\ayaka\Dropbox (�l)\Shoda\BS\180510\TF_IMY-TMY.xml');
chA = strcat('K1:VIS-BS_IM_TEST_Y_EXC');
chB = strcat('K1:VIS-BS_TM_OPLEV_YAW_DIAG_DQ');
[fr,tf] = transferFunction(data, chA, chB);
[fr,coh] = coherence(data, chA, chB);

[mag,phs]=bodesus(sysc0,'actYIM','OpLev_YTM',fr);

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


titlearg='Transfer Function from YIM to YTM';
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/BS_TF_IMY2TMY.png');
saveas(fig,char(figfile(k)));

B = gain_act_YIM; %[N/m]
Eff = B/Aav/1E+6; %[N/cnt]
Effsig = B/1E+6*(Asig^2/Aav^2);

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

%% For IM pitch OSEM

data = DttData('C:\Users\ayaka\Dropbox (�l)\Shoda\BS\180510\TF_IMP-TMP.xml');
chA = strcat('K1:VIS-BS_IM_TEST_P_EXC');
chB = strcat('K1:VIS-BS_IM_DAMP_P_IN1_DQ');
[fr,tf] = transferFunction(data, chA, chB);
[fr,coh] = coherence(data, chA, chB);

[mag,phs]=bodesus(sysc0,'actPIM','OSEM_PIM',fr);

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


titlearg='Transfer Function from PIM to PIM';
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/BS_TF_IMP2IMP.png');
saveas(fig,char(figfile(k)));

B = gain_act_PIM; %[N/m]
Eff = B/Aav/1E+6; %[N/cnt]
Effsig = B/1E+6*(Asig^2/Aav^2);

X = ['Actuation efficiency of actPIM(OSEM): ', num2str(Eff),'(+/- ',num2str(Effsig),') [N/cnt]'];
disp(X) 
disp(Aav)   

%% IM YAW

data = DttData('C:\Users\ayaka\Dropbox (�l)\Shoda\BS\180510\TF_IMY-TMY.xml');
chA = strcat('K1:VIS-BS_IM_TEST_Y_EXC');
chB = strcat('K1:VIS-BS_IM_DAMP_Y_IN1_DQ');
[fr,tf] = transferFunction(data, chA, chB);
[fr,coh] = coherence(data, chA, chB);

[mag,phs]=bodesus(sysc0,'actYIM','OSEM_YIM',fr);

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


titlearg='Transfer Function from YIM to YIM';
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/BS_TF_IMY2IMY.png');
saveas(fig,char(figfile(k)));

B = gain_act_YIM; %[N/m]
Eff = B/Aav/1E+6; %[N/cnt]
Effsig = B/1E+6*(Asig^2/Aav^2);

X = ['Actuation efficiency of act_YIM(OSEM): ', num2str(Eff),'(+/- ',num2str(Effsig),') [N/cnt]'];
disp(X)   
disp(Aav)