function [] = initDisturbances(args)
% initDisturbances - Initialize the disturbances
%
% Syntax: [] = initDisturbances(args)
%
% Inputs:
%    - args -

arguments
    % Global parameter to enable or disable the disturbances
    args.enable logical {mustBeNumericOrLogical} = true
    % Ground Motion - X direction
    args.Dwx logical {mustBeNumericOrLogical} = true
    % Ground Motion - Y direction
    args.Dwy logical {mustBeNumericOrLogical} = true
    % Ground Motion - Z direction
    args.Dwz logical {mustBeNumericOrLogical} = true
    % Translation Stage - X direction
    args.Fty_x logical {mustBeNumericOrLogical} = true
    % Translation Stage - Z direction
    args.Fty_z logical {mustBeNumericOrLogical} = true
    % Spindle - Z direction
    args.Frz_z logical {mustBeNumericOrLogical} = true
end

load('./disturbances/mat/dist_psd.mat', 'dist_f');

dist_f.f = dist_f.f(2:end);
dist_f.psd_gm = dist_f.psd_gm(2:end);
dist_f.psd_ty = dist_f.psd_ty(2:end);
dist_f.psd_rz = dist_f.psd_rz(2:end);

Fs = 2*dist_f.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
N  = 2*length(dist_f.f);   % Number of Samples match the one of the wanted PSD
T0 = N/Fs;                 % Signal Duration [s]
df = 1/T0;                 % Frequency resolution of the DFT [Hz]
                           % Also equal to (dist_f.f(2)-dist_f.f(1))
t = linspace(0, T0, N+1)'; % Time Vector [s]
Ts = 1/Fs;                 % Sampling Time [s]

phi = dist_f.psd_gm;
C = zeros(N/2,1);
for i = 1:N/2
  C(i) = sqrt(phi(i)*df);
end

if args.Dwx && args.enable
  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
  Cx = [Cx; flipud(conj(Cx(2:end)))];;
  Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
else
  Dwx = zeros(length(t), 1);
end

if args.Dwy && args.enable
  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
  Cx = [Cx; flipud(conj(Cx(2:end)))];;
  Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
else
  Dwy = zeros(length(t), 1);
end

if args.Dwy && args.enable
  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
  Cx = [Cx; flipud(conj(Cx(2:end)))];;
  Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
else
  Dwz = zeros(length(t), 1);
end

if args.Fty_x && args.enable
  phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
  C = zeros(N/2,1);
  for i = 1:N/2
    C(i) = sqrt(phi(i)*df);
  end
  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
  Cx = [Cx; flipud(conj(Cx(2:end)))];;
  u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
  Fty_x = u;
else
  Fty_x = zeros(length(t), 1);
end

if args.Fty_z && args.enable
  phi = dist_f.psd_ty;
  C = zeros(N/2,1);
  for i = 1:N/2
    C(i) = sqrt(phi(i)*df);
  end
  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
  Cx = [Cx; flipud(conj(Cx(2:end)))];;
  u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
  Fty_z = u;
else
  Fty_z = zeros(length(t), 1);
end

if args.Frz_z && args.enable
  phi = dist_f.psd_rz;
  C = zeros(N/2,1);
  for i = 1:N/2
    C(i) = sqrt(phi(i)*df);
  end
  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
  Cx = [Cx; flipud(conj(Cx(2:end)))];;
  u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
  Frz_z = u;
else
  Frz_z = zeros(length(t), 1);
end

u = zeros(length(t), 6);
Fd = u;

Dwx    = Dwx   - Dwx(1);
Dwy    = Dwy   - Dwy(1);
Dwz    = Dwz   - Dwz(1);
Fty_x  = Fty_x - Fty_x(1);
Fty_z  = Fty_z - Fty_z(1);
Frz_z  = Frz_z - Frz_z(1);

save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't');