calculate.m

function [trajectory, trajectoryDates, trajectoryClockError, trajectoryResiduals, trajectoryVelocities, trajectoryTimeDivergenceToGps, trajectorySegmentSize, trajectoryVariationResults] = calculate(pFloatDetails, pPressureAndDate, pSatData, pToaData, pSoundsources, pTrackingParameter, pOffsetsParameter, pLeapsecondsMatrix, pOffsetVariations, pStartEndPosition)
%CALCULATETRAJECTORY Calculates the trajectory using the given details.
%   Applies all required corrections to the data and calculates the trajectory
%   segment by segment afterwards.
%
%   The following corrections are applied for every soundsource individually:
%   1. Float drift is being added to TOA
%   2. Adds offsets and dates (sense is to be determined)
%   3. Corrective value of soundsource is being added
%   4. The TOA data is being interpolated
%   5. The TOA data is being matched to the next Rafos date
%   6. Doppler correction is applied if selected
%
%   Parameters:
%       pFloatDetails (struct):
%           The float details that are stored in a .rfb file.
%       pToaData (struct):
%           The toaData object, according to artoa data specification.
%       pSoundsources (struct):
%           All soundsources that are available during the activity of the
%           float.
%       pTrackingParameter (struct):
%           Tracking parameter according to artoa data specification.
%       pLeapsecondsMatrix (double):
%           The leapseconds matrix that is specified in the artoa.ini file.
%       pOffsetVariations (double):
%           The offsetvariations matrix, specified in artoa.ini file.
%       pStartEndPosition (optional) (double):
%           A matrix containing the start and end position as row vectors.

%% Parameter check
if ~exist('pStartEndPosition', 'var')
    pStartEndPosition = false;
end

%% Get required data
soundsourceCombinations = pTrackingParameter.soundsourceCombinations;
uniqueToaDates = unique(pToaData.toaDate);

%% Get all sound sources that are involved
[involvedSoundsources, involvedSoundsourceNames] = ...
    artoa.controller.track.getInvolvedSoundsources();

%% Initialize return variables
trajectory = false;
trajectoryDates = false;
trajectoryClockError = false;
trajectoryResiduals = false;
trajectoryTimeDivergenceToGps = false;
trajectoryVelocities = false;
trajectoryVariationResults = false;

%% Add drift and offsets to soundsources and float
soundsourceStruct = struct();
for i = 1:length(involvedSoundsourceNames)
    soundsourceStruct.(involvedSoundsourceNames{i}) = involvedSoundsources{i};
end
if pOffsetsParameter.useOffsets
    pToaData = artoa.toa.recalculate( ...
        pFloatDetails, ...
        pToaData, ...
        pOffsetsParameter.offsets, ...
        soundsourceStruct ...
    );
    pToaData.toa = pToaData.toa - pToaData.empiricalShift;

end

%% Prepare data for every sound source
preparedData = struct();
for i = 1:length(involvedSoundsources)
    currentSoundsource = involvedSoundsources{i};
    preparedData.(currentSoundsource.sourcename) = struct();
    % get toadata that is applied to soundsource and not deleted
    currentToaSelection = ...
        strcmp(pToaData.soundSource, {currentSoundsource.sourcename}) ...
        & pToaData.status ~= 2;
    
    if ~any(currentToaSelection)
        return
    end
    
    % get toa and date of current sound source
    currentToa = pToaData.toa(currentToaSelection);
    currentToaDate = pToaData.toaDate(currentToaSelection);
    
    
    % get rafos start and enddate of soundsource
    rafosBeginEmission = artoa.convert.dmy2rd( ...
        currentSoundsource.begemis(3), ...
        currentSoundsource.begemis(2), ...
        currentSoundsource.begemis(1) ...
    );
    rafosEndEmission = artoa.convert.dmy2rd( ...
        currentSoundsource.endemis(3), ...
        currentSoundsource.endemis(2), ...
        currentSoundsource.endemis(1) ...
    );

    % get rafos date of float based on cycle (expecting only 1 cycle)
    rafosFloatDateCycleBegin = artoa.convert.dmy2rd( ...
        pFloatDetails.cycle(6), ...
        pFloatDetails.cycle(5), ...
        pFloatDetails.cycle(4) ...
    );
    rafosFloatDateCycleEnd = artoa.convert.dmy2rd( ...
        pFloatDetails.cycle(13), ...
        pFloatDetails.cycle(12), ...
        pFloatDetails.cycle(11) ...
    );

