function [xyz, ptp] = CustomReadBodyTSData(varargin)
% CUSTOMREADBODYTSDATA read function for custom source type with the
% format:
% 
%    data.time
%    data.X
%    data.Y
%    data.Z
%    data.phi
%    data.theta
%    data.psi

% Copyright 1984-2021 The MathWorks, Inc.
%   Standard Mathworks

h = varargin{end};

if (size(varargin,2) == 2)
    t   = varargin{1};

    if isfinite(varargin{1})
        
        xyz(1) = interp1(h.TimeSeriesSource.time, h.TimeSeriesSource.X, t);
        xyz(2) = interp1(h.TimeSeriesSource.time, h.TimeSeriesSource.Y, t);
        xyz(3) = interp1(h.TimeSeriesSource.time, h.TimeSeriesSource.Z, t);

        ptp(1) = interp1(h.TimeSeriesSource.time, h.TimeSeriesSource.phi, t);
        ptp(2) = interp1(h.TimeSeriesSource.time, h.TimeSeriesSource.theta, t);
        ptp(3) = interp1(h.TimeSeriesSource.time, h.TimeSeriesSource.psi, t);
    else
        xyz = h.TimeSeriesSource.time(1); % first time
        ptp = h.TimeSeriesSource.time(end); % last time
    end
else
    %--- return initial data
    xyz = [h.TimeSeriesSource.X(1),...
           h.TimeSeriesSource.Y(1),...
           h.TimeSeriesSource.Z(1)];

    ptp = [h.TimeSeriesSource.phi(1),...
           h.TimeSeriesSource.theta(1),...
           h.TimeSeriesSource.psi(1)];
end


