function [trajectory, trajectoryDates, trajectoryClockError, trajectoryResiduals, trajectoryVelocities, trajectoryTimeDivergenceToGps] = calculateTrajectory(pFloatDetails, pPressureAndDate, pSatData, pToaData, pSoundsources, pTrackingParameter, pSoundVelocity, pLeapsecondsMatrix)
%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.
%       pSoundVelocity (double):
%           x by 3 vector, containing the soundspeed values corresponding
%           to the specified soundsources in the tracking parameter ->
%           soundsource combination table.
%       pLeapsecondsMatrix (double):
%           The leapseconds matrix that is specified in the artoa.ini file.


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

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

%% Add float drift to TOA
pToaData = artoa.data.addFloatDrift( ...
        pToaData, ...
        pTrackingParameter.floatOffsetBegin, ...
        pTrackingParameter.floatOffsetEnd ...
);

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

%% 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
    
    % subtract offset (corrective value) of sound source
    soundsourceOffsetCorrectiveValue = ...
        pTrackingParameter.soundsourceOffsets.(currentSoundsource.sourcename);
    currentToa = currentToa - soundsourceOffsetCorrectiveValue(1);
        
    % 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) ...
    );
    %preparedData.(currentSoundsource.sourcename).toaIndex = preparedData.(currentSoundsource.sourcename).toaIndex';
    
    % 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.data.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.data.doppcorr( ...
            preparedData.(currentSoundsource.sourcename).toaDate, ...
            preparedData.(currentSoundsource.sourcename).toa, ...
            pFloatDetails.schedule ...
    );
end

%% Get static data
floatReferenceTime = pFloatDetails.phasereftime;
%startPosition = [pSatData.lat_sat(1) pSatData.lon_sat(1)];
%trackingMethod = lower(pTrackingParameter.trackingMethodString);

trajectory = [];
trajectoryDates = [];
trajectoryClockError = [];
trajectoryResiduals = struct();
trajectoryTimeDivergenceToGps = struct();
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)
    [segmentPositions, segmentDates, segmentClockError, segmentResiduals] = artoa.float.calculateCombinationSegment( ...
            preparedData, ...
            soundsourceCombinations(oCombination, :), ...%trajectory(end, :), ...
            floatReferenceTime, ...
            pSoundVelocity(oCombination, :), ...
            pTrackingParameter.trackingMethodString ...
    );
    
    % 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];
    
    % 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 horizontal velocities
    [ ...
        segmentHorizontalVelocityTotal, ...
        segmentHorizontalVelocityLatitude, ...
        segmentHorizontalVelocityLongitude ...
    ] = artoa.data.calculateHorizontalVelocity( ...
        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 = intersect(pPressureAndDate{2}, segmentDates);
    [segmentVerticalVelocity, segmentVerticalDates] = artoa.data.calculateVerticalVelocity( ...
        verticalVelocityDates, ...
        pPressureAndDate{1}(ismember(pPressureAndDate{2}, verticalVelocityDates)), ...
        pTrackingParameter.interpolationInterval / 24 * 86400, ...
        'average' ...
    );
    trajectoryVelocities.vertical = [trajectoryVelocities.vertical; segmentVerticalVelocity];
    trajectoryVelocities.verticalDates = [trajectoryVelocities.verticalDates; segmentVerticalDates];
    
    % calculate time divergence to gps
    % get all sat dates
    satDates = artoa.convert.dmy2rd(pSatData.day_sat, pSatData.month_sat, pSatData.year_sat);
    % get all dates that intersect with segment
    [~, segmentDatesIndex, satIndex] = intersect(round(segmentDates), round(satDates));
    intersectedSegmentPositions = segmentPositions(segmentDatesIndex, :);
    intersectedSegmentDates = segmentDates(segmentDatesIndex, :);
    satPositions = [pSatData.lat_sat(satIndex), pSatData.lon_sat(satIndex)];
    % get all soundsources of segment
    segmentSoundsources = artoa.data.extractSoundsourcesFromStruct( ...
        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, ...
            pSoundVelocity(oCombination, oSoundsource) ...
        );
        [gpsDivergence, usedIndices] = artoa.data.calculateTimeDivergence( ...
            repmat(currentSoundsource.position, size(satPositions, 1), 1), ...
            satPositions, ...
            pSoundVelocity(oCombination, oSoundsource) ...
        );
        if isempty(usedIndices) || isempty(soundsourceDivergence) || isempty(gpsDivergence)
            continue;
        end
        difference = (soundsourceDivergence(usedIndices) - gpsDivergence);
        trajectoryTimeDivergenceToGps.(currentSoundsource.sourcename) = [ ...
            trajectoryTimeDivergenceToGps.(currentSoundsource.sourcename);
            segmentDates(usedIndices), difference
        ];
    end
end

end