nFrames = 1; % Number of 10 ms frames
fc = 3e9;    % Carrier frequency (Hz)

% Configure UE position in xy-coordinate plane
UEPos = [500 -20]; % in meters

% Configure number of gNBs and locate them at random positions in
% xy-coordinate plane
numgNBs = 5;
rng('default');                    % Set RNG state for repeatability
gNBPos = getgNBPositions(numgNBs); % In meters

% Plot UE and gNB positions
plotgNBAndUEPositions(gNBPos,UEPos,1:numgNBs);

cellIds = randperm(1008,numgNBs) - 1;

% Configure carrier properties
carrier = repmat(nrCarrierConfig,1,numgNBs);
for gNBIdx = 1:numgNBs
    carrier(gNBIdx).NCellID = cellIds(gNBIdx);
end
validateCarriers(carrier);

% Slot offsets of different PRS signals
prsSlotOffsets = 0:2:(2*numgNBs - 1);
prsIDs = randperm(4096,numgNBs) - 1;

% Configure PRS properties
prs = nrPRSConfig;
prs.PRSResourceSetPeriod = [10 0];
prs.PRSResourceOffset = 0;
prs.PRSResourceRepetition = 1;
prs.PRSResourceTimeGap = 1;
prs.MutingPattern1 = [];
prs.MutingPattern2 = [];
prs.NumRB = 52;
prs.RBOffset = 0;
prs.CombSize = 12;
prs.NumPRSSymbols = 12;
prs.SymbolStart = 0;
prs = repmat(prs,1,numgNBs);
for gNBIdx = 1:numgNBs
    prs(gNBIdx).PRSResourceOffset = prsSlotOffsets(gNBIdx);
    prs(gNBIdx).NPRSID = prsIDs(gNBIdx);
end

pdsch = nrPDSCHConfig;
pdsch.PRBSet = 0:51;
pdsch.SymbolAllocation = [0 14];
pdsch.DMRS.NumCDMGroupsWithoutData = 1;
pdsch = repmat(pdsch,1,numgNBs);
validateNumLayers(pdsch);

plCfg = nrPathLossConfig;
plCfg.Scenario = 'Uma';
plCfg.BuildingHeight = 5;    % In meters. It is required for 'RMa' scenario
plCfg.StreetWidth = 5;       % In meters. It is required for 'RMa' scenario
plCfg.EnvironmentHeight = 2; % In meters. It is required for 'UMa' and 'UMi' scenarios

los = [true true false true false];
if numel(los) ~= numgNBs
    error('nr5g:InvalidLOSLength',['Length of line of sight flag (' num2str(numel(los)) ...
        ') must be equal to the number of configured gNBs (' num2str(numgNBs) ').']);
end

totSlots = nFrames*carrier(1).SlotsPerFrame;
prsGrid = cell(1,numgNBs);
dataGrid = cell(1,numgNBs);
for slotIdx = 0:totSlots-1
    [carrier(:).NSlot] = deal(slotIdx);
    [prsSym,prsInd] = deal(cell(1,numgNBs));
    for gNBIdx = 1:numgNBs
        % Create an empty resource grid spanning one slot in time domain
        slotGrid = nrResourceGrid(carrier(gNBIdx),1);

        % Generate PRS symbols and indices
        prsSym{gNBIdx} = nrPRS(carrier(gNBIdx),prs(gNBIdx));
        prsInd{gNBIdx} = nrPRSIndices(carrier(gNBIdx),prs(gNBIdx));

        % Map PRS resources to slot grid
        slotGrid(prsInd{gNBIdx}) = prsSym{gNBIdx};
        prsGrid{gNBIdx} = [prsGrid{gNBIdx} slotGrid];
    end
    % Transmit data in slots in which the PRS is not transmitted by any of
    % the gNBs (to control the hearability problem)
    for gNBIdx = 1:numgNBs
        dataSlotGrid = nrResourceGrid(carrier(gNBIdx),1);
        if all(cellfun(@isempty,prsInd))
            % Generate PDSCH indices
            [pdschInd,pdschInfo] = nrPDSCHIndices(carrier(gNBIdx),pdsch(gNBIdx));

            % Generate random data bits for transmission
            data = randi([0 1],pdschInfo.G,1);
            % Generate PDSCH symbols
            pdschSym = nrPDSCH(carrier(gNBIdx),pdsch(gNBIdx),data);

            % Generate demodulation reference signal (DM-RS) indices and symbols
            dmrsInd = nrPDSCHDMRSIndices(carrier(gNBIdx),pdsch(gNBIdx));
            dmrsSym = nrPDSCHDMRS(carrier(gNBIdx),pdsch(gNBIdx));

            % Map PDSCH and its associated DM-RS to slot grid
            dataSlotGrid(pdschInd) = pdschSym;
            dataSlotGrid(dmrsInd) = dmrsSym;
        end
        dataGrid{gNBIdx} = [dataGrid{gNBIdx} dataSlotGrid];
    end
