%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% Type-A for KAGRA
% Coded by K. Okutomi on 2017.03.03
% ----------------------------------------------------------------------

%% PRELIMINARY
clear all;  % Clear workspace
close all;  % Close plot windows
%% PREPARATION
cd('/Users/kokioktm/Documents/suspension/MATLAB/script/TypeA'); % Change directory
addpath('../../utility');  % Add path to utilities
addpath('../../oktlib');
addpath('controlmodel');
addpath('susmodel');
g = 9.81;
pp = 2.0*pi;

%% IMPORT SUSPENSION MODEL
matfile='TypeA161114mdl';
load([matfile,'.mat']);

%% RESET SERVO FILTERS
addpath('parameter');         % Add path to servo
TypeA_paramNoCtrl170105;      % NO CONTROL MODE
rmpath ('parameter');         % Remove path to servo

%% IMPORT SIMULINK MODEL
mdlfile='TypeA_ctrl161209';
st     =linmod(mdlfile);
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc0  =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% FREQUENCY
freq =logspace(-2,2,1001);
freq2=logspace(-3,1,1001);
freq3=logspace(-2,1,1001);

%% NO CONTROL ANALYSIS
% =======================================================================
%% IMPORT NOISE
rawSeismic = importdata('../../noise/KamiokaSeismicHighNoise.dat');
dispSeismic = interp1(rawSeismic(:,1), rawSeismic(:,2), freq);
accSeismic = pp*pp.*freq.*freq.*dispSeismic;
% plotpsd({dispSeismic; accSeismic}, freq)

%% SYSTEM POLES
polySysc0 = poly(st.a);
poleSysc0 = roots(polySysc0)/pp;

figure(10)
plot(poleSysc0, 'bx', 'MarkerSize', 16, 'LineWidth', 2)
xlabel('Real')
ylabel('Imag')
%% SUSPENSION RESPONSE IN DIAGONAL I/Os
%  =====================================================================
%% Longitudinal
sustf_LF0 = calcsystf(sysc0, 'injLF0', 'LVDT_LF0', freq);
sustf_accLF0 = calcsystf(sysc0, 'injLF0', 'ACC_LF0', freq);
sustf_LBF = calcsystf(sysc0, 'injLBF', 'LVDT_LBF', freq);
sustf_LMT = calcsystf(sysc0, 'injLMT', 'PS_LMT', freq);
sustf_LIM = calcsystf(sysc0, 'injLIM', 'PS_LIM', freq);
sustf_LTM = calcsystf(sysc0, 'injLTM', 'PS_LTM', freq);

plotbode({sustf_LF0, sustf_LBF, sustf_LMT, sustf_LIM, sustf_LTM},...
         'ylim', [1e-4, 1e3], 'unit1', 'N', 'unit2', 'm');
saveas(gcf, ['fig/tfTypeA_Lalldiag_', datestr(now, 'yyyymmdd'), '.png'])
saveas(gcf, ['fig/tfTypeA_Lalldiag_', datestr(now, 'yyyymmdd')], 'epsc')
     
%% GAS
sustf_GASF0 = calcsystf(sysc0, 'injGASF0', 'LVDT_GASF0', freq);
sustf_GASF1 = calcsystf(sysc0, 'injGASF1', 'LVDT_GASF1', freq);
sustf_GASF2 = calcsystf(sysc0, 'injGASF2', 'LVDT_GASF2', freq);
sustf_GASF3 = calcsystf(sysc0, 'injGASF3', 'LVDT_GASF3', freq);
sustf_GASBF = calcsystf(sysc0, 'injGASBF', 'LVDT_GASBF', freq);

plotbode({sustf_GASF0, sustf_GASF1, sustf_GASF2, sustf_GASF3, sustf_GASBF},...
         'ylim', [1e-4, 1e3], 'unit1', 'N', 'unit2', 'm');
saveas(gcf, 'fig/tfTypeA_GASdiag_20170328.png')
saveas(gcf, 'fig/tfTypeA_GASdiag_20170328', 'epsc')
  
