SingleSource.m

function [positions, clockError] = SingleSource(pData, pSoundsourcePositions, pCombinationDetails, pStartEndPosition)
%SINGLESOURCE Estimates the trajectory by using only one soundsource.
%
%   Parameters:
%       pData (struct):
%           Data structured by soundsource. The toa is already corrected
%           (e.g. doppler correction etc.)
%       pSoundsourcePositions (struct):
%           The soundsource positions [deg].
%       pCombinationDetails (table):
%           Contains all information of the current soundsource
%           combination.
%       pStartEndPosition (double, logical):
%           Set to false if it has not been passed by artoa. Contains the
%           desired start and end position as a matrix of row vectors.
%
%
%   Returns:
%       positions (double):
%           The calculated list of segment positions as a X x 2 column vector.
%       clockError (double):
%           The estimated clock error.


%% Check input
referencePositionStart = cellfun( ...
    @str2double, ...
    strsplit(pCombinationDetails.referencePosition{1}) ...
    );
referencePositionEnd = cellfun( ...
    @str2double, ...
    strsplit(pCombinationDetails.referencePositionEnd{1}) ...
    );

if any(isnan(referencePositionStart)) || any(isnan(referencePositionEnd))
    error([mfilename ': SingleSource algorithm requires BOTH (Reference) Positions to be set!']);
end

%% Get required data
% use only the first soundsource, even if there are more available
soundsourceName = strsplit(pCombinationDetails.soundsources{1});
soundsourceName = soundsourceName{1};

%% Prepare data for calculations
% get a date vector (does not matter which one)
dates = cell2mat(pData.(soundsourceName).date);
distances = cell2mat(pData.(soundsourceName).distance);
soundsourcePosition = pSoundsourcePositions.(soundsourceName);

% initialize return variables
positions = [ ...
    referencePositionStart; ...
    ];
%endPosition = str2num(pCombinationDetails.referencePositionEnd{:});
%clockError = [];

%% Algorithm
dateLength = length(dates);  % number of points to calculate trajectory for
% time between position fixes   %!!!  needs generalization accordind to float's RAFOS schedule
timeStep = diff(dates(1:2)) * 86400;
averageHorizontalVelocity = ( ...
    distance(str2num(pCombinationDetails.referencePositionEnd{:}), ...
    str2num(pCombinationDetails.referencePosition{:})) * 60 * 1852) ...
    / ( ...
        (pCombinationDetails.combinationEnd - pCombinationDetails.combinationBegin) ...
        * 86400 ...
    );   % in meter per second

% calculate main loop values
maxJ = length(averageHorizontalVelocity:.05:5 * averageHorizontalVelocity);
j = 0;
% initialize container for calculated positions
positions_calculated(1:maxJ,:,1:2) = NaN*ones(maxJ,dateLength,2);

for horizontalVelocity = averageHorizontalVelocity:.05:5 * averageHorizontalVelocity
    j = j + 1;

    for i = 1:dateLength
        % get current distance [km] according to current toa
        currentDistance = distances(i);

        if i == 1
            lastFloatPosition(1) = referencePositionStart(1);
            lastFloatPosition(2) = referencePositionStart(2);
            [~, oldbearing] = distance( ...
                referencePositionStart(1), ...
                referencePositionStart(2), ...
                referencePositionEnd(1), ...
                referencePositionEnd(2) ...
                );
        end

        [lat, lon] = scxsc( ...
            soundsourcePosition(1), ...
            soundsourcePosition(2), ...
            km2deg(currentDistance), ...
            lastFloatPosition(1), ...
            lastFloatPosition(2), ...
            km2deg(horizontalVelocity * timeStep / 1000) ...
            );
         %% So we have usually 2 solution, which one to select? --> the one closer towards the final point
         
         [helper, newbearing] = distance( ...
             lastFloatPosition(1), ...
             lastFloatPosition(2), ...
             lat, ...
             lon ...
             );
         %! besser: der punkt der sich auf der Verbindungsachse zwischen
         % A und B in Richtung B bewegt
    
        
        if ~isnan(helper(1)) %  intersection found
            helper = abs(newbearing - oldbearing);
            helperindi = find(helper == min(helper));
            oldbearing = newbearing(helperindi);

            lastFloatPosition(1) = lat(helperindi);
            lastFloatPosition(2) = lon(helperindi);
            positions_calculated(j,i,1) = lastFloatPosition(1);
            positions_calculated(j,i,2) = lastFloatPosition(2);
        else
            break;
        end
    end
end

validdata = find(~isnan(positions_calculated(:, end, 1)));
helper = distance( ...
    referencePositionEnd(1), ...
    referencePositionEnd(2), ...
    positions_calculated(validdata, end, 1), ...
    positions_calculated(validdata, end, 2) ...
    );
jindi = find(helper == min(helper));

positions(1:dateLength, 1) = positions_calculated(validdata(jindi), :, 1)';
positions(1:dateLength, 2) = positions_calculated(validdata(jindi), :, 2)';

% ich vermute mal das dies der Unterschied gemessene toa zu der
% "von der errechneten Position abgeleiteten toa".   der ist null da wir
% ihn of 0 zwingen.
clockError = zeros(length(positions_calculated), 1);

end