end

% Plot carrier grid
plotGrid(prsGrid,dataGrid);

% Perform OFDM modulation of PRS and data signal at each gNB
txWaveform = cell(1,numgNBs);
for waveIdx = 1:numgNBs
    carrier(waveIdx).NSlot = 0;
    txWaveform{waveIdx} = nrOFDMModulate(carrier(waveIdx),prsGrid{waveIdx} + dataGrid{waveIdx});
end

% Compute OFDM information using first carrier, assuming all carriers are
% at same sampling rate
ofdmInfo = nrOFDMInfo(carrier(1));

speedOfLight = physconst('LightSpeed'); % Speed of light in m/s
sampleDelay = zeros(1,numgNBs);
radius = cell(1,numgNBs);
for gNBIdx = 1:numgNBs
   radius{gNBIdx} = sqrt((gNBPos{gNBIdx}(1) - UEPos(1))^2 + (gNBPos{gNBIdx}(2) - UEPos(2))^2);
   delay = radius{gNBIdx}/speedOfLight;                      % Delay in seconds
   sampleDelay(gNBIdx) = round(delay*ofdmInfo.SampleRate);   % Delay in samples
end

rxWaveform = zeros(length(txWaveform{1}) + max(sampleDelay),1);
rx = cell(1,numgNBs);
for gNBIdx = 1:numgNBs
    % Calculate path loss for each gNB and UE pair
    PLdB = nrPathLoss(plCfg,fc,los(gNBIdx),[gNBPos{gNBIdx}(:);0],[UEPos(:);0]);
    if PLdB < 0 || isnan(PLdB) || isinf(PLdB)
        error('nr5g:invalidPL',"Computed path loss (" + num2str(PLdB) + ...
            ") is invalid. Try changing the UE or gNB positions, or path loss configuration.");
    end
    PL = 10^(PLdB/10);

    % Add delay, pad, and attenuate
    rx{gNBIdx} = [zeros(sampleDelay(gNBIdx),1); txWaveform{gNBIdx}; ...
                zeros(max(sampleDelay)-sampleDelay(gNBIdx),1)]/sqrt(PL);

    % Sum waveforms from all gNBs
    rxWaveform = rxWaveform + rx{gNBIdx};
end

cellsToBeDetected = min(3,numgNBs);
if cellsToBeDetected < 3 || cellsToBeDetected > numgNBs
    error('nr5g:InvalidNumDetgNBs',['The number of detected gNBs (' num2str(cellsToBeDetected) ...
        ') must be greater than or equal to 3 and less than or equal to the total number of gNBs (' num2str(numgNBs) ').']);
