numAPs = 3; % Number of APs, must be >=3

snr = 5:2:15; % SNR (dB)
numIterations = 100; % Number of iterations to average the position error

chanBW = 'CBW20'; % Channel bandwidth
numTx = 1; % Number of transmit antennas
numSTS = 1; % Number of space-time streams
numLTFRepetitions = 2; % Number of LTF repetitions

% Configure the HE ranging NDP parameters of the STA
cfgSTA = heRangingConfig;
cfgSTA.ChannelBandwidth = chanBW;
cfgSTA.NumTransmitAntennas = numTx;
cfgSTA.SecureHELTF = верный;
cfgSTA.User {1}.NumSpaceTimeStreams = numSTS;
cfgSTA.User {1}.NumHELTFRepetition = numLTFRepetitions;
cfgSTA.GuardInterval = 1.6;

% Configure the HE ranging NDP parameters of the APs
cfgAP = ячейка (1, numAPs);
for iAP = 1:numAPs
    cfgAP {iAP} = heRangingConfig;
    cfgAP {iAP}.ChannelBandwidth = chanBW;
    cfgAP {iAP}.NumTransmitAntennas = numTx;
    cfgAP {iAP}.SecureHELTF = верный;
    cfgAP {iAP}.User {1}.NumSpaceTimeStreams = numSTS;
    cfgAP {iAP}.User {1}.NumHELTFRepetition = numLTFRepetitions;
    cfgAP {iAP}.GuardInterval = 1.6;
end

% Validate the simulation parameters
if numAPs<3
    error('Number of APs must be greater than or equals to 3');
end
if numIterations<=0
    error('Number of iterations must be a positive integer scalar');
end

% Define constants
speedOfLight = physconst('lightspeed');  % Speed of light (m/s)
subcarrierSpacing = 78.125e3;            % Subcarrier spacing (Hz)
carrierFrequency = 5e9;                  % Carrier frequency (Hz)
if carrierFrequency == 5e9
    delayULDL = 16e-6;                   % Time gap (tDelay) between UL NDP TOA and DL NDP TOD (sec)
elseif carrierFrequency == 2.4e9
    delayULDL = 8e-6; 
end

% Get sample rate
sampleRate = heRangingSampleRate(cfgSTA);

% Get occupied subcarrier indices and OFDM parameters
ofdmInfo = wlanHEOFDMInfo('HE-LTF',chanBW,cfgSTA.GuardInterval);