%% Yaw
sustf_YF0 = calcsystf(sysc0, 'injYF0', 'LVDT_YF0', freq2);
sustf_YBF = calcsystf(sysc0, 'injYBF', 'LVDT_YBF', freq2);

plotbode({sustf_YF0, sustf_YBF},...
         'ylim', [1e-4, 1e3], 'unit1', 'N', 'unit2', 'm');
saveas(gcf, ['fig/tfTypeA_Ytwrdiag_', datestr(now, 'yyyymmdd'), '.png'])
saveas(gcf, ['fig/tfTypeA_Ytwrdiag_', datestr(now, 'yyyymmdd')], 'epsc')
%}

%% SUSPENSION RESPONSE to TM RESIDUAL DISPLACEMENT
%  ========================================================================
%% Longitudinal actuators
tftm_actLF0 = calcsystf(sysc0, 'noiseActLF0', 'dispLTM', freq);
tftm_actLBF = calcsystf(sysc0, 'noiseActLBF', 'dispLTM', freq);
tftm_actLMT = calcsystf(sysc0, 'noiseActLMT', 'dispLTM', freq);
tftm_actLIM = calcsystf(sysc0, 'noiseActLIM', 'dispLTM', freq);
tftm_actLTM = calcsystf(sysc0, 'noiseActLTM', 'dispLTM', freq);

plotbode({tftm_actLF0, tftm_actLBF, tftm_actLMT, tftm_actLIM, tftm_actLTM},...
         'ylim', [1e-15, 1e0], 'unit1', 'N', 'unit2', 'm',...
         'legendLocation', 'southwest');
saveas(gcf, ['fig/tfTypeA_Lallact2LTM_', datestr(now, 'yyyymmdd'), '.png'])
saveas(gcf, ['fig/tfTypeA_Lallact2LTM_', datestr(now, 'yyyymmdd')], 'epsc')

%% DESIGN SERVO FILTERS
%  =========================================================================
%% IP translation
fltIPtrans = calczpktf([0], [3 3], 5*pp, freq);     % Normalized at 1 Hz
oltf_LF0 = plotbodeopenloopSISO(sustf_LF0, fltIPtrans, 30);
saveas(gcf, ['fig/oltfTypeA_LF0_', datestr(now, 'yyyymmdd'), '.png'])
saveas(gcf, ['fig/oltfTypeA_LF0_', datestr(now, 'yyyymmdd')], 'epsc')

%% IP rotation
fltIProt = calczpktf([0], [3 3], 5*pp, freq2);      % Normalized at 1 Hz
oltf_YF0 = plotbodeopenloopSISO(sustf_YF0, fltIProt, 10);
saveas(gcf, ['fig/oltfTypeA_YF0_', datestr(now, 'yyyymmdd'), '.png'])
saveas(gcf, ['fig/oltfTypeA_YF0_', datestr(now, 'yyyymmdd')], 'epsc')

%% BLENDING ANALYSIS
wBlend = 0.3;
nOrder = 7;
denom = [];
for k = 0:nOrder
    denom(k+1) = nchoosek(nOrder, k) * (wBlend*pp)^k;
end
blendLP = tf(denom(5:8), denom);
blendHP = tf(denom(1:4), denom);

%% F0 CONTROL IMPLEMENTATION
gainLF0 = -1;
gainTF0 = -1;
gainYF0 = -1;
servoLF0 = fltIPtrans.sys * 10;
servoTF0 = fltIPtrans.sys * 10;
servoYF0 = fltIProt.sys * 10;
blendLVDT_LF0 = blendLP;
blendLVDT_TF0 = blendLP;
blendLVDT_YF0 = 1;

