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

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


function [ref] = initializeReferences(opts_param)
    %% Default values for opts
    opts = struct(   ...
        'Ts',           1e-3, ...       % Sampling Frequency [s]
        'Dy_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
        'Dy_amplitude', 0, ...       % Amplitude of the displacement [m]
        'Dy_period',    1, ...          % Period of the displacement [s]
        'Ry_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
        'Ry_amplitude', 0, ...          % Amplitude [rad]
        'Ry_period',    10, ...         % Period of the displacement [s]
        'Rz_type',      'constant', ... % Either "constant" / "rotating"
        'Rz_amplitude', 0, ...          % Initial angle [rad]
        'Rz_period',    1, ...          % Period of the rotating [s]
        'Dh_type',      'constant', ... % For now, only constant is implemented
        'Dh_pos',       [0; 0; 0; 0; 0; 0], ...  % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
        'Rm_type',      'constant', ... % For now, only constant is implemented
        'Rm_pos',       [0; pi],  ...   % Initial position of the two masses
        'Dn_type',      'constant', ... % For now, only constant is implemented
        'Dn_pos',       [0; 0; 0; 0; 0; 0]  ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
    );

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

    %% Set Sampling Time
    Ts = opts.Ts;

    %% Translation stage - Dy
    t = 0:Ts:opts.Dy_period-Ts; % Time Vector [s]
    Dy = zeros(length(t), 1);
    switch opts.Dy_type
      case 'constant'
        Dy(:) = opts.Dy_amplitude;
      case 'triangular'
        Dy(:)                     = -4*opts.Dy_amplitude + 4*opts.Dy_amplitude/opts.Dy_period*t;
        Dy(t<0.75*opts.Dy_period) =  2*opts.Dy_amplitude - 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.75*opts.Dy_period);
        Dy(t<0.25*opts.Dy_period) =  4*opts.Dy_amplitude/opts.Dy_period*t(t<0.25*opts.Dy_period);
      case 'sinusoidal'
        Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t);
      otherwise
        warning('Dy_type is not set correctly');
    end
    Dy = struct('time', t, 'signals', struct('values', Dy));


    %% Tilt Stage - Ry
    t = 0:Ts:opts.Ry_period-Ts;
    Ry = zeros(length(t), 1);

    switch opts.Ry_type
      case 'constant'
        Ry(:) = opts.Ry_amplitude;
      case 'triangular'
        Ry(:)                     = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t;
        Ry(t<0.75*opts.Ry_period) =  2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period);
        Ry(t<0.25*opts.Ry_period) =  4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period);
      case 'sinusoidal'

      otherwise
        warning('Ry_type is not set correctly');
    end
    Ry = struct('time', t, 'signals', struct('values', Ry));

    %% Spindle - Rz
    t = 0:Ts:opts.Rz_period-Ts;
    Rz = zeros(length(t), 1);

    switch opts.Rz_type
      case 'constant'
        Rz(:) = opts.Rz_amplitude;
      case 'rotating'
        Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t;
      otherwise
        warning('Rz_type is not set correctly');
    end
    Rz = struct('time', t, 'signals', struct('values', Rz));

    %% Micro-Hexapod
    t = [0, Ts];
    Dh = zeros(length(t), 6);
    Dhl = zeros(length(t), 6);

    switch opts.Dh_type
      case 'constant'
        Dh = [opts.Dh_pos, opts.Dh_pos];

        load('./mat/stages.mat', 'micro_hexapod');

        AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)];

        tx = opts.Dh_pos(4);
        ty = opts.Dh_pos(5);
        tz = opts.Dh_pos(6);

        ARB = [cos(tz) -sin(tz) 0;
               sin(tz)  cos(tz) 0;
               0        0       1]*...
              [ cos(ty) 0 sin(ty);
               0        1 0;
               -sin(ty) 0 cos(ty)]*...
              [1 0        0;
               0 cos(tx) -sin(tx);
               0 sin(tx)  cos(tx)];

        [Dhl] = inverseKinematicsHexapod(micro_hexapod, AP, ARB);
        Dhl = [Dhl, Dhl];
      otherwise
        warning('Dh_type is not set correctly');
    end
    Dh = struct('time', t, 'signals', struct('values', Dh));
    Dhl = struct('time', t, 'signals', struct('values', Dhl));

    %% Axis Compensation - Rm
    t = [0, Ts];
    Rm = [opts.Rm_pos, opts.Rm_pos];
    Rm = struct('time', t, 'signals', struct('values', Rm));

    %% Nano-Hexapod
    t = [0, Ts];
    Dn = zeros(length(t), 6);

    switch opts.Dn_type
      case 'constant'
        Dn = [opts.Dn_pos, opts.Dn_pos];
      otherwise
        warning('Dn_type is not set correctly');
    end
    Dn = struct('time', t, 'signals', struct('values', Dn));

    %% Save
    save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
end