end
corr = cell(1,numgNBs);
delayEst = zeros(1,numgNBs);
maxCorr = zeros(1,numgNBs);
for gNBIdx = 1:numgNBs
    [~,mag] = nrTimingEstimate(carrier(gNBIdx),rxWaveform,prsGrid{gNBIdx});
    % Extract correlation data samples spanning about 1/14 ms for normal
    % cyclic prefix and about 1/12 ms for extended cyclic prefix (this
    % truncation is to ignore noisy side lobe peaks in correlation outcome)
    corr{gNBIdx} = mag(1:(ofdmInfo.Nfft*carrier(1).SubcarrierSpacing/15));
    % Delay estimate is at point of maximum correlation
    maxCorr(gNBIdx) = max(corr{gNBIdx});
    delayEst(gNBIdx) = find(corr{gNBIdx} == maxCorr(gNBIdx),1)-1;    
end

% Get detected gNB numbers based on correlation outcome
[~,detectedgNBs] = sort(maxCorr,'descend');
detectedgNBs = detectedgNBs(1:cellsToBeDetected);

% Plot PRS correlation results
plotPRSCorr(carrier,corr,ofdmInfo.SampleRate);

% Compute RSTD values for multilateration or trilateration
rstdVals = getRSTDValues(delayEst,ofdmInfo.SampleRate);

% Plot gNB and UE positions
txCellIDs = [carrier(:).NCellID];
cellIdx = 1;
curveX = {};
curveY = {};
for jj = detectedgNBs(1) % Assuming first detected gNB as reference gNB
    for ii = detectedgNBs(2:end)
        rstd = rstdVals(ii,jj)*speedOfLight; % Delay distance
        % Establish gNBs for which delay distance is applicable by
        % examining detected cell identities
        txi = find(txCellIDs == carrier(ii).NCellID);
        txj = find(txCellIDs == carrier(jj).NCellID);
        if (~isempty(txi) && ~isempty(txj))
            % Get x- and y- coordinates of hyperbola curve and store them
            [x,y] = getRSTDCurve(gNBPos{txi},gNBPos{txj},rstd);
            if isreal(x) && isreal(y)
                curveX{1,cellIdx} = x; %#ok<*SAGROW> 
                curveY{1,cellIdx} = y;

                % Get the gNB numbers corresponding to the current hyperbola
                % curve
                gNBNums{cellIdx} = [jj ii];
                cellIdx = cellIdx + 1;
            else
                warning("Due to the error in timing estimations, " + ...
                    "hyperbola curve cannot be deduced for the combination of gNB" + jj + " and gNB" + ii+". So, ignoring this gNB pair for the estimation of UE position.");
            end
        end
    end
end
numHyperbolaCurves = numel(curveX);
if numHyperbolaCurves < 2
    error('nr5g:InsufficientCurves',"The number of hyperbola curves ("+numHyperbolaCurves+") is not sufficient for the estimation of UE position in 2-D. Try with more number of gNBs.");
end

% Estimate UE position from hyperbola curves using
% |getEstimatedUEPosition|. This function computes the point of
% intersections of all hyperbola curves and does the average of those
% points to get the UE position.
estimatedPos = getEstimatedUEPosition(curveX,curveY);

% Compute positioning estimation error
EstimationErr = norm(UEPos-estimatedPos); % in meters
disp(['Estimated UE Position       : [' num2str(estimatedPos(1)) ' ' num2str(estimatedPos(2)) ']' 13 ...
      'UE Position Estimation Error: ' num2str(EstimationErr) ' meters']);

% Plot UE, gNB positions, and hyperbola curves
gNBsToPlot = unique([gNBNums{:}],'stable');
plotPositionsAndHyperbolaCurves(gNBPos,UEPos,gNBsToPlot,curveX,curveY,gNBNums,estimatedPos);

function gNBPos = getgNBPositions(numgNBs)
%   GNBPOS = getgNBPositions(NUMGNBS) returns a cell array of random
%   positions for gNBs, given the number of gNBs NUMGNBS.

    gNBPos = cell(1,numgNBs);
    for gNBIdx = 1:numgNBs
        % Position gNB randomly within gNBNum*2*pi/numgNBs radian sector
        phi = gNBIdx*2*pi/numgNBs + rand(1,1)*2*pi/(2*numgNBs) - 2*pi/(2*numgNBs);

        % Position gNB randomly between 4000 + (gNBNum*5000/numgNBs) and 5000 +
        % (gNBNum*5000/numgNBs) from UE
        r = randi([0,1000],1,1) + 4000 + (gNBIdx*5000/numgNBs);

        % Convert polar coordinates to Cartesian coordinates
        [x,y] = pol2cart(phi,r);
        gNBPos{gNBIdx} = [x,y];
    end
