% identifyPlant
%   :PROPERTIES:
%   :header-args:matlab+: :tangle src/identifyPlant.m
%   :header-args:matlab+: :comments org :mkdirp yes
%   :header-args:matlab+: :eval no :results none
%   :END:
%   <<sec:identifyPlant>>

% This Matlab function is accessible [[file:src/identifyPlant.m][here]].


function [sys] = identifyPlant(opts_param)
    %% Default values for opts
    opts = struct();

    %% Populate opts with input parameters
    if exist('opts_param','var')
        for opt = fieldnames(opts_param)'
            opts.(opt{1}) = opts_param.(opt{1});
        end
    end

    %% Options for Linearized
    options = linearizeOptions;
    options.SampleTime = 0;

    %% Name of the Simulink File
    mdl = 'sim_nano_station_id';

    %% Input/Output definition
    io(1) = linio([mdl, '/Fn'],   1, 'input'); % Cartesian forces applied by NASS
    io(2) = linio([mdl, '/Dw'],   1, 'input'); % Ground Motion
    io(3) = linio([mdl, '/Fs'],   1, 'input'); % External forces on the sample
    io(4) = linio([mdl, '/Fnl'],  1, 'input'); % Forces applied on the NASS's legs
    io(5) = linio([mdl, '/Dsm'],  1, 'output'); % Displacement of the sample
    io(6) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
    io(7) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
    io(8) = linio([mdl, '/Es'],   1, 'output'); % Position Error w.r.t. NASS base

    %% Run the linearization
    G = linearize(mdl, io, 0);
    G.InputName  = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
                    'Dgx', 'Dgy', 'Dgz', ...
                    'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
                    'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
    G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
                    'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
                    'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
                    'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'};

    %% Create the sub transfer functions
    % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
    sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}));
    % From ground motion to Sample displacement
    sys.G_gm   = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}));
    % From direct forces applied on the sample to displacement of the sample
    sys.G_fs   = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}));
    % From forces applied on NASS's legs to force sensor in each leg
    sys.G_iff  = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
    % From forces applied on NASS's legs to displacement of each leg
    sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
    % From forces applied on NASS's legs to displacement of each leg
    sys.G_plant = minreal(G({'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}));
end