%     % calculate soundsource offsets and dates
%     [offsets, dates] = artoa.data.calculateOffsetsAndDates( ...
%         [rafosFloatDateCycleBegin, rafosFloatDateCycleEnd], ...
%         [rafosBeginEmission, rafosEndEmission], ...
%         currentSoundsource.offset, ...
%         currentSoundsource.drift, ...
%         currentSoundsource.schedule ...
%     );
% 
%     % check overlap between float and sound source deployment and add sound
%     % source offset
%     for oDates = 1:length(dates)
%         kx = find(abs(currentToaDate - dates(oDates)) < 1/2);
%         currentToa(kx) = currentToa(kx) - offsets(oDates);
%     end

    % interpolate toa data
    [ ...
        preparedData.(currentSoundsource.sourcename).toaDate, ...
        preparedData.(currentSoundsource.sourcename).toa, ...
        preparedData.(currentSoundsource.sourcename).toaIndex ...
    ] = artoa.data.interpolateRafosData( ...
        currentToaDate, ...
        currentToa, ...
        pTrackingParameter.interpolationInterval, ...
        pTrackingParameter.gapSize, ...
        lower(pTrackingParameter.interpolationMethodString) ...
    );
    
%     % combine interpolated and original data and sort them
%     preparedData.(currentSoundsource.sourcename).toaDate = ...
%         [preparedData.(currentSoundsource.sourcename).toaDate; currentToaDate];
%     preparedData.(currentSoundsource.sourcename).toa = ...
%         [preparedData.(currentSoundsource.sourcename).toa; currentToa];
    
    % Find the nearest neighbor rafos date
    for oRafosDate = 1:length(preparedData.(currentSoundsource.sourcename).toaDate)
        preparedData.(currentSoundsource.sourcename).toaDate(oRafosDate) = ...
            artoa.data.findNearestNeighbor( ...
                uniqueToaDates, ...
                preparedData.(currentSoundsource.sourcename).toaDate(oRafosDate) ...
            );
    end
    
    % sort toa by date
    [preparedData.(currentSoundsource.sourcename).toaDate, sortIndices] = ...
        sort(preparedData.(currentSoundsource.sourcename).toaDate);
    preparedData.(currentSoundsource.sourcename).toa = ...
        preparedData.(currentSoundsource.sourcename).toa(sortIndices);
    
    % shift toa data
    [ ...
        preparedData.(currentSoundsource.sourcename).toaDate, ...
        preparedData.(currentSoundsource.sourcename).toa ...
    ] = artoa.toa.simulshift( ...
            preparedData.(currentSoundsource.sourcename).toaDate, ...
            preparedData.(currentSoundsource.sourcename).toa, ...
            pFloatDetails.schedule, ...
            pFloatDetails.windowstart, ...
            currentSoundsource.reftime, ...
            pFloatDetails.phasereftime ...
    );

    % save position
    preparedData.(currentSoundsource.sourcename).position = currentSoundsource.position;
    % save float window start
    preparedData.(currentSoundsource.sourcename).floatWindowStart = pFloatDetails.windowstart;
    % save soundsource reference time
    preparedData.(currentSoundsource.sourcename).soundsourceReferenceTime = currentSoundsource.reftime;
    
    % get leapseconds for soundsource
    preparedData.(currentSoundsource.sourcename).leapseconds = artoa.data.calculateLeapseconds( ...
        artoa.convert.dmy2rd(pFloatDetails.launchtime(3), pFloatDetails.launchtime(2), pFloatDetails.launchtime(1)), ...
        artoa.convert.dmy2rd(currentSoundsource.begemis(3), currentSoundsource.begemis(2), currentSoundsource.begemis(1)), ...
        pLeapsecondsMatrix ...
    );
    if isempty(preparedData.(currentSoundsource.sourcename).leapseconds)
        preparedData.(currentSoundsource.sourcename).leapseconds = 0;
    end
    
    if ~pTrackingParameter.dopplerCorrection
        continue
    end
    
    % apply doppler correction if selected
    [preparedData.(currentSoundsource.sourcename).toaDate, preparedData.(currentSoundsource.sourcename).toa] = ...
        artoa.toa.doppcorr( ...
            preparedData.(currentSoundsource.sourcename).toaDate, ...
            preparedData.(currentSoundsource.sourcename).toa, ...
            pFloatDetails.schedule ...
    );
end

%% Get static data
floatReferenceTime = pFloatDetails.phasereftime;

trajectory = [];
trajectoryDates = [];
trajectoryClockError = [];
trajectorySegmentSize = [];
trajectoryResiduals = struct();
trajectoryVariationResults = {};
trajectoryTimeDivergenceToGps = struct();
trajectoryTimeDivergenceToGps.latitude = [];
trajectoryTimeDivergenceToGps.longitude = [];
trajectoryTimeDivergenceToGps.date = [];
for i = 1:length(involvedSoundsourceNames)
    trajectoryResiduals.(involvedSoundsourceNames{i}) = [];
    trajectoryTimeDivergenceToGps.(involvedSoundsourceNames{i}) = [];