% Initialize average position error
numSNR = numel(snr); % Number of SNR points
avgPositionError = zeros(1,numSNR);
for isnr = 1:numSNR % Loop over SNR range
    
    % Set random substream index to ensure that each iteration uses a
    % repeatable set of random numbers
    stream = RandStream('combRecursive','Seed',123456);
    stream.Substream = isnr;
    RandStream.setGlobalStream(stream);
    
    % Loop over number of iterations to average the positioning error. For
    % each iteration, generate random positions for the STA and APs and model
    % ranging measurement exchange between each AP and STA.
    positionError = 0;
    linkFailCount = 0;
    for iter = 1:numIterations
        
        % Randomly place the STA and APs in the xy-plane
        [positionSTA, positionAP, distance] = heGeneratePositions(numAPs);
        
        % Range-based delay
        delay = distance/speedOfLight;
        sampleDelay = delay*sampleRate;
        
        % Loop over the number of APs
        distEst = zeros(1,numAPs); % Estimate distance
        for ap = 1:numAPs
            linkType = ["Uplink", "Downlink"];
            
            % Time of departure of UL NDP (t1)
            todUL = randsrc(1,1,0:1e-9:1e-6);
            
            % Loop for both UL and DL transmission
            numLinks = numel(linkType);
            txTime = zeros(1,numLinks);
            for l = 1:numLinks
                if linkType(l) == "Uplink" % STA to AP
                    cfgSTA.UplinkIndication = 1; % For UL
                    % Generate a random secure HE-LTF sequence for the exchange
                    cfgSTA.User{1}.SecureHELTFSequence = dec2hex(randsrc(1,10,(0:15)))';
                    cfg = cfgSTA;
                else % AP to STA
                     % Generate a random secure HE-LTF sequence for the exchange
                    cfgAP{ap}.User{1}.SecureHELTFSequence = dec2hex(randsrc(1,10,(0:15)))';                 
                    cfg = cfgAP{ap}; % For DL
                end
                
                % HE Ranging NDP transmission
                tx = heRangingWaveformGenerator(cfg);
                
                % Introduce time delay in the transmit waveform
                txDelay = heDelaySignal(tx,sampleDelay(ap)); % Add both integer and fractional delay
                
                % Pass the waveform through AWGN channel
                % Account for noise energy in nulls so the SNR is defined
                % per active subcarrier and per transmit antenna
                rx = awgn(txDelay,snr(isnr)-10*log10(ofdmInfo.FFTLength/(numTx*ofdmInfo.NumTones)));
                
                % Perform synchronization by assuming that the transmit
                % side secure HE-LTF sequence is known at the receiver
                [chanEst, integerOffset] = heRangingSynchronize(rx,cfg);
                
                % Estimate the transmission time between UL and DL
                if ~isempty(chanEst) % If packet detection is successful
                    diffAngleChan = diff(unwrap(angle(chanEst))) ;
                    fracDelay = -mean(diffAngleChan(:))/(2*pi*subcarrierSpacing); % Estimate fractional time delay
                    intDelay = integerOffset/sampleRate;  % Estimate integer time delay
                    txTime(l) = intDelay + fracDelay; % Transmission time
                    rangeEstFlag = 1; % If packet detection is successful, enable the range estimation flag 
                else
                    rangeEstFlag = 0; % If packet detection fails, disable the range estimation flag 
                    break;
                end
                    
            end
            if rangeEstFlag % If packet detection is successful
                % TOA of UL waveform (t2)
                toaUL = todUL + txTime(1);
                
                % Time of departure of DL waveform (t3)
                todDL = toaUL + delayULDL;
                
                % TOA DL waveform (t4)
                toaDL = todDL + txTime(2);
                
                % Compute the RTT
                rtt = (toaDL-todUL) - (todDL-toaUL);
                
                % Estimate the distance between the STA and AP
                distEst(ap) = (rtt/2)*speedOfLight;
            else
                break; % If packet detection fails, discard the link between STA and AP
            end
        end
        
        % If range estimation flag is enabled, then estimate the STA
        % position using trilateration and compute the position error.
        if rangeEstFlag
            positionSTAEst = hePositionEstimate(positionAP,distEst);
            positionError = positionError + sqrt(sum((positionSTAEst - positionSTA).^2));
        else
            positionSTAEst = NaN;
            linkFailCount = linkFailCount + 1; % Count the failed links used to average the position error
        end
    end
    if linkFailCount == numIterations % If all the links fail at a given SNR value
        disp(['At SNR = ',num2str(snr(isnr)),' dB, ', 'all ranging NDP packet transmissions failed'])
    else
        avgPositionError(isnr) = positionError/(numIterations-linkFailCount);
        disp(['At SNR = ',num2str(snr(isnr)),' dB, ', 'Positioning error in meters = ', num2str(avgPositionError(isnr))]) 
    end
end

if ~isnan(positionSTAEst)
    % Plot the position of APs and STA
    heRangingPlotPositions(positionAP,positionSTAEst,distEst,snr(isnr),iter);
end

% Plot the position error (meters) against SNR (dB)
figure
semilogy(snr,avgPositionError,'-b*','LineWidth',2,...
    'MarkerEdgeColor','b',...
    'MarkerSize',10)
hold on; grid on
xlabel('SNR (dB)')
ylabel('Estimated position error (meters)')
title('Position accuracy in an 802.11az network')