% VHT link parameters
cfgVHTTx = wlanVHTConfig( ...
    'ChannelBandwidth',    'CBW80', ...
    'NumTransmitAntennas', 3, ...
    'NumSpaceTimeStreams', 2, ...
    'SpatialMapping',      'Hadamard', ...
    'STBC',                true, ...
    'MCS',                 5, ...
    'GuardInterval',       'Long', ...
    'APEPLength',          1052);

% Propagation channel
numRx = 3;                  % Number of receive antennas
delayProfile = 'Model-C';   % TGac channel delay profile

% Impairments
noisePower = -30;  % Noise power to apply in dBW
cfo = 62e3;        % Carrier frequency offset (Hz)

% Generated waveform parameters
numTxPkt = 1;      % Number of transmitted packets
idleTime = 20e-6;  % Idle time before and after each packet

% Generate waveform
rx = vhtSigRecGenerateWaveform(cfgVHTTx, numRx, ...
    delayProfile, noisePower, cfo, numTxPkt, idleTime);

cfgVHTRx = wlanVHTConfig('ChannelBandwidth', cfgVHTTx.ChannelBandwidth);
idxLSTF = wlanFieldIndices(cfgVHTRx, 'L-STF');
idxLLTF = wlanFieldIndices(cfgVHTRx, 'L-LTF');
idxLSIG = wlanFieldIndices(cfgVHTRx, 'L-SIG');
idxSIGA = wlanFieldIndices(cfgVHTRx, 'VHT-SIG-A');

chanBW = cfgVHTTx.ChannelBandwidth;
sr = wlanSampleRate(cfgVHTTx);

% Setup plots for example
[spectrumAnalyzer, timeScope, constellationDiagram] = vhtSigRecSetupPlots(sr);

% Minimum packet length is 10 OFDM symbols
lstfLen = double(idxLSTF(2)); % Number of samples in L-STF
minPktLen = lstfLen*5;

rxWaveLen = size(rx, 1);

searchOffset = 0; % Offset from start of waveform in samples
while (searchOffset + minPktLen) <= rxWaveLen
    % Packet detection
    pktOffset = wlanPacketDetect(rx, chanBW, searchOffset);

    % Adjust packet offset
    pktOffset = searchOffset + pktOffset;
    if isempty(pktOffset) || (pktOffset + idxLSIG(2) > rxWaveLen)
        error('** No packet detected **');
    end

    % Coarse frequency offset estimation using L-STF
    LSTF = rx(pktOffset + (idxLSTF(1):idxLSTF(2)), :);
    coarseFreqOffset = wlanCoarseCFOEstimate(LSTF, chanBW);

    % Coarse frequency offset compensation
    rx = helperFrequencyOffset(rx,sr,-coarseFreqOffset);

    % Symbol timing synchronization
    LLTFSearchBuffer = rx(pktOffset+(idxLSTF(1):idxLSIG(2)),:);
    pktOffset = pktOffset+wlanSymbolTimingEstimate(LLTFSearchBuffer,chanBW);
    if (pktOffset + minPktLen) > rxWaveLen
        fprintf('** Not enough samples to recover packet **\n\n');
        break;
    end

    % Timing synchronization complete: packet detected
    fprintf('Packet detected at index %d\n\n', pktOffset + 1);

    % Fine frequency offset estimation using L-LTF
    LLTF = rx(pktOffset + (idxLLTF(1):idxLLTF(2)), :);
    fineFreqOffset = wlanFineCFOEstimate(LLTF, chanBW);

    % Fine frequency offset compensation
    rx = helperFrequencyOffset(rx, sr, -fineFreqOffset);

    % Display estimated carrier frequency offset
    cfoCorrection = coarseFreqOffset + fineFreqOffset; % Total CFO
    fprintf('Estimated CFO: %5.1f Hz\n\n', cfoCorrection);

    break; % Front-end processing complete, stop searching for a packet
end

% Channel estimation using L-LTF
LLTF = rx(pktOffset + (idxLLTF(1):idxLLTF(2)), :);
demodLLTF = wlanLLTFDemodulate(LLTF, chanBW);
chanEstLLTF = wlanLLTFChannelEstimate(demodLLTF, chanBW);