end
trajectoryVelocities = struct();
trajectoryVelocities.horizontalTotal = [];
trajectoryVelocities.horizontalLongitude = [];
trajectoryVelocities.horizontalLatitude = [];
trajectoryVelocities.vertical = [];
trajectoryVelocities.verticalDates = [];
% trajectoryTimeDivergenceToGps = struct();
% trajectoryTimeDivergenceToGps.duration = [];
% trajectoryTimeDivergenceToGps.date = [];

%% Calculate for every combination
for oCombination = 1:size(soundsourceCombinations, 1)
    % set reference point to the last position of trajectory if not set
    % manually
    currentCombination = soundsourceCombinations(oCombination, :);
    if isempty(currentCombination.referencePosition{1}) ...
            && isempty(trajectory) ...
            && islogical(pStartEndPosition)
        return;
    end
    
    [ ...
        segmentPositions, ...
        segmentDates, ...
        segmentClockError, ...
        segmentResiduals ...
        ] = artoa.trajectory.calculateCombinationSegment( ...
        preparedData, ...
        currentCombination, ...
        floatReferenceTime, ...
        pOffsetsParameter.offsets(:, 'AppliedSoundspeed'), ...
        currentCombination.backwardTracking(1), ...
        pStartEndPosition ...
    );
    
    % remove NaNs from segment
    validPositions = all(~isnan(segmentPositions), 2);
    segmentPositions = segmentPositions(validPositions, :);
    segmentDates = segmentDates(validPositions);
    segmentClockError = segmentClockError(validPositions);
    segmentResiduals = structfun(@(x) x(validPositions), segmentResiduals, 'UniformOutput', false);% segmentResiduals(validPositions);
    
    if islogical(segmentPositions) && segmentPositions == false
        trajectory = false;
        trajectoryDates = false;
        trajectoryClockError = false;
        trajectoryResiduals = false;
        return
    end
    trajectory = [ ...
        trajectory; ...
        segmentPositions ...
    ];
    trajectoryDates = [trajectoryDates; segmentDates];
    if size(segmentClockError, 2) == 2 && size(trajectoryClockError, 2) == 3
        segmentClockError(:, 3) = NaN(size(segmentClockError, 1), 1);
    end
    trajectoryClockError = [trajectoryClockError; segmentClockError];
    trajectorySegmentSize = [trajectorySegmentSize; size(segmentPositions, 1)];
    
    % store residuals
    for oInvolvedSoundsources = 1:length(involvedSoundsourceNames)
        if isfield(segmentResiduals, involvedSoundsourceNames{oInvolvedSoundsources})
            trajectoryResiduals.(involvedSoundsourceNames{oInvolvedSoundsources}) = [ ...
                trajectoryResiduals.(involvedSoundsourceNames{oInvolvedSoundsources}); ...
                segmentResiduals.(involvedSoundsourceNames{oInvolvedSoundsources}) ...
            ];
        else
            trajectoryResiduals.(involvedSoundsourceNames{oInvolvedSoundsources}) = [ ...
                trajectoryResiduals.(involvedSoundsourceNames{oInvolvedSoundsources}); ...
                NaN(size(segmentPositions, 1), 1) ...
            ];
        end
    end
    
    % CALCULATE VARIATIONS
    if length(strsplit(strtrim(currentCombination.soundsources{1}))) == 2 ...
            && ~artoa.trajectory.isInterpolationMethod(currentCombination.trackingMethod{1}) ...
            && pTrackingParameter.calculateVariations
        [variationResults, ~, ~] = artoa.trajectory.calculateByOffsetVariations( ...
            preparedData, ...
            currentCombination, ...
            floatReferenceTime, ...
            pOffsetsParameter.offsets(:, 'AppliedSoundspeed'), ...
            pTrackingParameter, ...
            pOffsetVariations ...
            );
        trajectoryVariationResults = [trajectoryVariationResults, variationResults];
    end
    
    % calculate horizontal velocities
    [ ...
        segmentHorizontalVelocityTotal, ...
        segmentHorizontalVelocityLatitude, ...
        segmentHorizontalVelocityLongitude ...
    ] = artoa.trajectory.horizontalVelocity( ...
        segmentPositions(:, 1), ...
        segmentPositions(:, 2), ...
        pTrackingParameter.interpolationInterval / 24 * 86400 ...
    );
    trajectoryVelocities.horizontalTotal = [trajectoryVelocities.horizontalTotal; segmentHorizontalVelocityTotal];
    trajectoryVelocities.horizontalLatitude = [trajectoryVelocities.horizontalLatitude; segmentHorizontalVelocityLatitude];
    trajectoryVelocities.horizontalLongitude = [trajectoryVelocities.horizontalLongitude; segmentHorizontalVelocityLongitude];
    
    % calculate vertical velocities
    % get intersecting dates
    %verticalVelocityDates = NaN(size(segmentDates));
    [verticalVelocityDates, ~, indicesDates] = intersect(pPressureAndDate{2}, segmentDates);
    %verticalVelocityDates(indicesDates) = segmentDates(indicesDates);
    [segmentVerticalVelocity, segmentVerticalDates] = artoa.trajectory.verticalVelocity( ...
        verticalVelocityDates, ...
        pPressureAndDate{1}(ismember(pPressureAndDate{2}, verticalVelocityDates)), ...
        pTrackingParameter.interpolationInterval / 24 * 86400, ...
        'average' ...
    );
    trajectoryVelocities.vertical = [trajectoryVelocities.vertical; segmentVerticalVelocity];
    trajectoryVelocities.verticalDates = [trajectoryVelocities.verticalDates; segmentVerticalDates];
    
    if isempty(segmentDates)
        continue;
    end
    
    % Calculate time divergence to gps
    % interpolate the trajectory linearly to have a position every day
    [ ...
        segmentInterpolatedDates, ...
        segmentInterpolatedLat ...
    ] = artoa.data.interpolateRafosData( ...
        segmentDates, ...
        segmentPositions(:, 1), ...
        24, ...
        9999, ...
        'linear' ...
    );
    [ ...
        ~, ...
        segmentInterpolatedLon ...
    ] = artoa.data.interpolateRafosData( ...
        segmentDates, ...
        segmentPositions(:, 2), ...
        24, ...
        9999, ...
        'linear' ...
    );
    segmentInterpolatedPositions = [ segmentInterpolatedLat, segmentInterpolatedLon];
    % get all sat dates and positions
    satDates = artoa.convert.dmy2rd(pSatData.day_sat, pSatData.month_sat, pSatData.year_sat);
    satPositions = [pSatData.lat_sat(:), pSatData.lon_sat(:)];
    tmpIndex = ~isnan(satDates) & ~any(isnan(satPositions), 2);
    % remove nans
    satDates = satDates(tmpIndex);
    satPositions = satPositions(tmpIndex, :);
    clear tmpIndex;
    % get all dates that intersect with segment
    [~, segmentDatesIndex, segmentSatIndex] = intersect(floor(segmentInterpolatedDates), satDates);
    intersectedSegmentPositions = segmentInterpolatedPositions(segmentDatesIndex, :);
    intersectedSegmentDates = floor(segmentInterpolatedDates(segmentDatesIndex, :));
    segmentSatPositions = satPositions(segmentSatIndex, :);
    % get all soundsources of segment
    segmentSoundsources = artoa.soundsources.extract( ...
        strsplit(soundsourceCombinations.soundsources{oCombination}), ...
        pSoundsources ...
    );
    for oSoundsource = 1:length(segmentSoundsources)
        currentSoundsource = segmentSoundsources{oSoundsource};
        % calculate divergence for soundsource position
        [soundsourceDivergence, ~] = artoa.data.calculateTimeDivergence( ...
            repmat(currentSoundsource.position, size(intersectedSegmentPositions, 1), 1), ...
            intersectedSegmentPositions, ...
            pOffsetsParameter.offsets{currentSoundsource.sourcename, 'AppliedSoundspeed'} ...
        );
        [gpsDivergence, usedIndices] = artoa.data.calculateTimeDivergence( ...
            repmat(currentSoundsource.position, size(segmentSatPositions, 1), 1), ...
            segmentSatPositions, ...
            pOffsetsParameter.offsets{currentSoundsource.sourcename, 'AppliedSoundspeed'} ...
        );
        if isempty(usedIndices) || isempty(soundsourceDivergence) || isempty(gpsDivergence)
            continue;
        end
        difference = (soundsourceDivergence(usedIndices) - gpsDivergence(:));
        trajectoryTimeDivergenceToGps.(currentSoundsource.sourcename) = [ ...
            trajectoryTimeDivergenceToGps.(currentSoundsource.sourcename);
            intersectedSegmentDates(usedIndices), difference
        ];
    end
    % save positions and dates that have been interpolated and used for time divergence
    trajectoryTimeDivergenceToGps.latitude = [trajectoryTimeDivergenceToGps.latitude; intersectedSegmentPositions(:, 1)];
    trajectoryTimeDivergenceToGps.longitude = [trajectoryTimeDivergenceToGps.longitude; intersectedSegmentPositions(:, 2)];
    trajectoryTimeDivergenceToGps.date = [trajectoryTimeDivergenceToGps.date; intersectedSegmentDates];
end


end