Using Matlab in the Lab (REWORK)

function main_vehicle_ids(varargin)
	% Set the matlab domain ID for communicating with the middleware (which is always 1)
    matlabDomainID = 1;

	% Clear command window, store path of current script, go to that path
    clc
    script_directoy = fileparts([mfilename('fullpath') '.m']);
    cd(script_directoy)

    % Set the path of the init script relative to the path of your script (or absolute), assert that it exists, add it to the path so that it can be found and than call it
	% Then go back to the script directory
    init_script_path = fullfile('../', '/init_script.m');
    assert(isfile(init_script_path), 'Missing file "%s".', init_script_path);
    addpath(fileparts(init_script_path));
    [matlabParticipant, stateReader, trajectoryWriter, systemTriggerReader, readyStatusWriter, trigger_stop] = init_script(matlabDomainID);
    cd(script_directoy)

	% Remember the set vehicle IDs in a new variable
    vehicle_ids = varargin;

    %% Use a waitset for the reader of vehicle states, which is later used to indicate if a new computation should be started
	% The waitset makes sure that take() or read() from the reader's storage does not return if no new data is available (until the timeout is reached, here 10 seconds
	% so that one can still check for a stop signal regularly)
    stateReader.WaitSet = true;
    stateReader.WaitSetTimeout = 10;

    %% Do not display figures
    set(0,'DefaultFigureVisible','off');

% (Assuming that vehicle_ids contains the IDs your script is responsible for)
% Send first ready signal to the Middleware to indicate that your program is ready to operate
% The signal needs to be sent for all assigned vehicle ids separately (usually, one script on one NUC only manages one of the IDs)
% Also works for simulated time - period etc are set in Middleware, so you can ignore the timestamp field
for i = 1 : length(vehicle_ids)
	% Create a new msg of type ReadyStatus (idl)
    ready_msg = ReadyStatus;

	% Set the values of ready_msg
    ready_msg.source_id = strcat('hlc_', num2str(vehicle_ids{i}));
    ready_stamp = TimeStamp;
    ready_stamp.nanoseconds = uint64(0);
    ready_msg.next_start_stamp = ready_stamp;

	% Send the ready msg
    readyStatusWriter.write(ready_msg);
end

% Wait for start signal / stop if a stop signal was received
got_stop = false;
got_start = false;
while(true)
	% Read the newest system trigger message, if one exists
    trigger = SystemTrigger;
    sampleCount = 0;
    [trigger, status, sampleCount, sampleInfo] = systemTriggerReader.take(trigger);

	% Go through all received messages since the last check, if they exist (ssampleCount gives the number of read messages)
	% Any received message would indicate "start", but you have to check if a stop signal was received as well
    while sampleCount > 0
		% Check for stop signal
        if trigger.next_start().nanoseconds() == trigger_stop
            got_stop = true;
        elseif trigger.next_start().nanoseconds() >= 0
            got_start = true;
        end

		% Read the next sample and continue until sampleCount is zero again (all samples have been read then)
        [trigger, status, sampleCount, sampleInfo] = systemTriggerReader.take(trigger);
    end

	% Stop the loop if a signal was received, then proceed with handling a stop signal (stop the program) or a start signal (continue)
    if got_stop | got_start
        break;
    end
end

% Again: Go through all samples and stop the program if you received a stop signal
% break_while is the name used in the sample program (as it breaks the enclosing while-loop of the whole program), but you could also name it e.g. stop_received
trigger = SystemTrigger;
sampleCount = 0;
[trigger, status, sampleCount, sampleInfo] = systemTriggerReader.take(trigger);

break_while = false;

while sampleCount > 0
    current_time = trigger.next_start().nanoseconds();
    if current_time == trigger_stop
        break_while = true;
    end

    [trigger, status, sampleCount, sampleInfo] = systemTriggerReader.take(trigger);
end

if break_while
    ...

...
% Read the current vehicle information from the according reader, if such data is available
% Due to the set waitset, you wait for up to 10 seconds at take(...) if no data is available
sample = VehicleStateList;
status = 0;
stateSampleCount = 0;
sampleInfo = DDS.SampleInfo;
[sample, status, stateSampleCount, sampleInfo] = stateReader.take(sample);

% In this example, messages that were received during computation that should be ignored are not thrown away before waiting for new data
% If your script is faster than the period with which it is called, this is not problematic, in any other case you will probably end up using wrong timing and outdated vehicle data
% Thus, you should get rid of old data at the end of any iteration ('clear' stateReader)

% Check if any new message was received, then proceed with handling the data, computing your solution and finally sending back a command to the vehicle(s) your script controls
if stateSampleCount > 0 
    ...

% Call the programs which calculate the trajectories of all HLCs, then send the results back to the vehicles
if (size(vehicle_ids) > 0)
    for i = 1 : length(vehicle_ids)
        msg_leader = leader(vehicle_ids{i}, sample.t_now);

		% Send resulting trajectory to the vehicle
        trajectoryWriter.write(msg_leader);
    end
end

%Create msg of type trajectory
trajectory = VehicleCommandTrajectory;

% Set the vehicle ID
trajectory.vehicle_id = uint8(vehicle_id);

% In this example, we only send one trajectory point (which is not sufficient for interpolation)
trajectory_points = [];
point1 = TrajectoryPoint;

% This trajectory point should be considered in the future, so add some nanoseconds to the current time, then set the time stamp for the trajectory point
% t_eval here is the time that we got from the middleware in the VehicleStateList message - you are not supposed to use your own clock / timer (else you would have trouble working with simulated time)
time = t_eval + 400000000;
stamp = TimeStamp;
stamp.nanoseconds = uint64(time);
point1.t = stamp;

% Set other trajectory values - position and velocity in 2D coordinates
point1.px = trajectory_point(1);
point1.py = trajectory_point(2);
point1.vx = trajectory_point(3);
point1.vy = trajectory_point(4);

% Append to the list of all trajectory points
trajectory_points = [trajectory_points [point1]];
trajectory.trajectory_points = trajectory_points;

% Send the trajectory to the vehicle
trajectoryWriter.write(trajectory);