function [trajectory, trajectoryDates, trajectoryClockError, trajectoryResiduals, trajectoryVelocities] = calculateTrajectory(pFloatDetails, pPressureAndDate, 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
involvedSoundsourceNames = ...
unique(flatten( ...
cellfun( ...
@strsplit, soundsourceCombinations.soundsources, 'UniformOutput', false ...
) ...
));
involvedSoundsources = artoa.data.extractSoundsourcesFromStruct( ...
involvedSoundsourceNames, ...
pSoundsources ...
);
%% Add float drift to TOA
pToaData = artoa.data.addFloatDrift( ...
pToaData, ...
pTrackingParameter.floatOffsetBegin, ...
pTrackingParameter.floatOffsetEnd ...
);
%% 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)
trajectory = false;
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();
for i = 1:length(involvedSoundsourceNames)
trajectoryResiduals.(involvedSoundsourceNames{i}) = [];
end
trajectoryVelocities = struct();
trajectoryVelocities.horizontalTotal = [];
trajectoryVelocities.horizontalLongitude = [];
trajectoryVelocities.horizontalLatitude = [];
trajectoryVelocities.vertical = [];
%% 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 ...
);
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.horvelo( ...
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
[segmentVerticalVelocity] = artoa.data.vervelo( ...
segmentDates, ...
pPressureAndDate{1}(ismember(pPressureAndDate{2}, segmentDates)), ...
pTrackingParameter.interpolationInterval/24*86400, ...
'average' ...
);
trajectoryVelocities.vertical = [trajectoryVelocities.vertical; segmentVerticalVelocity];
end
return