%% Type-A suspension SISO control analysis
%  2018.05.17 coded by K.Okutomi
%  ======================================================================

clear all
%%
cd('/Users/kokioktm/Documents/suspension/MATLAB/script/TypeA')
addpath('../../koktlib');
addpath('../../utility');
addpath('controlmodel');
addpath('parameter');
addpath('susmodel');
addpath('sumconExport');

%% Load model
load('TypeA_20K_180429mdl.mat');    % Suspension state-state model
TypeA_20K_180429;                   % Exported suspension ss model
TypeA_paramNoCtrl180517;            % Parameter set
%%
mdlfile = 'TypeA_siso180515';       % Simulink model
st      = linmod(mdlfile);          % extract linearized Simulink model
invl    = strrep(st.InputName, [mdlfile,'/'],'');   % trim InputName
outvl   = strrep(st.OutputName,[mdlfile,'/'],'');   % trim OutputName
sysc0   = ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% Eigenmode classification
%  1/e decay time が要求されるモード／されないモードを分類
%  sumcon2mdlconst を用いて生成したss modelだと固有値解析するときにいくつかの極が
%  splitして非振動極になってたりするっぽいので、制御系なしのsuspensionの特性解析は
%  SUMCONから直接exportしたss modelを使うのが良い気がする。
%  ただし自由度の順番が(LTVRPY)系と異なるので注意（SUMCONでは(TVLPYR)系）。
%
sus = ss(ssA(13:162, 13:162), ssB(13:162, 7:81), ssC(13:162, 13:162), ssD(13:162, 7:81),...
         'InputName', varinput(7:81), 'OutputName', varoutput(13:162),...
         'StateName', varoutput(13:162));
[phi, fmode, Qmode] = eigenmode(sus.a);
phi_d = phi(1:75, :);
phi_v = phi(76:150, :);
phi_dTM = phi(70:75, :);
phi_vTM = phi(145:150, :);
[rowd, cold] = find(phi_dTM);
[rowv, colv] = find(phi_vTM);

%% Controllability & Observability
Cgram = gram(sus, 'c');
Ogram = gram(sus, 'o');
sv_sus = hsvd(sus);
figure(1)
semilogy((1:length(sv_sus)/2), sv_sus(1:2:end), 'r.', 'markersize', 20)
grid on

% Hankel特異値の大きなモードを特定することができる
% ただし、適切な入力行列と出力行列を指定する必要がある
[susm, Tmodal] = canon(sus, 'modal');
[susb, svb, Tbal] = balreal(susm);

%% Mechanical Trasnfer Functions
freq = logspace(-2, 1, 1001);
[IPLmag, IPLphs] = mybode(sysc0('LVDT_IPL','injIPL'), freq);


figure(1)
subplot('Position', [0.1 0.45 0.85 0.52])
loglog(freq, IPLmag, 'r')
grid on
ylabel('Magnitude', 'fontsize', 28)
set(gca, 'XTickLabel', '')
subplot('Position', [0.1 0.105 0.85 0.3])
semilogx(freq, IPLphs, 'r')
ylim([-180, 180])
xlabel('Frequency [Hz]', 'fontsize', 28)
ylabel('Phase [deg]', 'fontsize', 28)
set(gca, 'YTick', (-180:90:180))
grid on

%% GAS modal damping
chvertIn  = {'FyMD'; 'FyF1'; 'FyF2'; 'FyF3'; 'FyBF'; 'FyPF';...
             'FyMNR'; 'FyMN'; 'FyIRM'; 'FyIM'; 'FyRM'; 'FyTM'};
chvertOut = {'yMD'; 'yF1'; 'yF2'; 'yF3'; 'yBF'; 'yPF';...
             'yMNR'; 'yMN'; 'yIRM'; 'yIM'; 'yRM'; 'yTM';...
             'vyMD'; 'vyF1'; 'vyF2'; 'vyF3'; 'vyBF'; 'vyPF';...
             'vyMNR'; 'vyMN'; 'vyIRM'; 'vyIM'; 'vyRM'; 'vyTM'};
susV = minreal(sus(chvertOut, chvertIn));

% 状態変数の並び替え
[row, ~] = find(inv(susV.c));
susV = xperm(susV, row);
susV.StateName = chvertOut;

%%
senGAS = [0 1 0 0 0 0 ;     % F0 GAS LVDT
          0 -1 1 0 0 0;     % F1 GAS LVDT
          0 0 -1 1 0 0;     % F2 GAS LVDT
          0 0 0 -1 1 0;     % F3 GAS LVDT
          0 0 0 0 -1 1];    % BF GAS LVDT
actGAS = [0 0 0 0 0;
          1 -1 0 0 0;
          0 1 -1 0 0;
          0 0 1 -1 0;
          0 0 0 1 -1;
          0 0 0 0 1];



%% 1/e decay time plot
plotsyspoleQ(sysc0)


%% Old junks
%{
Asus = sys1.a(13:162, 13:162);
Bsus = sys1.b(13:162, 7:81);
Csus = sys1.c(13:162, 13:162);
Dsus = sys1.d(13:162, 7:81);
sus  = ss(Asus, Bsus, Csus, Dsus,...
          'InputName', sys1.InputName(7:81), 'OutputName', sys1.OutputName(13:162),...
          'StateName', sys1.StateName(13:162));
[susm, Tm] = canon(sus, 'modal');
T(abs(T)<1e-6) = 0;
[phi, eigen] = eig(Asus);
eigen = diag(eigen);
phi(abs(phi)<1e-6) = 0;

% [phi, modes] = modaldecomp(sus);
% [phi0, eigen0] = eig(sysc0.a);
[phi0, fmode, Qmode, phi1] = eigenmode(Asus);
%}