% Estimate noise power in non-HT fields
noiseVarNonHT = helperNoiseEstimate(demodLLTF);

% Detect the format of the packet
fmt = wlanFormatDetect(rx(pktOffset + (idxLSIG(1):idxSIGA(2)), :), ...
    chanEstLLTF, noiseVarNonHT, chanBW);
disp([fmt ' format detected']);
if ~strcmp(fmt,'VHT')
    error('** A format other than VHT has been detected **');
end

% Recover L-SIG field bits
disp('Decoding L-SIG... ');
[rxLSIGBits, failCheck, eqLSIGSym] = wlanLSIGRecover(rx(pktOffset + (idxLSIG(1):idxLSIG(2)), :), ...
    chanEstLLTF, noiseVarNonHT, chanBW);

if failCheck % Skip L-STF length of samples and continue searching
    disp('** L-SIG check fail **');
else
    disp('L-SIG check pass');
end

% Measure EVM of L-SIG symbol
EVM = comm.EVM;
EVM.ReferenceSignalSource = 'Estimated from reference constellation';
EVM.ReferenceConstellation = wlanReferenceSymbols('BPSK');
rmsEVM = EVM(eqLSIGSym);
fprintf('L-SIG EVM: %2.2f%% RMS\n', rmsEVM);

% Calculate the receive time and corresponding number of samples in the
% packet
lengthBits = rxLSIGBits(6:17).';
RXTime = ceil((bi2de(double(lengthBits)) + 3)/3) * 4 + 20; % us
numRxSamples = RXTime * 1e-6 * sr; % Number of samples in receive time

fprintf('RXTIME: %dus\n', RXTime);
fprintf('Number of samples in packet: %d\n\n', numRxSamples);

sampleOffset = max((-lstfLen + pktOffset), 1); % First index to plot
sampleSpan = numRxSamples + 2*lstfLen;         % Number of samples to plot
% Plot as much of the packet (and extra samples) as we can
plotIdx = sampleOffset:min(sampleOffset + sampleSpan, rxWaveLen);

% Configure timeScope to display the packet
timeScope.TimeSpan = sampleSpan/sr;
timeScope.TimeDisplayOffset = sampleOffset/sr;
timeScope.YLimits = [0 max(abs(rx(:)))];
timeScope(abs(rx(plotIdx ,:)));

% Display the spectrum of the detected packet
spectrumAnalyzer(rx(pktOffset + (1:numRxSamples), :));

% Recover VHT-SIG-A field bits
disp('Decoding VHT-SIG-A... ');
[rxSIGABits, failCRC, eqSIGASym] = wlanVHTSIGARecover(rx(pktOffset + (idxSIGA(1):idxSIGA(2)), :), ...
    chanEstLLTF, noiseVarNonHT, chanBW);

if failCRC
    disp('** VHT-SIG-A CRC fail **');
else
    disp('VHT-SIG-A CRC pass');
end

% Measure EVM of VHT-SIG-A symbols for BPSK and QBPSK modulation schemes
release(EVM);
EVM.ReferenceConstellation = wlanReferenceSymbols('BPSK');
rmsEVMSym1 = EVM(eqSIGASym(:,1));
release(EVM);
EVM.ReferenceConstellation = wlanReferenceSymbols('QBPSK');
rmsEVMSym2 = EVM(eqSIGASym(:,2));
fprintf('VHT-SIG-A EVM: %2.2f%% RMS\n', mean([rmsEVMSym1 rmsEVMSym2]));

% Create a VHT format configuration object by retrieving packet parameters
% from the decoded L-SIG and VHT-SIG-A bits
cfgVHTRx = helperVHTConfigRecover(rxLSIGBits, rxSIGABits);

% Display the transmission configuration obtained from VHT-SIG-A
vhtSigRecDisplaySIGAInfo(cfgVHTRx);

% Obtain starting and ending indices for VHT-LTF and VHT-Data fields
% using retrieved packet parameters
idxVHTLTF  = wlanFieldIndices(cfgVHTRx, 'VHT-LTF');
idxVHTSIGB = wlanFieldIndices(cfgVHTRx, 'VHT-SIG-B');
idxVHTData = wlanFieldIndices(cfgVHTRx, 'VHT-Data');

