%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% Type-A with Dummy Mass for KAGRA
% Coded by K. Okutomi on 2016.12.15
% ----------------------------------------------------------------------

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

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

%% IMPORT 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);

%% NO CONTROL ANALYSIS
% ----------------------------------------------------------------------------
%% PLOT TRANSFER FUNCTION OF THE SUSPENSION MECHANICAL RESPONSE
%{
%% F0 actuation
%{
bodesusplotcmpopt3(sysc0,...
    {'injLF0','ACC_LF0';'injLF0','LVDT_LF0'},...
    freq,...
    'color', {'m-', 'r-'},...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actLF0_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injTF0','ACC_TF0';'injTF0','LVDT_TF0'},...
    freq,...
    'color', {'m-', 'r-'},...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actTF0_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injYF0','ACC_YF0';'injYF0','LVDT_YF0'},...
    freq,...
    'color', {'m-', 'r-'},...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actYF0_170106', 'epsc')
%}

%% GAS actuation
%{
bodesusplotcmpopt3(sysc0,...
    {'injGASF0','LVDT_GASF0'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actGASF0_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injGASF1','LVDT_GASF1'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actGASF1_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injGASF2','LVDT_GASF2'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actGASF2_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injGASF3','LVDT_GASF3'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actGASF3_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injGASBF','LVDT_GASBF'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actGASBF_170106', 'epsc')
%}

%% BF actuation
%{
bodesusplotcmpopt3(sysc0,...
    {'injLBF','LVDT_LBF'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actLBF_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injTBF','LVDT_TBF'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actTBF_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injVBF','LVDT_VBF'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actVBF_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injRBF','LVDT_RBF'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actRBF_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injPBF','LVDT_PBF'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actPBF_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injYBF','LVDT_YBF'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actYBF_170106', 'epsc')
%}

%% MT actuation
%{
bodesusplotcmpopt3(sysc0,...
    {'injLMT','PS_LMT'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actLMT_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injTMT','PS_TMT'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actTMT_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injVMT','PS_VMT'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actVMT_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injRMT','PS_RMT'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actRMT_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injPMT','PS_PMT'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actPMT_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injYMT','PS_YMT'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actYMT_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injRMT','OpLev_RMT'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actRMTtoOpLev_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injPMT','OpLev_PMT'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actPMTtoOpLev_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injYMT','OpLev_YMT'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actYMTtoOpLev_170106', 'epsc')
%}

%% IM actuation
%{
bodesusplotcmpopt3(sysc0,...
    {'injLIM','PS_LIM'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actLIM_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injTIM','PS_TIM'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actTIM_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injVIM','PS_VIM'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actVIM_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injRIM','PS_RIM'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actRIM_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injPIM','PS_PIM'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actPIM_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injYIM','PS_YIM'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actYIM_170106', 'epsc')
%}

%% TM actuation
%{
bodesusplotcmpopt3(sysc0,...
    {'injLTM','PS_LTM'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/tfTypeA_actLTM_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injPTM','PS_PTM'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actPTM_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injYTM','PS_YTM'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actYTM_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injPTM','OpLev_PTM'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actPTMtoOpLev_170106', 'epsc')

bodesusplotcmpopt3(sysc0,...
    {'injYTM','OpLev_YTM'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','(N m)','unit2','rad');
saveas(gca, 'fig/tfTypeA_actYTMtoOpLev_170106', 'epsc')

%}
%}

%% PLOT OPEN-LOOP TRANSFER FUNCTION OF FEEDBACK CONTROLS
%% LF0 control Loop
ipservo  = myzpk([0], [3., 3.], 300*pp);
OLTF_LF0 = bodesusplotOLTF(sysc0,...
    'injLF0','LVDT_LF0',ipservo,freq,...
    'ylim',[1e-4,1e2],...
    'unit1','1');
saveas(gca, 'fig/oltfTypeA_LF0_170123', 'epsc')

%% YF0 control loop
ipservoY = myzpk([0], [1.e0, 1.e0], 10*pp);
OLTF_YF0 = bodesusplotOLTF(sysc0,...
    'injYF0','LVDT_YF0',ipservoY,freq2,...
    'ylim',[1e-2,1e3],...
    'unit1','1');
% saveas(gca, 'fig/oltfTypeA_YF0_170123', 'epsc')

%% YBF control loop
close all
bfservoY = myzpk([0], [0.3, 0.3], 5*pp);
OLTF_YF0 = bodesusplotOLTF(sysc0,...
    'injYBF','LVDT_YBF',bfservoY,freq2,...
    'ylim',[1e-4,1e2],...
    'unit1','1');
saveas(gca, 'fig/OLtf_TypeA_DP_YBF_160728.png')
%% GASF0 control loop
close all
gasf0servo = myzpk([0], [5., 5.], 100*pp);
OLTF_YF0 = bodesusplotOLTF(sysc0,...
    'injGASF0','LVDT_GASF0',gasf0servo,freq,...
    'ylim',[1e-4,1e2],...
    'unit1','1');
saveas(gca, 'fig/OLtf_TypeA_DP_GASF0_160728.png')


%%
%%%%% CONTROL MODEL %%%%%
%% IMPORT SERVO FILTERS
addpath('parameter');         % Add path to servo
TypeA_DP_paramAllCtrl160728;      % ALL CONTROL MODE
rmpath ('parameter');         % Remove path to servo

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

%% PLOT NYQUIST DIAGRAM
% set(0,'defaultFigurePosition',[50, 50, 850, 650]);
% set(0,'defaultAxesLineWidth',1.5);
% set(0,'defaultLineLineWidth',1.5);

nyq = nyquistoptions;
nyq.Grid = 'on';
nyq.ShowFullContour = 'on';

fig = figure;
% nyqLF0 = nyquistplot(sysc('LVDT_LF0','injLF0'),nyq);
% nyqYF0 = nyquistplot(sysc('LVDT_YF0','injYF0'),nyq);
% nyqYBF = nyquistplot(sysc('LVDT_YBF','injYBF'),nyq);
nyqGASF0 = nyquistplot(sysc('LVDT_GASF0','injGASF0'),nyq);
a=get(gca,'Children'); 
data=get(a(1),'Children'); 
set(data(1), 'LineWidth',3); 
set(data(2), 'LineWidth',3); 

%% PLOT CLOSED LOOP TRANSFER FUNCTION
%% LF0 actuation
bodesusplotcmpopt3(sysc,...
    {'injLF0','LVDT_LF0';'injLF0','LVDT_LBF';'injLF0','dispLDP'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/CLTF_TypeA_DP_actLF0_160728.png')
%% YF0 actuation
bodesusplotcmpopt3(sysc,...
    {'injYF0','LVDT_YF0';'injYF0','LVDT_YBF';'injYF0','dispYDP'},...
    freq2,...
    'ylim',[1e-4,1e2],...
    'unit1','(NEm)','unit2','rad');
saveas(gca, 'fig/CLTF_TypeA_DP_actYF0_160728.png')
%% YBF actuation
close all
bodesusplotcmpopt3(sysc,...
    {'injYBF','LVDT_YF0';'injYBF','LVDT_YBF';'injYBF','dispYDP'},...
    freq2,...
    'ylim',[1e-6,1e2],...
    'unit1','(NEm)','unit2','rad');
saveas(gca, 'fig/CFTF_TypeA_DP_actYBF_160728.png')
%% GASF0 actuation
close all
bodesusplotcmpopt3(sysc,...
    {'injGASF0','LVDT_GASF0';'injGASF0','LVDT_GASBF';'injGASF0','dispVDP'},...
    freq,...
    'ylim',[1e-3,1e3],...
    'unit1','N','unit2','m');
saveas(gca, 'fig/CLTF_TypeA_DP_actGASF0_160728.png')

%% PLOT VIBRATION ISOLARION RATIO
bodesusplotcmpopt4({sysc,sysc0},...
    'accLGnd','dispLDP',...
    freq,'legend',{'Control ON','Control OFF'},...
    'ylim',[1e-3,1e3],...
    'unit1','m','unit2','m');
saveas(gca, 'fig/VIR_TypeA_DP_toLDP_160728.png')
%%
bodesusplotcmpopt3(sysc0,...
    {'accLGnd','LVDT_LF0';'accLGnd','LVDT_LBF';'accLGnd','dispLDP'},...
    freq,...
    'title','Displacement from Ground Acceleration, Control OFF',...
    'ylim',[1e-8,1e4],...
    'unit1','(m/s^2)','unit2','m');
bodesusplotcmpopt3(sysc,...
    {'accLGnd','LVDT_LF0';'accLGnd','LVDT_LBF';'accLGnd','dispLDP'},...
    freq,...
    'title','Displacement from Ground Acceleration, Control ON',...
    'ylim',[1e-8,1e4],...
    'unit1','(m/s^2)','unit2','m');
% saveas(gca, 'fig/VIR_TypeA_DP_NoCtrl_toLDP_160728.png')


%% NOISE
% NOISE
 data_LVDT = importdata('../../noise/LVDTnoiseADC_disp.dat'); % um/rtHz
noise_LVDT = interp1(data_LVDT(:,1),data_LVDT(:,2)*1e-6,freq')';     %  m/rtHz
noiseLVDT_LF0 = noise_LVDT/sqrt(3/2);        % [m/rtHz]
%  data_GEO = importdata('../../noise/GEOnoiseproto_vel.dat'); % um/sec/rtHz
% noise_GEO = interp1(data_GEO(:,1),data_GEO(:,2)*1e-6,freq')';%  m/sec/rtHz
% n_GEO_LF0 = noise_GEO/sqrt(3/2);        % [m/rtHz]
% 
% %% MODEL
% addpath('servofilter');         % Add path to servo
% typeA_RT_servo_lock_160427;  % LOCK ACQUISITION MODE
% rmpath ('servofilter');         % Remove path to servo

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

%% CALC
% [magsL,phsL]=bodesus(sysc,'n_LVDT_LF0','disp_LDM',freq);
% [magsG,~]=bodesus(sysc,'n_ACC_LF0','disp_LDM',freq);
[magsS,~]=bodesus(sysc,'accLGnd','dispLDP',freq);
[mag0S,~]=bodesus(sysc0,'accLGnd','dispLDP',freq);
[magsSip,~]=bodesus(sysc,'accLGnd','LVDT_LF0',freq);
[mag0Sip,~]=bodesus(sysc0,'accLGnd','LVDT_LF0',freq);
[magsSy,~]=bodesus(sysc,'injYF0','dispYDP',freq);
[mag0Sy,~]=bodesus(sysc0,'injYF0','dispYDP',freq);
% magsL=magsL.*n_LVDT_LF0;
% magsG=magsG.*n_GEO_LF0;
magsS=magsS.*freq.*freq*pp*pp.*noiseSeismic;
mag0S=mag0S.*freq.*freq*pp*pp.*noiseSeismic;
magsSy=magsSip.*magsSy;
mag0Sy=mag0Sip.*mag0Sy;
% magsT=sumpsd({magsL,magsG,magsS});
rmsS=makerms(freq,magsS);
rmsSv=makerms(freq,magsS.*freq*pp);
rms0S=makerms(freq,mag0S);
rms0Sv=makerms(freq,mag0S.*freq*pp);
rmsSy=makerms(freq,magsSy);
rmsSyv=makerms(freq,magsSy.*freq*pp);
rms0Sy=makerms(freq,mag0Sy);
rms0Syv=makerms(freq,mag0Sy.*freq*pp);
% rmsT=makerms(freq,magsT);
% rmsTv=makerms(freq,magsT.*freq*pp);

%% Requirement
% reqBS=freq;
% for i=1:length(freq)
%    if freq(i)<8
%     reqBS(i)=0;
%    else
%     reqBS(i)=1e-15*(freq(i)/10)^(-1.5);
%    end
% end

%% PLOT
mypsdplotopt({noiseSeismic,mag0S,magsS,rms0S,rmsS},freq,...
    'legend',{'Seismic','Control OFF','Control ON',...
    'Control OFF RMS','Control ON RMS'},...
    'color',{'k-','b-','r-','c--','m--'},...
    'ylim',[1e-12,1e-5],'ylabel','Magnitude [m/rtHz] or [m]')
saveas(gca,'fig/psdTypeA_DP_AllCtrl_LDP160728.png')
%%
mypsdplotopt({noiseSeismic,mag0Sy,magsSy,rms0Sy,rmsSy},freq,...
    'legend',{'Seismic','Control OFF','Control ON',...
    'Control OFF RMS','Control ON RMS'},...
    'color',{'k-','b-','r-','c--','m--'},...
    'ylim',[1e-12,1e-2],'ylabel','Magnitude [rad/rtHz] or [rad]')
saveas(gca,'fig/psdTypeA_DP_AllCtrl_1mmAssymYDP160728.png')
%% 
syspole = poledamping({sysc0, sysc});
saveas(gca, 'fig/decaytimeTypeA_DP_AllCtrl160728.png')

% mypsdplotopt({magsG,magsL,magsS,magsT,rmsT,rmsTv},freq,...
%     'title','Blending at 0.05 Hz',...
%     'legend',{'GEO','LVDT','Seismic','total','RMS',...
%     'RMS velocity'},...
%     'color',{'b-','g-','k-','r-','r--','m--'},...
%     'ylim',[1e-12,1e-5],'ylabel','Magnitude [m/rtHz] or [m] or [m/sec]')
% saveas(gca,'figure/typeA_RT_IPdamp_DCservoON_160427','epsc')
% 
% %% COMPARE
% mypsdplotopt({magsG,magsL,magsS,magsT,mag0S,reqBS},freq,...
%     'title','Noise coupling',...
%     'legend',{'geophone','LVDT','seismic','Controlled total'...
%     'No controlled','BS requirement'},...
%     'color',{'g-','b-','k-','r-','m--','k--'},...
%     'ylim',[1e-20,1e-5],'ylabel','Magnitude [m/rtHz]')
% saveas(gca,'figure/typeA_RT_IPdamp_highfreq_noise_160427','epsc')
%