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

% - [ ] *This function should not be used anymore*. Now there are two functions to initialize disturbances and references.

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


function [inputs] = initializeInputs(opts_param)
    %% Default values for opts
    opts = struct(   ...
        'Dw', false, ...
        'Dy', false, ...
        'Ry', false, ...
        'Rz', false, ...
        'Dh', false, ...
        'Rm', false, ...
        'Dn', false  ...
    );

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

    %% Load Sampling Time and Simulation Time
    load('./mat/sim_conf.mat', 'sim_conf');

    %% Define the time vector
    t = 0:sim_conf.Ts:sim_conf.Tsim;

    %% Ground motion - Dw
    if islogical(opts.Dw) && opts.Dw == true
        load('./mat/perturbations.mat', 'Wxg');
        Dw = 1/sqrt(2)*100*random('norm', 0, 1, length(t), 3);
        Dw(:, 1) = lsim(Wxg, Dw(:, 1), t);
        Dw(:, 2) = lsim(Wxg, Dw(:, 2), t);
        Dw(:, 3) = lsim(Wxg, Dw(:, 3), t);
    elseif islogical(opts.Dw) && opts.Dw == false
        Dw = zeros(length(t), 3);
    else
        Dw = opts.Dw;
    end

    %% Translation stage - Dy
    if islogical(opts.Dy) && opts.Dy == true
        Dy = zeros(length(t), 1);
    elseif islogical(opts.Dy) && opts.Dy == false
        Dy = zeros(length(t), 1);
    else
        Dy = opts.Dy;
    end

    %% Tilt Stage - Ry
    if islogical(opts.Ry) && opts.Ry == true
        Ry = 3*(2*pi/360)*sin(2*pi*0.2*t);
    elseif islogical(opts.Ry) && opts.Ry == false
        Ry = zeros(length(t), 1);
    elseif isnumeric(opts.Ry) && length(opts.Ry) == 1
        Ry = opts.Ry*(2*pi/360)*ones(length(t), 1);
    else
        Ry = opts.Ry;
    end

    %% Spindle - Rz
    if islogical(opts.Rz) && opts.Rz == true
        Rz = 2*pi*0.5*t;
    elseif islogical(opts.Rz) && opts.Rz == false
        Rz = zeros(length(t), 1);
    elseif isnumeric(opts.Rz) && length(opts.Rz) == 1
        Rz = opts.Rz*(2*pi/60)*t;
    else
        Rz = opts.Rz;
    end

    %% Micro Hexapod - Dh
    if islogical(opts.Dh) && opts.Dh == true
        Dh = zeros(length(t), 6);
    elseif islogical(opts.Dh) && opts.Dh == false
        Dh = zeros(length(t), 6);
    else
        Dh = opts.Dh;
    end

    %% Axis Compensation - Rm
    if islogical(opts.Rm)
        Rm = zeros(length(t), 2);
        Rm(:, 2) = pi*ones(length(t), 1);
    else
        Rm = opts.Rm;
    end

    %% Nano Hexapod - Dn
    if islogical(opts.Dn) && opts.Dn == true
        Dn = zeros(length(t), 6);
    elseif islogical(opts.Dn) && opts.Dn == false
        Dn = zeros(length(t), 6);
    else
        Dn = opts.Dn;
    end

    %% Setpoint - Ds
    Ds = zeros(length(t), 6);
    for i = 1:length(t)
        Ds(i, :) = computeSetpoint(Dy(i), Ry(i), Rz(i));
    end

    %% External Forces applied on the Granite
    Fg = zeros(length(t), 3);

    %% External Forces applied on the Sample
    Fs = zeros(length(t), 6);

    %% Create the input Structure that will contain all the inputs
    inputs = struct( ...
        'Ts', sim_conf.Ts,       ...
        'Dw', timeseries(Dw, t), ...
        'Dy', timeseries(Dy, t), ...
        'Ry', timeseries(Ry, t), ...
        'Rz', timeseries(Rz, t), ...
        'Dh', timeseries(Dh, t), ...
        'Rm', timeseries(Rm, t), ...
        'Dn', timeseries(Dn, t), ...
        'Ds', timeseries(Ds, t), ...
        'Fg', timeseries(Fg, t), ...
        'Fs', timeseries(Fs, t)  ...
    );

    %% Save
    save('./mat/inputs.mat', 'inputs');

    %% Custom Functions
    function setpoint = computeSetpoint(ty, ry, rz)
    %%
    setpoint = zeros(6, 1);

    %% Ty
    TMTy = [1 0 0 0  ;
          0 1 0 ty ;
          0 0 1 0  ;
          0 0 0 1 ];

    %% Ry
    TMRy = [ cos(ry) 0 sin(ry) 0 ;
          0       1 0       0 ;
          -sin(ry) 0 cos(ry) 0 ;
          0        0 0       1 ];

    %% Rz
    TMRz = [cos(rz) -sin(rz) 0 0 ;
          sin(rz)  cos(rz) 0 0 ;
          0        0       1 0 ;
          0        0       0 1 ];

    %% All stages
    TM = TMTy*TMRy*TMRz;

    [thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));

    setpoint(1:3) = TM(1:3, 4);
    setpoint(4:6) = [thetax, thetay, thetaz];

    %% Custom Functions
    function [thetax, thetay, thetaz] = RM2angle(R)
        if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
            thetay = -asin(R(3, 1));
            thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
            thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
        else
            thetaz = 0;
            if abs(R(3, 1)+1) < 1e-6 % R31 = -1
                thetay = pi/2;
                thetax = thetaz + atan2(R(1, 2), R(1, 3));
            else
                thetay = -pi/2;
                thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
            end
        end
    end
    end
end