% Warn if waveform does not contain whole packet
if (pktOffset + double(idxVHTData(2))) > rxWaveLen
    fprintf('** Not enough samples to recover entire packet **\n\n');
end

% Estimate MIMO channel using VHT-LTF and retrieved packet parameters
demodVHTLTF = wlanVHTLTFDemodulate(rx(pktOffset + (idxVHTLTF(1):idxVHTLTF(2)), :), cfgVHTRx);
chanEstVHTLTF = wlanVHTLTFChannelEstimate(demodVHTLTF, cfgVHTRx);

% Estimate noise power in VHT-SIG-B fields
noiseVarVHT = helperNoiseEstimate(demodLLTF, chanBW, cfgVHTRx.NumSpaceTimeStreams);

% VHT-SIG-B Recover
disp('Decoding VHT-SIG-B...');
[rxSIGBBits, eqSIGBSym] = wlanVHTSIGBRecover(rx(pktOffset + (idxVHTSIGB(1):idxVHTSIGB(2)),:), ...
    chanEstVHTLTF, noiseVarVHT, chanBW);

% Measure EVM of VHT-SIG-B symbol
release(EVM);
EVM.ReferenceConstellation = wlanReferenceSymbols('BPSK');
rmsEVM = EVM(eqSIGBSym);
fprintf('VHT-SIG-B EVM: %2.2f%% RMS\n', rmsEVM);

% Interpret VHT-SIG-B bits to recover the APEP length (rounded up to a
% multiple of four bytes) and generate reference CRC bits
[refSIGBCRC, sigbAPEPLength] = helperInterpretSIGB(rxSIGBBits, chanBW, true);
disp('Decoded VHT-SIG-B contents: ');
fprintf('  APEP Length (rounded up to 4 byte multiple): %d bytes\n\n', sigbAPEPLength);

% Get single stream channel estimate
chanEstSSPilots = vhtSingleStreamChannelEstimate(demodVHTLTF, cfgVHTRx);

% Extract VHT Data samples from the waveform
vhtdata = rx(pktOffset + (idxVHTData(1):idxVHTData(2)), :);

% Estimate the noise power in VHT data field
noiseVarVHT = vhtNoiseEstimate(vhtdata, chanEstSSPilots, cfgVHTRx);

% Recover PSDU bits using retrieved packet parameters and channel
% estimates from VHT-LTF
disp('Decoding VHT Data field...');
[rxPSDU, rxSIGBCRC, eqDataSym] = wlanVHTDataRecover(vhtdata, chanEstVHTLTF, noiseVarVHT, cfgVHTRx);

% Plot equalized constellation for each spatial stream
refConst = wlanReferenceSymbols(cfgVHTRx);
[Nsd, Nsym, Nss] = size(eqDataSym);
eqDataSymPerSS = reshape(eqDataSym, Nsd*Nsym, Nss);
for iss = 1:Nss
    constellationDiagram{iss}.ReferenceConstellation = refConst;
    constellationDiagram{iss}(eqDataSymPerSS(:, iss));
end

% Measure EVM of VHT-Data symbols
release(EVM);
EVM.ReferenceConstellation = refConst;
rmsEVM = EVM(eqDataSym(:));
fprintf('VHT-Data EVM: %2.2f%% RMS\n', rmsEVM);

% Test VHT-SIG-B CRC from service bits within VHT Data against
% reference calculated with VHT-SIG-B bits
if ~isequal(refSIGBCRC, rxSIGBCRC)
    disp('** VHT-SIG-B CRC fail **');
else
    disp('VHT-SIG-B CRC pass');
end

mpduList = wlanAMPDUDeaggregate(rxPSDU, cfgVHTRx);
fprintf('Number of MPDUs present in the A-MPDU: %d\n', numel(mpduList));

for i = 1:numel(mpduList)
    [macCfg, payload, decodeStatus] = wlanMPDUDecode(mpduList{i}, cfgVHTRx, ...
                                                    'DataFormat', 'octets');
    if strcmp(decodeStatus, 'FCSFailed')
        fprintf('** FCS failed for MPDU-%d **\n', i);
    else
        fprintf('FCS passed for MPDU-%d\n', i);
    end
end