solve.m

function [a, b, x] = solve(pRfb, pSoundsources, pTrackingParameter, pToaData, pSatData, pAppliedTemperature, pAppliedPressure, pLeapsecondsMatrix)
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here

%% Initialize required variables
%pSoundsources = artoa.controller.getSoundsourcesWithAppliedToa();
satPositions = [pSatData.lat_sat, pSatData.lon_sat];
satDates = artoa.convert.dmy2rd(pSatData.day_sat, pSatData.month_sat, pSatData.year_sat);
toaDates = [];
satToas = [];
satDistances = [];
floatDetails = pRfb.FLOAT;

%% Create table
results = struct();

%% Calculate SAT TOAs for every soundsource and get nearest neighbor measured toa
fnames = fieldnames(pSoundsources);
for i = 1:length(fnames)
    results.(fnames{i}) = table();
    [date, toa] = artoa.toa.predictFromGps( ...
        pRfb, ...
        pSoundsources.(fnames{i}), ...
        struct( ...
            'temperature', pAppliedTemperature, ...
            'pressure', pAppliedPressure, ...
            'method', pTrackingParameter.soundspeedMethodString, ...
            'soundSource', NaN ...
        ), ...
        pLeapsecondsMatrix ...
    );
    results.(fnames{i}).satDate = date;
    results.(fnames{i}).satToa = toa;
    results.(fnames{i}).daysSinceStart = ...
        date ...
        - artoa.convert.dmy2rd( ...
            pSoundsources.(fnames{i}).begemis(3), ...
            pSoundsources.(fnames{i}).begemis(2), ...
            pSoundsources.(fnames{i}).begemis(1) ...
        );
    tmpDistances = [];
    % calculate distances
    for oDistance = 1:length(toa)
        if isnan(toa(oDistance)) | any(isnan(satPositions(oDistance, :)))
            tmpDistances = [tmpDistances; NaN];
            continue;
        end
        tmpDistances = [ ...
            tmpDistances; ...
            artoa.data.calculateGeodist( ...
                satPositions(oDistance, :), ...
                pSoundsources.(fnames{i}).position ...
            ) ...
        ];
    end
    results.(fnames{i}).satDistances = tmpDistances;
    % find nearest neighbor toa
    tmpMeasuredToas = NaN(size(results.(fnames{i}).satDate));
    tmpIndexBelongingToSoso = strcmp(pToaData.soundSource, fnames{i});
    if ~any(tmpIndexBelongingToSoso)
        results.(fnames{i}).measuredToa = tmpMeasuredToas;
        continue;
    end
    tmpIndexBelongingToSoso = tmpIndexBelongingToSoso & (pToaData.status ~= 2);
    tmpToaDate = pToaData.toaDate(tmpIndexBelongingToSoso);
    tmpToa = pToaData.toa(tmpIndexBelongingToSoso);
    
    % interpolate toa data
    [ ...
        tmpToaDate, ...
        tmpToa, ...
        ~ ...
    ] = artoa.data.interpolateRafosData( ...
        tmpToaDate, ...
        tmpToa, ...
        pTrackingParameter.interpolationInterval, ...
        pTrackingParameter.gapSize, ...
        lower(pTrackingParameter.interpolationMethodString) ...
    );
    
    % find nearest neighbor
    for oSatDates = 1:length(results.(fnames{i}).satDate)
        neighborDate = artoa.data.findNearestNeighbor( ...
            tmpToaDate, ...
            results.(fnames{i}).satDate(oSatDates) ...
        );
        tmpMeasuredToas(oSatDates) = tmpToa(tmpToaDate == neighborDate);
    end
    results.(fnames{i}).measuredToa = tmpMeasuredToas;
    clear tmpDistances;
end

%% Construct matrices A and B

rowCount = length(satDates) * length(fnames);

aCore = zeros(rowCount, 2 * length(fnames));
b = zeros(rowCount, 1);
distances = zeros(rowCount, 1);
daysSinceFloatStart = NaN(rowCount, 1);

for i = 1:length(fnames)
    rowIndices = ((i - 1) * length(satDates) + 1):i * length(satDates);
    startColIndex = (2 * (i - 1)) + 1;
    distances(rowIndices, 1) = results.(fnames{i}).satDistances;
    aCore(rowIndices, startColIndex) = 1;
    aCore(rowIndices, startColIndex + 1) = results.(fnames{i}).daysSinceStart;
    b(rowIndices, 1) = results.(fnames{i}).measuredToa;
    daysSinceFloatStart(rowIndices, 1) = ...
        results.(fnames{i}).satDate ...
        - artoa.convert.dmy2rd( ...
            floatDetails.launchtime(3), ...
            floatDetails.launchtime(2), ...
            floatDetails.launchtime(1) ...
        );
    % construct b
%     foundToas = findCorrespondingToas(fnames{i}, results.(fnames{i}).satDate);
end

a = [ ...
    distances, aCore, ones(size(daysSinceFloatStart)), daysSinceFloatStart ...
];

% remove all NaN from matrix
indicesToUse = all(~isnan(a), 2) & all(aCore >= 0, 2) & ~isnan(b);

a = a(indicesToUse, :);
b = b(indicesToUse, :);

x = pinv(a) * b;


%     function [toas] = findCorrespondingToas(soundsourceName, satDate)
%         toas = NaN(size(satDate));
%         for o = 1:length(satDate)
%             if isnan(satDate(o))
%                 continue;
%             end
%             neighbor = artoa.data.findNearestNeighbor(pToaData.date, satDate(o));
%         end
%     end


end