end

function plotgNBAndUEPositions(gNBPos,UEPos,gNBNums)
%   plotgNBAndUEPositions(GNBPOS,UEPOS,GNBNUMS) plots gNB and UE positions.

    numgNBs = numel(gNBNums);
    colors = getColors(numel(gNBPos));

    figure;
    hold on;
    legendstr = cell(1,numgNBs);

    % Plot position of each gNB
    for gNBIdx = 1:numgNBs
        plot(gNBPos{gNBNums(gNBIdx)}(1),gNBPos{gNBNums(gNBIdx)}(2), ...
            'Color',colors(gNBNums(gNBIdx),:),'Marker','^', ...
            'MarkerSize',11,'LineWidth',2,'MarkerFaceColor',colors(gNBNums(gNBIdx),:));
        legendstr{gNBIdx} = sprintf('gNB%d',gNBIdx);
    end

    % Plot UE position
    plot(UEPos(1),UEPos(2),'ko','MarkerSize',10,'LineWidth',2,'MarkerFaceColor','k');

    axis equal;
    legend([legendstr 'UE']);
    xlabel('X Position (meters)');
    ylabel('Y Position (meters)');

end

function validateCarriers(carrier)
%   validateCarriers(CARRIER) validates the carrier properties of all
%   gNBs.

    % Validate physical layer cell identities
    cellIDs = [carrier(:).NCellID];
    if numel(cellIDs) ~= numel(unique(cellIDs))
        error('nr5g:invalidNCellID','Physical layer cell identities of all of the carriers must be unique.');
    end

    % Validate subcarrier spacings
    scsVals = [carrier(:).SubcarrierSpacing];
    if ~isscalar(unique(scsVals))
        error('nr5g:invalidSCS','Subcarrier spacing values of all of the carriers must be same.');
    end

    % Validate cyclic prefix lengths
    cpVals = {carrier(:).CyclicPrefix};
    if ~all(strcmpi(cpVals{1},cpVals))
        error('nr5g:invalidCP','Cyclic prefix lengths of all of the carriers must be same.');
    end

    % Validate NSizeGrid values
    nSizeGridVals = [carrier(:).NSizeGrid];
    if ~isscalar(unique(nSizeGridVals))
        error('nr5g:invalidNSizeGrid','NSizeGrid of all of the carriers must be same.');
    end

    % Validate NStartGrid values
    nStartGridVals = [carrier(:).NStartGrid];
    if ~isscalar(unique(nStartGridVals))
        error('nr5g:invalidNStartGrid','NStartGrid of all of the carriers must be same.');
    end

end

function validateNumLayers(pdsch)
%   validateNumLayers(PDSCH) validates the number of transmission layers,
%   given the array of physical downlink shared channel configuration
%   objects PDSCH.

numLayers = [pdsch(:).NumLayers];
if ~all(numLayers == 1)
    error('nr5g:invalidNLayers',['The number of transmission layers ' ...
        'configured for the data transmission must be 1.']);
end
end

