calculateCombinationSegment.m

function [segmentPositions, segmentDates, segmentClockError, segmentResiduals] = calculateCombinationSegment(pCorrectedData, pCombinationDetails, pFloatReferenceTime, pSoundVelocity, pTrackingMethod)
%CALCULATECOMBINATIONSEGMENT Combination segment calculation.
%   Using the input data, the function calculates the segment positions,
%   its corresponding dates as well as the clock error (in case of hyperbolic calculation)
%
%   Parameters:
%       pCorrectedData (struct):
%           Data structured by soundsource. The toa is already corrected
%           (e.g. doppler correction etc.)
%       pCombinationDetails (table):
%           The combination table that is produced by the track parameter
%           window.
%       pFloatReferenceTime (double):
%           Vector containing the float reference time, in the
%           format [hour, minute]
%       pSoundVelocity (double):
%           Sound velocity next to the sound sources that are used.
%           If hyperbolic is being used, it needs to be a vector containing
%           at least as many entries as the count of sound sources being
%           used.
%           For all other methods, only one value needs to be set.
%           Unit: [m/s]
%       pTrackingMethod (string/char):
%           Contains the tracking algorithm that should be used.
%           Available methods:
%               least square
%               exclusive least square
%               circular
%               hyperbolic
%
%
%   Returns:
%       segmentPositions (double):
%           The calculated list of segment positions as a column vector.
%       segmentDates (double):
%           The corresponding rafos dates of the segment positions.
%       clockError (double):
%           The estimated clock error.
%       residuals (table):
%           The calculated residuals corresponding to segmentDates for
%           every soundsource.

%% Initialize return variables
segmentPositions = [];
segmentDates = [];
segmentClockError = [];
segmentVelocities = struct();

%% Get sound sources
soundsourceNames = unique(strsplit(strtrim(pCombinationDetails.soundsources{1}), ' '));

%% Get tracking method
trackingMethod = lower(pTrackingMethod);

%% Get start and end point
segmentStart = pCombinationDetails{1, 1};
segmentEnd = pCombinationDetails{1, 2};

%% Get reference position
referencePosition = cellfun(@str2double, strsplit(pCombinationDetails.referencePosition{1}));

%% Filter toa data
intersectedToaDates = pCorrectedData.(soundsourceNames{1}).toaDate;
soundsourcePositions = [pCorrectedData.(soundsourceNames{1}).position];
for i = 2:length(soundsourceNames)
    [intersectedToaDates] = intersect(intersectedToaDates, pCorrectedData.(soundsourceNames{i}).toaDate);
    soundsourcePositions = [soundsourcePositions; pCorrectedData.(soundsourceNames{i}).position];
end

%% Remove all dates that are out of bounds
intersectedToaDates = intersectedToaDates( ...
    intersectedToaDates >= segmentStart ...
    & intersectedToaDates <= segmentEnd ...
);

distanceToSoundsources = {};
%segmentPositions = [pStartposition];
segmentPositions = referencePosition;
for oDates = 1:length(intersectedToaDates)
    currentDateValue = intersectedToaDates(oDates);
    distanceToSoundsources{oDates} = [];
    for i = 1:length(soundsourceNames)
        currentName = soundsourceNames{i};
        selection = pCorrectedData.(currentName).toaDate == currentDateValue;
        filteredValue = pCorrectedData.(currentName).toa(selection);
        %filteredData.(currentName).toa(oDates) = filteredValue(1);
        %filteredValue = pCorrectedData.(currentName).toaDate(selection);
        %filteredData.(currentName).toaDate(oDates) = filteredValue(1);
        
%         relativeToa = ( ...
%             filteredValue(1) ...
%             + (pFloatReferenceTime(1) * 60 + pFloatReferenceTime(2)) * 60 ...
%             - (pCorrectedData.(currentName).soundsourceReferenceTime(1) * 3600 + pCorrectedData.(currentName).soundsourceReferenceTime(2) * 60 - pCorrectedData.(currentName).leapseconds) ...
%         );

        windowStartTime = ...
            pFloatReferenceTime(1) * 3600 ...
            + pFloatReferenceTime(2) * 60 ...
            - pCorrectedData.(currentName).soundsourceReferenceTime(1) * 3600 ...
            - pCorrectedData.(currentName).soundsourceReferenceTime(2) * 60;
        leapseconds = pCorrectedData.(currentName).leapseconds;
        floatWindowStart = pCorrectedData.(currentName).floatWindowStart * 60;
    
        relativeToa = filteredValue(1) ...
            + windowStartTime ...
            + leapseconds ...
            - floatWindowStart;
        
        % calculate distance to the source
        distanceToSoundsources{oDates} = [ ...
            distanceToSoundsources{oDates}; ...
            relativeToa * (pSoundVelocity(i)/1000)
        ];