st     = linmod(mdlfile);
invl   = strrep(st.InputName, [mdlfile,'/'],'');
outvl  = strrep(st.OutputName,[mdlfile,'/'],'');
syscIP = ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% VIBRATION ISOLATION RATIO
sustf_accGnd2LTM0 = calcsystf(sysc0, 'accLGnd', 'dispLTM', freq);
sustf_accGnd2LTM1 = calcsystf(syscIP, 'accLGnd', 'dispLTM', freq);
sustf_dispGnd2LTM0 = sustf_accGnd2LTM0;
sustf_dispGnd2LTM0.mag = sustf_dispGnd2LTM0.mag .*pp.*pp.*freq.*freq;
sustf_dispGnd2LTM0.phs = anglewithin180deg(sustf_dispGnd2LTM0.phs + 180);
sustf_dispGnd2LTM1 = sustf_accGnd2LTM1;
sustf_dispGnd2LTM1.mag = sustf_dispGnd2LTM1.mag .*pp.*pp.*freq.*freq;
sustf_dispGnd2LTM1.phs = anglewithin180deg(sustf_dispGnd2LTM1.phs + 180);
plotbode({sustf_dispGnd2LTM0, sustf_dispGnd2LTM1},...
         'ylim', [1e-10, 1e2],...
         'legend', {'No control', 'IP control'})


%% BF translation
sustf_LBF_cIP = calcsystf(syscIP, 'injLBF', 'LVDT_LBF', freq);
fltBFtrans = calczpktf([0], [10 10], 100*pp, freq);     % Normalized at 1 Hz
oltf_LBF = plotbodeopenloopSISO(sustf_LBF_cIP, fltBFtrans, 30);
saveas(gcf, ['fig/oltfTypeA_LBF_', datestr(now, 'yyyymmdd'), '.png'])
saveas(gcf, ['fig/oltfTypeA_LBF_', datestr(now, 'yyyymmdd')], 'epsc')

%% BF rotation
fltBFyaw = calczpktf([0], [10 10], 100*pp, freq2);      % Normalized at 1 Hz
oltf_YBF = plotbodeopenloopSISO(sustf_YBF, fltBFyaw, 100);
saveas(gcf, ['fig/oltfTypeA_YBF_', datestr(now, 'yyyymmdd'), '.png'])
saveas(gcf, ['fig/oltfTypeA_YBF_', datestr(now, 'yyyymmdd')], 'epsc')

%% BF CONTROL IMPLEMENTATION
% gainLBF = -1;
% gainTBF = -1;
gainYBF = -1;
% servoLBF = fltBFtrans.sys * 30;
% servoTBF = fltBFtrans.sys * 30;
servoYBF = fltBFyaw.sys * 100;

st     = linmod(mdlfile);
invl   = strrep(st.InputName, [mdlfile,'/'],'');
outvl  = strrep(st.OutputName,[mdlfile,'/'],'');
syscIPBF = ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% GAS F0
fltGASF0 = calczpktf([0], [3 3], 10*pp, freq);     % Normalized at 1 Hz
oltf_LF0 = plotbodeopenloopSISO(sustf_GASF0, fltGASF0, 3);
saveas(gcf, ['fig/oltfTypeA_GASF0_', datestr(now, 'yyyymmdd'), '.png'])
saveas(gcf, ['fig/oltfTypeA_GASF0_', datestr(now, 'yyyymmdd')], 'epsc')

%% GAS CONTROL IMPLEMENTATION
gainGASF0 = -1;
servoGASF0 = fltGASF0.sys * 3;

st     = linmod(mdlfile);
invl   = strrep(st.InputName, [mdlfile,'/'],'');
outvl  = strrep(st.OutputName,[mdlfile,'/'],'');
syscIPBFGAS = ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% VIBRATION ISOLATION RATIO
sustf_accGnd2VTM0 = calcsystf(sysc0, 'accVGnd', 'dispVTM', freq);
sustf_accGnd2VTM1 = calcsystf(syscIPBFGAS, 'accVGnd', 'dispVTM', freq);
sustf_dispGnd2VTM0 = sustf_accGnd2VTM0;
sustf_dispGnd2VTM0.mag = sustf_dispGnd2VTM0.mag .*pp.*pp.*freq.*freq;
sustf_dispGnd2VTM0.phs = anglewithin180deg(sustf_dispGnd2VTM0.phs + 180);
sustf_dispGnd2VTM1 = sustf_accGnd2VTM1;
sustf_dispGnd2VTM1.mag = sustf_dispGnd2VTM1.mag .*pp.*pp.*freq.*freq;
sustf_dispGnd2VTM1.phs = anglewithin180deg(sustf_dispGnd2VTM1.phs + 180);
plotbode({sustf_dispGnd2VTM0, sustf_dispGnd2VTM1},...
         'ylim', [1e-10, 1e2],...
         'legend', {'No control', 'GAS control'})