function plotPRSCorr(carrier,corr,sr)
%   plotPRSCorr(CARRIER,CORR,SR) plots PRS-based correlation for each gNB
%   CORR, given the array of carrier-specific configuration objects CARRIER
%   and the sampling rate SR.

    numgNBs = numel(corr);
    colors = getColors(numgNBs);
    figure;
    hold on;

    % Plot correlation for each gNodeB
    t = (0:length(corr{1}) - 1)/sr;
    legendstr = cell(1,2*numgNBs);
    for gNBIdx = 1:numgNBs
        plot(t,abs(corr{gNBIdx}), ...
            'Color',colors(gNBIdx,:),'LineWidth',2);
        legendstr{gNBIdx} = sprintf('gNB%d (NCellID = %d)', ...
            gNBIdx,carrier(gNBIdx).NCellID);
    end

    % Plot correlation peaks
    for gNBIdx = 1:numgNBs
        c = abs(corr{gNBIdx});
        j = find(c == max(c),1);
        plot(t(j),c(j),'Marker','o','MarkerSize',5, ...
            'Color',colors(gNBIdx,:),'LineWidth',2);
        legendstr{numgNBs+gNBIdx} = '';
    end
    legend(legendstr);
    xlabel('Time (seconds)');
    ylabel('Absolute Value');
    title('PRS Correlations for All gNBs');
end

function rstd = getRSTDValues(toa,sr)
%   RSTD = getRSTDValues(TOA,SR) computes RSTD values, given the time of
%   arrivals TOA and the sampling rate SR.

    % Compute number of samples delay between arrivals from different gNBs
    rstd = zeros(length(toa));
    for jj = 1:length(toa)
        for ii = 1:length(toa)
            rstd(ii,jj) = toa(ii) - toa(jj);
        end
    end

    % Get RSTD values in time
    rstd = rstd./sr;

end

function [x,y] = getRSTDCurve(gNB1,gNB2,rstd)
%   [X,Y] = getRSTDCurve(GNB1,GNB2,RSTD) returns the x- and y-coordinates
%   of a hyperbola equation corresponding to an RSTD value, given the pair
%   of gNB positions gNB1 and gNB2.

    % Calculate vector between two gNBs
    delta = gNB1 - gNB2;

    % Express distance vector in polar form
    [phi,r] = cart2pol(delta(1),delta(2));
    rd = (r+rstd)/2;

    % Compute the hyperbola parameters
    a = (r/2)-rd;         % Vertex
    c = r/2;              % Focus
    b = sqrt(c^2-a^2);    % Co-vertex
    hk = (gNB1 + gNB2)/2;
    mu = -2:1e-3:2;

    % Get x- and y- coordinates of hyperbola equation
    x = (a*cosh(mu)*cos(phi)-b*sinh(mu)*sin(phi)) + hk(1);
    y = (a*cosh(mu)*sin(phi)+b*sinh(mu)*cos(phi)) + hk(2);

end

function plotPositionsAndHyperbolaCurves(gNBPos,UEPos,detgNBNums,curveX,curveY,gNBNums,estPos)
%   plotPositionsAndHyperbolaCurves(GNBPOS,UEPOS,DETGNBNUMS,CURVEX,CURVEY,GNBNUMS,ESTPOS)
%   plots gNB, UE positions, and hyperbola curves by considering these
%   inputs:
%   GNBPOS     - Positions of gNBs
%   UEPOS      - Position of UE
%   DETGNBNUMS - Detected gNB numbers
%   CURVEX     - Cell array having the x-coordinates of hyperbola curves
%   CURVEY     - Cell array having the y-coordinates of hyperbola curves
%   GNBNUMS    - Cell array of gNB numbers corresponding to all hyperbolas
%   ESTPOS     - Estimated UE position

    plotgNBAndUEPositions(gNBPos,UEPos,detgNBNums);
    for curveIdx = 1:numel(curveX)
        curves(curveIdx) = plot(curveX{1,curveIdx},curveY{1,curveIdx}, ...
               '--','LineWidth',1,'Color','k');
    end
    % Add estimated UE position to figure
    plot(estPos(1),estPos(2),'+','MarkerSize',10, ...
        'MarkerFaceColor','#D95319','MarkerEdgeColor','#D95319','LineWidth',2);

    % Add legend
    gNBLegends = cellstr(repmat("gNB",1,numel(detgNBNums)) + ...
        detgNBNums + [" (reference gNB)" repmat("",1,numel(detgNBNums)-1)]);
    legend([gNBLegends {'Actual UE Position'} repmat({''},1,numel(curveX)) {'Estimated UE Position'}]);

    for curveIdx = 1:numel(curves)
        % Create a data tip and add a row to the data tip to display
        % gNBs information for each hyperbola curve
        dt = datatip(curves(curveIdx));
        row = dataTipTextRow("Hyperbola of gNB" + gNBNums{curveIdx}(1) + ...
            " and gNB" + gNBNums{curveIdx}(2),'');
        curves(curveIdx).DataTipTemplate.DataTipRows = [row; curves(curveIdx).DataTipTemplate.DataTipRows];
        % Delete the data tip that is added above
        delete(dt);
    end