%         distanceToSoundsources{oDates} = [ ...
%             distanceToSoundsources{oDates}; ...
%             (...
%                 filteredValue(1) ... %filteredData.(currentName).toa(oDates) ...
%                 + (pFloatReferenceTime(1) * 60 + pFloatReferenceTime(2)) * 60 ...
%                 - (pCorrectedData.(currentName).soundsourceReferenceTime(1) * 3600 + pCorrectedData.(currentName).soundsourceReferenceTime(2) * 60 - pCorrectedData.(currentName).leapseconds) ...
%             ) * (pSoundVelocity/1000)
%         ];

    end

end

if strcmp(trackingMethod, 'hyperbolic') && (length(soundsourceNames) < 3)
    trackingMethod = 'circular';
end

switch trackingMethod
    case {'least square', 'exclusive least square'}
        for oDates = 1:length(intersectedToaDates)
            if strcmp(pTrackingMethod, 'exclusive least square') ...
                && (sum(~isnan(distanceToSoundsources{oDates})) ~= length(soundsourcePositions))
                segmentPositions = [segmentPositions; [NaN, NaN]];
                segmentClockError = [segmentClockError; NaN(1, 3)];
                continue;
            end
            segmentPositions = [segmentPositions; ...
                rad2deg(artoa.vendor.ls_converge(segmentPositions(end, :), length(soundsourceNames), soundsourcePositions, distanceToSoundsources{oDates}, pSoundVelocity(1)/1000)) ...
            ];
            segmentClockError = [segmentClockError; NaN(1, 3)];
        end
    case 'circular'
        for oDates = 1:length(intersectedToaDates)
            distances = distanceToSoundsources{oDates};
            segmentPositions = [segmentPositions; ...
                rad2deg(artoa.vendor.xnavai(deg2rad(soundsourcePositions), distances, deg2rad(referencePosition), 2)) ...
            ];
            segmentClockError = [segmentClockError; NaN(1, 3)];
        end
    case 'hyperbolic'
        for oDates = 1:length(intersectedToaDates)
            distances = distanceToSoundsources{oDates};
            if any(isnan(distances))
                segmentPositions = [segmentPositions; [NaN NaN]];
                segmentClockError = [segmentClockError; NaN(1, 3)];
                continue;
            end
            segmentPositions = [segmentPositions; ...
                rad2deg(artoa.vendor.xnavai(deg2rad(soundsourcePositions), distances, deg2rad(referencePosition), 3)) ...
            ];
            currentClockError = NaN(1, 3);
            for oClockError = 1:3
                currentClockError(oClockError) = ...
                    ( ...
                        artoa.data.calculateEllipk4(segmentPositions(end, :), ...
                        deg2rad(soundsourcePositions(oClockError,:))) - distances(oClockError) ...
                    ) / (pSoundVelocity(oClockError)/1000);
            end
            segmentClockError = [segmentClockError; currentClockError];
        end
        
%         if length(unique(sosonr)) < 3;   % check if 3 sources have been specified in track panel
%             fprintf(1,'Hyperbolic tracking not possible: only 2 sound sources specified, using circular tracking\r');
%             pnt=xnavai(sosopos/RAD,dist,refpos/RAD,2); % use circular by default
%         else
%             if (isnan(dist(1)) | isnan(dist(2)) | isnan(dist(3))); % check if data for all 3 sources is availble
%                 tpnt=[tpnt;[NaN,NaN]];
%             else
%                 pnt=xnavai(sosopos/RAD,dist,refpos/RAD,3);
%                 clkerror(1) = (artoa.data.calculateEllipk4(pnt,sosopos(1,:)/RAD)-dist(1))/svsoso(1);
%                 clkerror(2) = (artoa.data.calculateEllipk4(pnt,sosopos(2,:)/RAD)-dist(2))/svsoso(2);
%                 clkerror(3) = (artoa.data.calculateEllipk4(pnt,sosopos(3,:)/RAD)-dist(3))/svsoso(3);
%             end
%         end

end

%% Store toa dates
segmentDates = intersectedToaDates;

%% Remove reference position from trajectory
segmentPositions = segmentPositions(2:end, :);

%% Calculate residuals

% initalize variables
segmentResiduals = struct();
for i = 1:length(soundsourceNames)
    segmentResiduals.(soundsourceNames{i}) = [];
end

% calculate residuals for every timestep
for oDates = 1:length(intersectedToaDates)
    [tmpResiduals, ~] = artoa.float.calculateResiduals( ...
        segmentPositions(1, :), ...
        distanceToSoundsources{oDates}, ...
        soundsourcePositions, ...
        pSoundVelocity ...
    );
    % split up for storing it as table
    for i = 1:length(soundsourceNames)
        segmentResiduals.(soundsourceNames{i}) = [ ...
            segmentResiduals.(soundsourceNames{i}); ...
            tmpResiduals(i) ...
        ];
    end
end


end