%%
cltf_GASF0 = calcsystf(syscIPBFGAS, 'injGASF0', 'LVDT_GASF0', freq);
plotbode({cltf_GASF0})
%%
cltf_YBF = calcsystf(syscIPBF, 'injYBF', 'LVDT_YBF', freq);
plotbode({cltf_YBF})
%% POLE-Q PLOT
plotsyspoleQ({sysc0, syscIP, syscIPBF, syscIPBFGAS},...
             'legend', {'Control OFF', 'IP Control', 'IP+BF Control', 'IP+BF+GAS Control', '1 min.'})
saveas(gcf, ['fig/poleQTypeA_IPBFGASctrl_', datestr(now, 'yyyymmdd'), '.png'])
saveas(gcf, ['fig/poleQTypeA_IPBFGASctrl_', datestr(now, 'yyyymmdd')], 'epsc')

%% CALC TM DISPLACEMENT
close all

% sustf_accGnd2LTM2 = calcsystf(syscIPBF, 'accLGnd', 'dispLTM', freq3);
% sustf_accGnd2LTM3 = calcsystf(syscIPBFGAS, 'accLGnd', 'dispLTM', freq3);
% sustf_dispGnd2LTM2 = sustf_accGnd2LTM2;
% sustf_dispGnd2LTM2.mag = sustf_dispGnd2LTM2.mag .*pp.*pp.*freq3.*freq3;
% sustf_dispGnd2LTM2.phs = anglewithin180deg(sustf_dispGnd2LTM2.phs + 180);
% sustf_dispGnd2LTM3 = sustf_accGnd2LTM3;
% sustf_dispGnd2LTM3.mag = sustf_dispGnd2LTM3.mag .*pp.*pp.*freq3.*freq3;
% sustf_dispGnd2LTM3.phs = anglewithin180deg(sustf_dispGnd2LTM3.phs + 180);

% LTMseis = sustf_Gnd2LTM.mag .* accSeismic;
% plotpsd({dispSeismic; LTMseis}, freq,...
%         'legend', {'Seismic', 'dispLTM'})

fileID = fopen('tfGnd2TMctrled.txt', 'w');
fprintf(fileID, '%s\n', '# TRANSFER FUNCTION: KAGRA TYPE-A SAS');
fprintf(fileID, '%10s\t%10s\t%10s\t%10s\t%10s\n',...
        '# freq [Hz]', 'magLTM/gnd', 'phsLTM/gnd', 'magVTM/gnd', 'phsVTM/gnd');
fprintf(fileID, '%e\t%e\t%e\t%e\t%e\n',...
        [freq; sustf_dispGnd2LTM1.mag; sustf_dispGnd2LTM1.phs;...
         sustf_dispGnd2VTM1.mag; sustf_dispGnd2VTM1.phs]);
fclose(fileID);

%% PURE SAS SS MODEL RESPONSE CHECK
%{
mdlfile ='TypeA161114';
st      = linmod(mdlfile);
invl    = strrep(st.InputName, [mdlfile,'/'],'');
outvl   = strrep(st.OutputName,[mdlfile,'/'],'');
sysSus  = ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%%
sustf_Gnd2LTM = calcsystf(sysSus, 'accLg', 'LTM', freq);
LTMseis = sustf_Gnd2LTM.mag .* accSeismic;
plotpsd({dispSeismic; LTMseis}, freq,...
        'legend', {'Seismic', 'dispLTM'})
%}