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

%% PRELIMINARY
clear all;  % Clear workspace
close all;  % Close plot windows
addpath('../../utility');  % Add path to utilities
addpath('controlmodel');
addpath('susmodel');
g = 9.81;

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

%% TUNING DAMPER
% This part compensates the failure in converting the structural damping to
% viscous damping.
% sys1.a(15,15)=sys1.a(15,15)*280; % YF0
% sys1.a(16,16)=sys1.a(16,16)*100000; % velLMD
% sys1.a(21,21)=sys1.a(21,21)*300000; % velYMD
% sys1.a(27,27)=sys1.a(27,27)*300000; % velYSF1

%%
%%%%% NO-CONTROL MODEL %%%%%
%% IMPORT SERVO FILTERS
addpath('parameter');         % Add path to servo
TypeA_DP_paramNoCtrl160713;      % NO 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,'/'],'');
sysc0  =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

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


%% PLOT TRANSFER FUNCTION OF THE SUSPENSION MECHANICAL RESPONSE
%{
TF_F0 = calcbodesus(sysc0,...
        {'injLF0','LVDT_LF0';'injTF0','LVDT_TF0';'injYF0','LVDT_YF0'}, freq);
TF_BF = calcbodesus(sysc0,...
        {'injLBF','LVDT_LBF';'injTBF','LVDT_TBF';'injVBF','LVDT_VBF';...
         'injRBF','LVDT_RBF';'injPBF','LVDT_PBF';'injYBF','LVDT_YBF'}, freq);
TFtest = calcbodesus(sysc0,...
        {'injYBF','LVDT_YF0'}, freq2);
%% LF0 actuation
close all
bodesusplotcmpopt3(sysc0,...
    {'injLF0','LVDT_LF0';'injLF0','LVDT_LBF';'injLF0','dispLDP'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','1');
saveas(gca, 'fig/tfTypeA_DP_actLF0_160714.png')
%% YF0 actuation
bodesusplotcmpopt3(sysc0,...
    {'injYF0','LVDT_YF0';'injYF0','LVDT_YBF';'injYF0','dispYDP'},...
    freq2,...
    'ylim',[1e-4,1e2],...
    'unit1','1');
saveas(gca, 'fig/tfTypeA_DP_actYF0_160714.png')
%% LBF actuation
close all
bodesusplotcmpopt3(sysc0,...
    {'injLBF','LVDT_LF0';'injLBF','LVDT_LBF';'injLBF','dispLDP'},...
    freq,...
    'ylim',[1e-6,1e2],...
    'unit1','1');
saveas(gca, 'fig/tfTypeA_DP_actLBF_160714.png')
%% YBF actuation
close all
bodesusplotcmpopt3(sysc0,...
    {'injYBF','LVDT_YF0';'injYBF','LVDT_YBF';'injYBF','dispYDP'},...
    freq2,...
    'ylim',[1e-6,1e2],...
    'unit1','1');
saveas(gca, 'fig/tfTypeA_DP_actYBF_160714.png')

%}

%% PLOT OPEN-LOOP TRANSFER FUNCTION OF FEEDBACK CONTROLS
%% LF0 control Loop
% ipservo  = myzpk([0.067], [3., 3.], 300*pp);
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/OLtf_TypeA_DP_LF0_160714.png')
%% YF0 control loop
ipservoY = myzpk([0], [1.e0, 1.e0], 10*pp);
OLTF_YF0 = bodesusplotOLTF(sysc0,...
    'injYF0','LVDT_YF0',ipservoY,freq2,...
    'ylim',[1e-3,1e3],...
    'unit1','1');
% saveas(gca, 'fig/OLtf_TypeA_DP_YF0_160714.png')

%% COLSED LOOP TRANSFER FUNCTION
% bodesusplotcmpopt(sysc0,...
%     {'actLF0','LVDT_LF0';'actYF0','LVDT_YF0'},freq,...
%     'ylim',[1e-4,1e1],...
%     'color',{'b-', 'g-'},...
%     'calibration',{1, 1},...
%     'unit1','1');

%%
%%%%% CONTROL MODEL %%%%%
%% IMPORT SERVO FILTERS
addpath('parameter');         % Add path to servo
TypeA_DP_paramIPCtrl160726;      % NO 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);
a=get(gca,'Children'); 
data=get(a(1),'Children'); 
set(data(1), 'LineWidth',10); 
set(data(2), 'LineWidth',10); 

%% 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
% n_LVDT_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]
%  data_seism = importdata('../../noise/KamiokaSeismicHighNoise.dat'); % m/rtHz
% noise_seism = interp1(data_seism(:,1),data_seism(:,2),freq')';% 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','disp_LDM',freq);
% [mag0S,~]=bodesus(sysc0,'accLGND','disp_LDM',freq);
% magsL=magsL.*n_LVDT_LF0;
% magsG=magsG.*n_GEO_LF0;
% magsS=magsS.*freq.*freq*pp*pp.*noise_seism;
% mag0S=mag0S.*freq.*freq*pp*pp.*noise_seism;
% 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);
% rmsT=makerms(freq,magsT);
% rmsTv=makerms(freq,magsT.*freq*pp);
% rms0S=makerms(freq,mag0S);
% rms0Sv=makerms(freq,mag0S.*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({magsS,mag0S,rmsS,rmsSv,rms0S,rms0Sv,noise_seism},freq,...
%     'title','DM Displacement Noise (GEO. Blending at 0.05 Hz)',...
%     'legend',{'IPctrl','no ctrl','IPctrl RMS',...
%     'IPctrl RMSvel.','no ctrl RMS','no ctrl RMSvel.','Seismic'},...
%     'color',{'r-','b-','r--','m--','b--','c--','k-'},...
%     'ylim',[1e-12,1e-5],'ylabel','Magnitude [m/rtHz] or [m] or [m/sec]')
% saveas(gca,'figure/typeA_RT_IPdamp_cmp_160512','epsc')
% 
syspole = poledamping({sysc0, sysc});
% saveas(gca, 'fig/decaytimeTypeA_DP_F0Ctrl160714.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')
% 
% %% TILT
% 
% freq3=logspace(-2.5,1,1001);
% 
% [magt0,phst0]=bodesus(sysc0,'accPGND','IFO_LTM',freq3);
% [magt1,phst1]=bodesus(sysc,'accPGND','IFO_LTM',freq3);
% magt0=magt0.*freq3.*freq3*pp*pp;
% magt1=magt1.*freq3.*freq3*pp*pp;
% 
% mypsdplotopt({magt1,magt0},freq3,...
%     'title','Sensitivity to the tilt',...
%     'ylim',[1e-3,1e4],'ylabel','Magnitude [m/rad]',...
%     'color',{'r-','b-'},'legend',{'Control ON','Control OFF'})
% 
% export_fig('figure/typeB1proto_IPdamp_DCservoON_tilt_150629.pdf')