function res = example_trajectory_Pellegrini2009Test()
% example_trajectory_Pellegrini2009Test - Creates test cases for the 
% conformance checking procedure in [1].
%
% Syntax:
%    res = example_trajectory_Pellegrini2009Test
%
% Inputs:
%    -
%
% Outputs:
%    res - true/false 
%
% References:
%    [1] S. B. Liu, H. Roehm, C. Heinzemann, I. Lütkebohle, J. Oehlerking 
%        and M. Althoff, "Provably safe motion of mobile robots in human 
%        environments," 2017 IEEE/RSJ International Conference on 
%        Intelligent Robots and Systems (IROS), 2017, pp. 1351-1357.
%
% Example:
%    ---
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: ---

% Authors:       Matthias Althoff
% Written:       29-June-2023             
% Last update:   ---
% Last revision: ---

% ------------------------------ BEGIN CODE -------------------------------

% set path
path = [CORAROOT filesep 'models' filesep 'testCases' filesep 'pedestrians' filesep 'ewap_dataset' filesep 'seq_hotel' filesep];

% load data
[ped_ID,p_x,p_y,v_x,v_y] = aux_obsmath_import([path,'obsmat.txt']);

% import dataset
samplingTime = 0.4;
Tfinal = 2.0;

% Connect partial trajectories of each pedestrian
pedRecording = [];
nrOfPedestrians = max(ped_ID);
count = 1;
for i=1:nrOfPedestrians
    % measuerement
    y_m = [p_x(ped_ID==i),p_y(ped_ID==i)];
    % state
    x_m = [p_x(ped_ID==i),p_y(ped_ID==i),v_x(ped_ID==i),v_y(ped_ID==i)];
    
    % Create test cases by instantiating TestCase objects
    if ~isempty(y_m) && (i~=314) % ID 314 is an outlier
        pedRecording = [pedRecording; trajectory([],x_m',y_m',[],samplingTime)];  
        count = count+1;
    end
end

%% Maximize test data by considering each sampling instant as initial point
% init cell array of test cases
Pellegrini2009Test = [];
% length of sequential tests
seqTestLength = Tfinal/samplingTime+1;
% create sequential test cases for each pedestrian
for i=1:length(pedRecording)
    additionalTests = sequentialTrajectories(pedRecording(i),seqTestLength);
    Pellegrini2009Test = [Pellegrini2009Test; additionalTests];
end

%%save test suite
% path
path = [CORAROOT filesep 'models' filesep 'testCases' filesep 'pedestrians'];
if ~isfolder(path)
    mkdir([CORAROOT filesep 'models' filesep 'testCases'],'pedestrians');
end
% save
save([path filesep 'Pellegrini2009Test'], 'Pellegrini2009Test');

% example completed
res = true;

end


% Auxiliary functions -----------------------------------------------------

%% Import data
function [ped_ID,p_x,p_y,v_x,v_y] = aux_obsmath_import(filename)

    %% Import data from text file.
    % Script for importing data from the following text file:
    %
    %    Z:\Studiumsunterlagen\Mobile Robot Paper\ewap_dataset\seq_hotel\obsmat.txt
    %
    % To extend the code to different selected data or a different text file,
    % generate a function instead of a script.

    % Auto-generated by MATLAB on 2016/12/14 11:32:57

    %% Format string for each line of text:
    %   column1: double (%f)
    %	column2: double (%f)
    %   column3: double (%f)
    %	column4: double (%f)
    %   column5: double (%f)
    %	column6: double (%f)
    %   column7: double (%f)
    %	column8: double (%f)
    % For more information, see the TEXTSCAN documentation.
    formatSpec = '%16f%16f%16f%16f%16f%16f%16f%f%[^\n\r]';

    %% Open the text file.
    fileID = fopen(filename,'r');

    %% Read columns of data according to format string.
    % This call is based on the structure of the file used to generate this
    % code. If an error occurs for a different file, try regenerating the code
    % from the Import Tool.
    dataArray = textscan(fileID, formatSpec, 'Delimiter', '', 'WhiteSpace', '', 'EmptyValue' ,NaN, 'ReturnOnError', false);

    %% Close the text file.
    fclose(fileID);

    %% Post processing for unimportable data.
    % No unimportable data rules were applied during the import, so no post
    % processing code is included. To generate code which works for
    % unimportable data, select unimportable cells in a file and regenerate the
    % script.

    %% Allocate imported array to column variable names
    ped_ID = dataArray{:, 2};
    p_x = dataArray{:, 3};
    p_y = dataArray{:, 5};
    v_x = dataArray{:, 6};
    v_y = dataArray{:, 8};
end


% ------------------------------ END OF CODE ------------------------------
