%% First make sure the model is open, and we are connected to the
% Speedgoat Machine.

%% Run Multiple Simulations
my_model = 'iff_measure';
tg = slrt;

%% For each strut
for i_leg = 1:6
    %% Get excitation strut
    p = Simulink.Mask.get([my_model, '/Subsystem7']);
    p.Parameters.Value = sprintf('%i', i_leg);
    pause(0.1);
    
    %% Connect
    set_param(my_model,'SimulationCommand','connect');
    pause(0.1);

    %% Run the simulation
    sprintf('Start excitation for strut %i', i_leg)
    set_param(my_model,'SimulationCommand','start')
    
    %% Wait for the simulation to finish
    pause(1)
    while strcmp(get_param(my_model,'SimulationStatus'), 'external')
        pause(1)
    end
    sprintf('Finished excitation for strut %i', i_leg);

    %% Disconnect
    set_param(my_model,'SimulationCommand','disconnect');
    pause(0.1);

    %% Save the data
    f = SimulinkRealTime.openFTP(tg);
    mget(f, 'data/data.dat');
    close(f);
    data = SimulinkRealTime.utils.getFileScopeData('data/data.dat').data;

    de = data(:, 1:6);   % Measurment displacement (encoder) [m]
    Vs = data(:, 7:12);  % Force Sensor [V]
    u  = data(:, 13:18); % Control Output [V]
    Va = data(:, 19);    % Excitation Signal [V]
%     Rx = data(:, 20:25); % Reference Signal [m/rad]
    t  = data(:, end);   % Time [s]
    
    load('sim_data/data_sim.mat', 'Ts'); % To save Sampling Period
    save('sim_data/Kiff.mat', 'Kiff'); % To save Controller
    
    save(sprintf('mat/frf_data_exc_strut_%i_spindle_3m_iff_60rpm.mat', i_leg), ...
        't', 'Ts', 'de', 'Vs', 'u', 'Va', 'Kiff');

    sprintf('Saved Data for strut %i', i_leg)
    pause(0.1);
end