%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% SUSPENSION MODEL CONSTRUCTOR
% This code constructs a state-space model of suspension system and 
% express it as a simulink model with a "state-space model" block.
% ----------------------------------------------------------------------
% 3D rigid body model
% Type-B payload prototype for KAGRA
% Coded by T. Sekiguchi on 2015/06/03
% ----------------------------------------------------------------------


%% PRELIMINARY

clear all;  % Clear workspace
close all;  % Close plot windows

addpath('../../utility');  % Add path to utilities



%% PARAMETER

addpath('parameter');  % Add path to parameter files

typeB1protoparam_150502;  % Get parameters
% Type-B payload prototype parameters version 141008

rmpath('parameter');   % Remove path from parameter files



%% MAKE STRUCTURE

% Make suspension structure
sus = makesusstructure('payproto141008');

% Add grounds
sus = addground(sus,'GND');

% Add rigid bodies
sus = addrigidbody(sus,'IR',mIR,moiIR); % intermediate recoil mass
sus = addrigidbody(sus,'IM',mIM,moiIM); % intermediate mass
sus = addrigidbody(sus,'RM',mRM,moiRM); % recoil mass
sus = addrigidbody(sus,'TM',mTM,moiTM); % test mass

% Add suspension wires and springs
sus = addsuswire(sus,...       % GND-IR
    n_wF22IR,...               % number of wires
    {},...                     % OPTION
    'GND',...                  % upper body
    'IR',...                   % lower body
    [...                       % GEOMETRIC PARAMETER
    h1_wF22IR,...              % vertical position of upper SP
    h2_wF22IR,...              % vertical position of lower SP
    r1_wF22IR...               % horizontal distance of upper SP from CoM
    ],...                      % 
    l_wF22IR,...               % length
    d_wF22IR,...               % diameter
    m_wF22IR,...               % total load
    E_wF22IR,...               % Young's modulus
    G_wF22IR,...               % shear modulus
    Q_wF22IR...                % pendulum Q
    );

sus = addsuswire(sus,...       % GND-IM
    n_wF22IM,...               % number of wires
    {...                       % OPTION
    'spring',...               % add spring
    'neck',...                 % wire with necks
    },...                      %
    'GND',...                  % upper body
    'IM',...                   % lower body
    [...                       % GEOMETRIC PARAMETER
    h1_wF22IM,...              % vertical position of upper SP
    h2_wF22IM...               % vertical position of lower SP
    ],...                      %
    [l_wF22IM,ln_wF22IM],...   % length & neck length
    [d_wF22IM,dn_wF22IM],...   % diameter & neck diameter
    m_wF22IM,...               % total load
    E_wF22IM,...               % Young's modulus
    G_wF22IM,...               % shear modulus
    Q_wF22IM,...               % pendulum Q
    {'spring',...              % SPRING
    fsp_wF22IM,...             % resonant frequency
    Bsp_wF22IM,...             % saturation due to CoP
    Qsp_wF22IM...              % spring Q
    }...                       %
    );

sus = addsuswire(sus,...       % IM-RM
    n_wIM2RM,...               % number of wires
    {},...                     % OPTIONS
    'IM',...                   % upper body
    'RM',...                   % lower body
    [...                       % GEOMETRIC PARAMETERS
    h1_wIM2RM,...              % vertical position of upper SP
    h2_wIM2RM,...              % vertical position of lower SP
    w1_wIM2RM,...              % transversal position of upper SP
    d1_wIM2RM...              % longitudinal position of upper SP
    ],...
    l_wIM2RM,...               % length
    d_wIM2RM,...               % diameter
    m_wIM2RM,...               % total load
    E_wIM2RM,...               % Young's modulus
    G_wIM2RM,...               % shear modulus
    Q_wIM2RM...                % pendulum Q
    );

sus = addsuswire(sus,...       % IM-TM
    n_wIM2TM,...               % number of wires
    {},...                     % OPTIONS
    'IM',...                   % upper body
    'TM',...                   % lower body
    [...                       % GEOMETRIC PARAMETERS
    h1_wIM2TM,...              % vertical position of upper SP
    h2_wIM2TM,...              % vertical position of lower SP
    w1_wIM2TM,...              % transversal position of upper SP
    d1_wIM2TM...              % longitudinal position of upper SP
    ],...
    l_wIM2TM,...               % length
    d_wIM2TM,...               % diameter
    m_wIM2TM,...               % total load
    E_wIM2TM,...               % Young's modulus
    G_wIM2TM,...               % shear modulus
    Q_wIM2TM...                % pendulum Q
    );



%% BUILD SUSPENSION MODEL

sus = buildsusmodel(sus);      % make state-space matrix



%% SIMULINK MODEL

% input variables
var_a_GND = {'accLGND','accTGND','accVGND','accRGND','accPGND','accYGND'}; % GND acceleration
var_a_IM  = {'actLIM','actTIM','actVIM','actRIM','actPIM','actYIM'}; % IM actuation
var_a_IR  = {'actLIR','actTIR','actVIR','actRIR','actPIR','actYIR'}; % IR actuation
var_a_RM  = {'actLRM','actPRM','actYRM'};                % RM actuation
var_a_TM  = {'actLTM','actTTM','actVTM','actRTM','actPTM','actYTM'}; % TM actuation

% input variable list
invsim = [var_a_GND,var_a_IM,var_a_IR,var_a_RM,var_a_TM];

% output variables
var_d_IM  = {'LIM','TIM','VIM','RIM','PIM','YIM'};       % IM displacement
var_d_IR  = {'LIR','TIR','VIR','RIR','PIR','YIR'};       % IR displacement
var_d_RM  = {'LRM','PRM','YRM'};                         % RM displacement
var_d_TM  = {'LTM','TTM','VTM','RTM','PTM','YTM'};       % TM displacement

% output variable list
outvsim=[var_d_IM,var_d_IR,var_d_RM,var_d_TM];

% simulink model
sys1=sus.ss;               % ss model
constructsimmodel(...      % CONSTRUCT SIMUKINK BLOCK MODEL
    'typeBpay',...         % model name
    sys1,...               % state-space model
    'sys1',...             % state-space model name
    invsim,...             % input variables
    outvsim...             % output variables
    );                     % 
save('payloadprotosusmdl','sys1','sus');  % save sus model

%% Passive Calculation
freq=logspace(-2,2,1001);
bodesusplot(sys1,'actLTM','LTM',freq);

%% Eigen
eiglist=makeeigenlist(sus);