end

function colors = getColors(numgNBs)
%    COLORS = getColors(NUMGNBS) returns the RGB triplets COLORS for the
%    plotting purpose, given the number of gNBs NUMGNBS.

    % Form RGB triplets
    if numgNBs <= 10
        colors = [0.8500, 0.3250, 0.0980; 0.9290, 0.6940, 0.1250; 0.4940, 0.1840, 0.5560; ...
                  0.4660, 0.6740, 0.1880; 0.3010, 0.7450, 0.9330; 0.6350, 0.0780, 0.1840; ...
                  0.9290, 0.9, 0.3; 0.9290, 0.5, 0.9; 0.6660, 0.3740, 0.2880;0, 0.4470, 0.7410];
    else
        % Generate 30 more extra RGB triplets than required. It is to skip
        % the gray and white shades as these shades are reserved for the
        % data and no transmissions in carrier grid plot in this example.
        % With this, you can get unique colors for up to 1000 gNBs.
        colors = colorcube(numgNBs+30);
        colors = colors(1:numgNBs,:);
    end
end

function plotGrid(prsGrid,dataGrid)
%   plotGrid(PRSGRID,DATAGRID) plots the carrier grid from all gNBs.

    numgNBs = numel(prsGrid);
    figure()
    mymap = [1 1 1; ...       % White color for background
             0.8 0.8 0.8; ... % Gray color for PDSCH data from all gNBs
             getColors(numgNBs)];
    chpval = 3:numgNBs+2;

    gridWithPRS = zeros(size(prsGrid{1}));
    gridWithData = zeros(size(dataGrid{1}));
    names = cell(1,numgNBs);
    for gNBIdx = 1:numgNBs
        gridWithPRS = gridWithPRS + abs(prsGrid{gNBIdx})*chpval(gNBIdx);
        gridWithData = gridWithData + abs(dataGrid{gNBIdx});
        names{gNBIdx} = ['PRS from gNB' num2str(gNBIdx)];
    end
    names = [{'Data from all gNBs'} names];
    % Replace all zero values of gridWithPRS with proper scaling (value of 1)
    % for white background
    gridWithPRS(gridWithPRS == 0) = 1;
    gridWithData(gridWithData ~=0) = 1;

    % Plot grid
    image(gridWithPRS+gridWithData); % In the resultant grid, 1 represents
                                     % the white background, 2 (constitute
                                     % of 1s from gridWithPRS and
                                     % gridWithData) represents PDSCH, and
                                     % so on
    % Apply colormap
    colormap(mymap);
    axis xy;

    % Generate lines
    L = line(ones(numgNBs+1),ones(numgNBs+1),'LineWidth',8);
    % Index colormap and associate selected colors with lines
    set(L,{'color'},mat2cell(mymap(2:numgNBs+2,:),ones(1,numgNBs+1),3)); % Set the colors

    % Create legend
    legend(names{:});

    % Add title
    title('Carrier Grid Containing PRS and PDSCH from Multiple gNBs');

    % Add labels to x-axis and y-axis
    xlabel('OFDM Symbols');
    ylabel('Subcarriers');
end

function estPos = getEstimatedUEPosition(xCell,yCell)
%   ESTPOS = getEstimatedUEPosition(XCELL,YCELL) returns the x- and y-
%   coordinates of the UE position, given the hyperbolas XCELL and YCELL.

    % Steps involved in this computation:
    % 1. Find closest points between hyperbolic surfaces
    % 2. Linearize surfaces around the points closest to intersection to
    % interpolate actual intersection location

    numCurves = numel(xCell);
    % Make all vectors have equal length
    maxLen = max(cellfun(@length,xCell));
    for curveIdx = 1:numCurves
        xCell{curveIdx} = [xCell{curveIdx} inf(1,maxLen-length(xCell{curveIdx}))];
        yCell{curveIdx} = [yCell{curveIdx} inf(1,maxLen-length(yCell{curveIdx}))];
    end
    tempIdx = 1;
    for idx1 = 1:numCurves-1
        for idx2 = (idx1+1):numCurves
            [firstCurve,secondCurve] = findMinDistanceElements(xCell{idx1},yCell{idx1},xCell{idx2},yCell{idx2});
            for idx3 = 1:numel(firstCurve)
                [x1a,y1a,x1b,y1b] = deal(firstCurve{idx3}(1,1),firstCurve{idx3}(1,2), ...
                                         firstCurve{idx3}(2,1),firstCurve{idx3}(2,2));
                [x2a,y2a,x2b,y2b] = deal(secondCurve{idx3}(1,1),secondCurve{idx3}(1,2), ...
                                         secondCurve{idx3}(2,1),secondCurve{idx3}(2,2));
                a1 = (y1b-y1a)/(x1b-x1a);
                b1 = y1a - a1*x1a;

                a2 = (y2b-y2a)/(x2b-x2a);
                b2 = y2a - a2*x2a;

                xC(tempIdx) = (b2-b1)/(a1-a2);
                yC(tempIdx) = a1*xC(tempIdx) + b1;
                tempIdx = tempIdx+1;
            end
        end
    end
    estPos = [mean(xC) mean(yC)];
end

function [firstCurvePoints,secondCurvePoints] = findMinDistanceElements(xA,yA,xB,yB)
%   [FIRSTCURVEPOINTS,SECONDCURVEPOINTS] = findMinDistanceElements(XA,YA,XB,YB)
%   returns the closest points between the given hyperbolic surfaces.

    distAB1 = zeros(numel(xA),numel(xB));
    for idx1 = 1:numel(xA)
        distAB1(idx1,:) = sqrt((xB-xA(idx1)).^2 + (yB-yA(idx1)).^2);
    end
    [~,rows] = min(distAB1,[],'omitnan');
    [~,col] = min(min(distAB1,[],'omitnan'));

    % Extract the indices of the points on two curves which are up to 5
    % meters apart. This is to identify the multiple intersections between
    % two hyperbola curves.
    [allRows,allCols] = find(distAB1 <= distAB1(rows(col),col)+5);
    diffVec = abs(allRows - allRows(1));
    index = find(diffVec > (min(diffVec) + max(diffVec))/2,1);
    allRows = [allRows(1);allRows(index)];
    allCols = [allCols(1);allCols(index)];

    for idx = 1:numel(allRows)
        firstCurveIndices = allRows(idx);
        secondCurveIndices = allCols(idx);
        x1a = xA(firstCurveIndices);
        y1a = yA(firstCurveIndices);
        x2a = xB(secondCurveIndices);
        y2a = yB(secondCurveIndices);

        % Use subsequent points to create line for linearization
        if firstCurveIndices == numel(rows)
            x1b = xA(firstCurveIndices-1);
            y1b = yA(firstCurveIndices-1);
        else
            x1b = xA(firstCurveIndices+1);
            y1b = yA(firstCurveIndices+1);
        end
        if secondCurveIndices == numel(rows)
            x2b = xB(secondCurveIndices-1);
            y2b = yB(secondCurveIndices-1);
        else
            x2b = xB(secondCurveIndices+1);
            y2b = yB(secondCurveIndices+1);
        end
        firstCurvePoints{idx} = [x1a y1a;x1b y1b]; %#ok<*AGROW> 
        secondCurvePoints{idx} = [x2a y2a;x2b y2b];
    end
end