nPeriods = 5;

channel.DelayProfile = 'EVA';
channel.DopplerFreq = 70.0;
channel.NRxAnts = 2;
channel.MIMOCorrelation = 'Low';
channel.Seed = 1;
channel.ModelType = 'GMEDS';
channel.NTerms = 16;
channel.NormalizeTxAnts = 'On';
channel.NormalizePathGains = 'On';
channel.InitPhase = 'Random';

SNRIn = [-10.0 -5.0 0.0 5.0];

% Perfect channel estimator flag
perfectChanEstimator = false;

cec.PilotAverage = 'UserDefined';
cec.TimeWindow = 15;
cec.FreqWindow = 23;
cec.InterpType = 'linear';

measureBLERForSCI = true;
measureBLERForSLSCH = true;

period = PSCCHPeriod;

% Configure the PSCCH period parameters for reference pool #1-FDD, 5MHz
period.Config = PSCCHPeriod.defaultConfig(1,'5MHz');
% Set the control and data channel cyclic prefix length
period.Config.sc_CP_Len_r12 = 'Normal';
period.Config.data_CP_Len_r12 = 'Normal';

displaySimulationInformation = true;

if (displaySimulationInformation)
    figure;
    period.displayPeriod;
    drawnow;
end

% Number of allocated resource blocks (NPRB) and Transport Block Size (TBS)
% for RMC CD.1 FDD, TS 36.101 Table A.6.5-1
NPRB = 10;
TBS = 872;

% Create UE configuration and SCI message with frequency hopping enabled
ue.NSLRB = period.Config.NSLRB;
sciMessage.FreqHopping = 1;
sciMessage.SCIFormat = 'Format0';
sciMessage = lteSCI(ue,sciMessage);

% Set the hopping bits for Type 2 hopping and the time resource pattern for
% RMC CC.3 FDD, TS 36.101 Table A.6.4-1
sciMessage.Allocation.HoppingBits = 1;
sciMessage.TimeResourcePattern = 8;

% Set the PSSCH MCS according to the TBS
ITBSs = lteMCS(0:28,'PUSCH');
sciMessage.ModCoding = find(lteTBS(NPRB,ITBSs)==TBS,1,'first') - 1;

% Set NSAID value, would be assigned by higher layers
sciMessage.NSAID = 117;

% Store NSAID value in 'expectedNSAID' so that the receiver won't have to
% access 'sciMessage' at all
expectedNSAID = sciMessage.NSAID;

[RIVset,range] = period.getAllowedRIV(sciMessage);
RIVset = RIVset(range(:,1) == NPRB);

% Select the first PSCCH and PSSCH resource
sciMessage.PSCCHResource = 0;
sciMessage.Allocation.RIV = RIVset(1);

% Display the subframes and resource blocks for transmission
if (displaySimulationInformation)
    figure;
    period.displayPeriod(sciMessage);
    drawnow;
end

sciAssumed = true;

controlErrors = zeros(length(SNRIn),nPeriods);
sharedErrors = zeros(length(SNRIn),nPeriods);

controlRxs = ones(length(SNRIn),nPeriods) * 2;
sharedRxs = ones(length(SNRIn),nPeriods) * 4;

% If SCI BLER is not being measured, 'sciAssumed' must be set so that the
% receiver can assume knowledge of the SCI
if (~measureBLERForSCI)
    sciAssumed = true;
end

rng('default');

% Repeat for each PSCCH period
p = 1;
while (p<=nPeriods)

    % Update PSCCH period number
    period.Config.NPSCCHPeriod = p - 1;

    % Choose a random PSCCH resource
    sciMessage.PSCCHResource = randi([0 period.NumPSCCHResource-1]);

    % Choose a random PSSCH resource
    sciMessage.Allocation.RIV = RIVset(randi(length(RIVset)));

    if (displaySimulationInformation)
        fprintf('\nSimulating PSCCH period %d of %d\n',p,nPeriods);
        fprintf('Randomly selected PSCCH resource = %d\n',sciMessage.PSCCHResource);
        fprintf('Randomly selected PSSCH RIV = %d\n',sciMessage.Allocation.RIV);
    end

    % Generate PSCCH period waveform
    txWaveform = period.generateWaveform(sciMessage);

    % Set the sampling rate and init time for frequency-selective fading
    ue.CyclicPrefixSL = period.Config.sc_CP_Len_r12;
    info = lteSLSCFDMAInfo(ue);
    channel.SamplingRate = info.SamplingRate;
    channel.InitTime = (p-1)*period.Config.sc_Period_r12*0.001;
    periodInitTime = channel.InitTime; % Cache the period init time
    % Apply frequency-selective fading
    [fadedWaveform, fadingInfo] = lteFadingChannel(channel,txWaveform);

    % For each SNR point, add AWGN and receive control and shared channels
    for i = 1:length(SNRIn)

        if (displaySimulationInformation && (measureBLERForSCI || measureBLERForSLSCH))
            fprintf('Receiving at %gdB SNR\n',SNRIn(i));
        end

        % Add AWGN
        SNR = 10^(SNRIn(i)/20);
        N = 1/(SNR*sqrt(double(info.Nfft)))/sqrt(2.0);
        noise = N*complex(randn(size(fadedWaveform)), randn(size(fadedWaveform)));
        rxWaveform = fadedWaveform + noise;

        % Create PSCCH receiver configuration
        pscch.SidelinkMode = 'D2D';
        pscch.NSLRB = period.Config.NSLRB;
        pscch.CyclicPrefixSL = period.Config.sc_CP_Len_r12;

        % Establish the number of time-domain samples 'Nt' per subframe and
        % number of SC-FDMA symbols 'L' per subframe
        pscchGridInfo = lteSLSCFDMAInfo(pscch);
        Nt = pscchGridInfo.SamplingRate * 1e-3;
        L = numel(pscchGridInfo.CyclicPrefixLengths);

        % If SCI BLER measurement is configured:
        sciDecoded = false;
        if (measureBLERForSCI)
            % Synchronize and extract each subframe of the PSCCH subframe
            % pool from the received waveform and perform SC-FDMA
            % demodulation. The timing offset used is the offset
            % corresponding to the strongest correlation peak when
            % correlating with the PSCCH DRS for each PRB in the PSCCH
            % subframe pool
            nSubframes = period.Config.sc_Period_r12;
            pscchPoolGrid = repmat(lteSLResourceGrid(pscch),1,nSubframes,size(rxWaveform,2));
            offsetpool = []; % Variable to store the offset for each PSCCH subframe
            for l = period.PSCCHSubframePool
                if(perfectChanEstimator)
                    offset  = hPerfectTimingEstimate(fadingInfo);
                else
                    subframeWaveform = rxWaveform(l*Nt + (1:Nt),:);
                    bestCorr = 0.0;
                    for prb = period.PSCCHResourceBlockPool.'
                        pscch.PRBSet = prb;
                        [thisoffset,thiscorr] = lteSLFrameOffsetPSCCH(pscch,subframeWaveform);
                        % Find the best correlation across all antennas and
                        % store the corresponding offset
                        maxCorr = max(thiscorr(:));
                        if maxCorr > bestCorr
                            bestCorr = maxCorr;
                            offset = max(0,thisoffset); % No negative offset
                        end
                    end
                end
                subframe = lteSLSCFDMADemodulate(pscch,rxWaveform(l*Nt + offset + (1:Nt),:));
                pscchPoolGrid(:,l*L + (1:L),:) = subframe;
                offsetpool = [offsetpool offset]; %#ok<AGROW>
            end

            % Repeat for each PSCCH resource until the SCI is decoded
            pscchResource = 0;
            while (pscchResource < period.NumPSCCHResource && ~sciDecoded)

                % Get the PSCCH subframes 'sf' and PRB allocations 'prb'
                % for the current PSCCH resource
                [sf,prb] = period.getPSCCHResources(pscchResource);

                % Remove any resources that overlap with synchronization
                % subframes. PSCCH will not be transmitted in these
                % subframes
                [~,syncIndex] = intersect(sf,period.SyncSubframes);
                sf(syncIndex) = [];
                prb(syncIndex) = [];

                % Repeat for each PSCCH transmission instance until the SCI
                % is decoded
                tx = 1;
                while (tx <= min(length(sf),2) && ~sciDecoded)

                    % Configure the PSCCH receiver for the PRB allocation
                    pscch.PRBSet = prb(:,tx);

                    % Select the appropriate subframe resource grid from
                    % the PSCCH subframe pool
                    subframe = pscchPoolGrid(:,sf(tx)*L + (1:L),:);

                    % Perform channel estimation
                    if(perfectChanEstimator)
                        % Update the init time to correspond to the current
                        % subframe
                        channel.InitTime = periodInitTime + (sf(tx)*Nt/channel.SamplingRate);
                        [hest,nest] = perfectChannelEstimate(pscch,channel,noise,offsetpool(tx));
                    else
                        [hest,nest] = lteSLChannelEstimatePSCCH(pscch,cec,subframe);
                    end
                    % Extract the received PSCCH symbols and the
                    % corresponding channel estimate, and perform
                    % equalization
                    [pscchIndices,pscchIndicesInfo] = ltePSCCHIndices(pscch);
                    [pscchRx,pscchHest] = lteExtractResources(pscchIndices,subframe,hest);
                    pscchSymbols = lteEqualizeMMSE(pscchRx,pscchHest,nest);

                    % If we are receiving the first PSCCH transmission
                    % instance, reset the receiver buffer
                    if (tx==1)
                        codedSciBits = zeros(pscchIndicesInfo.G,1);
                    end

                    % Demodulate the PSCCH and add the result into the
                    % receiver buffer
                    codedSciBits = codedSciBits + ltePSCCHDecode(pscchSymbols);

                    % Decode the SCI message. If successful (CRC=0), check
                    % the NSAID field of the decoded message against the
                    % transmitted message
                    sciInfo = lteSCIInfo(pscch);
                    [sciBits,sciCRC] = lteSCIDecode(sciInfo.Format0,codedSciBits);
                    if (sciCRC==0)
                        sciMessageRx = lteSCI(pscch,sciBits);
                        if (sciMessageRx.NSAID==expectedNSAID)
                            sciDecoded = true;
                            controlRxs(i,p) = tx;
                            if (displaySimulationInformation)
                                fprintf('   SCI decoded, transmissions combined = %d\n',tx);
                            end
                        else
                            if (displaySimulationInformation)
                                fprintf('   SCI decoded, but NSAID value (%d) did not match expected value (%d)\n',sciMessageRx.NSAID,expectedNSAID);
                            end
                        end
                    end

                    % Increment the PSCCH transmission instance index
                    tx = tx + 1;

                end

                % Increment the PSCCH resource number
                pscchResource = pscchResource + 1;

            end

        end

        % If SCI decoding failed and the SCI is assumed for SL-SCH
        % decoding, set the decoded SCI message equal to the transmitted
        % SCI message
        if (~sciDecoded)
            if (displaySimulationInformation && measureBLERForSCI)
                fprintf('   SCI decoding failed\n');
            end
            if (sciAssumed)
                sciMessageRx = sciMessage;
            end
        end

        % If SL-SCH BLER measurement is configured and if the SCI was
        % successfully decoded or if SCI decoding success is assumed,
        % perform PSSCH reception and SL-SCH decoding
        slschDecoded = false;
        if (measureBLERForSLSCH && (sciDecoded || sciAssumed))

            % Create PSSCH receiver configuration
            pssch.SidelinkMode = 'D2D';
            pssch.NSLRB = period.Config.NSLRB;
            pssch.CyclicPrefixSL = period.Config.data_CP_Len_r12;
            pssch.NTurboDecIts = 5;

            % Establish the number of time-domain samples 'Nt' per subframe
            psschGridInfo = lteSLSCFDMAInfo(pssch);
            Nt = psschGridInfo.SamplingRate * 1e-3;

            % Get the PSSCH subframes 'sf', PRB allocations 'prb' and PSSCH
            % subframe numbers 'nsf' for the configuration given by the
            % decoded SCI message. 'sf' are the subframe numbers within the
            % SC period, 'nsf' are the subframe numbers within the PSSCH
            % subframe pool
            [sf,prb,nsf] = period.getPSSCHResources(sciMessageRx);

            % Create the redundancy version index sequence (IRV)
            % corresponding to the PSSCH transmission instances. This must
            % be computed "up front" prior to the removal of any
            % synchronization subframes in the next step, otherwise the RV
            % sequence is lost
            IRV = 0:length(sf)-1;

            % Remove any resources that overlap with synchronization
            % subframes. PSSCH will not be transmitted in these subframes
            [~,syncIndex] = intersect(sf,period.SyncSubframes);
            sf(syncIndex) = [];
            prb(:,syncIndex) = [];
            nsf(syncIndex) = [];
            IRV(syncIndex) = [];

            % Configure the PSSCH receiver NSAID value and modulation from
            % the decoded SCI message, and establish the SL-SCH transport
            % block size (TBS)
            pssch.NSAID = sciMessageRx.NSAID;
            [ITBS,modulation] = lteMCS(sciMessageRx.ModCoding,'PUSCH');
            if (strcmpi(modulation,'64QAM'))
                modulation = '16QAM';
            end
            pssch.Modulation = modulation;
            NPRB = max(size(prb,1),1);
            TBS = lteTBS(NPRB,ITBS);

            % Repeat for each PSSCH transmission instance until the SL-SCH
            % is decoded
            tx = 1;
            slschDecState = [];
            rvsequence = [0 2 3 1];
            while (tx <= min(length(sf),4) && ~slschDecoded)

                % Configure the PSSCH receiver for the PSSCH subframe
                % number, PRB allocation and redundancy version (RV). If
                % the RV index is zero (corresponding to the first PSSCH
                % transmission instance in a block of 4 transmissions),
                % reset the receiver buffer
                pssch.NSubframePSSCH = nsf(tx);
                pssch.PRBSet = prb(:,tx);
                pssch.RV = rvsequence(mod(IRV(tx),4)+1);
                if (IRV(tx)==0)
                    slschDecState = [];
                end

                % Perform timing synchronization, extract the appropriate
                % subframe of the received waveform, and perform SC-FDMA
                % demodulation
                if(perfectChanEstimator)
                    offset  = hPerfectTimingEstimate(fadingInfo);
                else
                    offset = lteSLFrameOffsetPSSCH(pssch,rxWaveform(sf(tx)*Nt + (1:Nt),:));
                    offset = max(0,offset); % No negative offset
                end
                subframeWaveform = rxWaveform(sf(tx)*Nt + offset + (1:Nt),:);
                subframe = lteSLSCFDMADemodulate(pssch,subframeWaveform);

                % Perform channel estimation, extract the received PSSCH
                % symbols and the corresponding channel estimate, and
                % perform equalization
                if(perfectChanEstimator)
                    % Update the init time to correspond to the current
                    % subframe
                    channel.InitTime = periodInitTime + (sf(tx)*Nt/channel.SamplingRate);
                    [hest,nest] = perfectChannelEstimate(pssch,channel,noise,offset);
                else
                    [hest,nest] = lteSLChannelEstimatePSSCH(pssch,cec,subframe);
                end
                psschIndices = ltePSSCHIndices(pssch);
                [psschRx,psschHest] = lteExtractResources(psschIndices,subframe,hest);
                psschSymbols = lteEqualizeMMSE(psschRx,psschHest,nest);

                % Demodulate the PSSCH
                codedSlschBits = ltePSSCHDecode(pssch,psschSymbols);

                % Decode the SL-SCH including soft combining into the
                % receiver buffer and check the CRC
                [~,slschCRC,slschDecState] = lteSLSCHDecode(pssch,TBS,codedSlschBits,slschDecState);
                if (slschCRC==0)
                    slschDecoded = true;
                    sharedRxs(i,p) = tx;
                    if (displaySimulationInformation)
                        fprintf('   SL-SCH decoded, transmissions combined = %d\n',tx);
                    end
                end

                % Increment the PSSCH transmission instance index
                tx = tx + 1;

            end

        end

        % Record control and shared channel errors
        controlErrors(i,p) = ~sciDecoded;
        sharedErrors(i,p) = ~slschDecoded;

        if (displaySimulationInformation && ~slschDecoded && measureBLERForSLSCH)
            fprintf('   SL-SCH decoding failed\n');
        end

    end

    % Update the PSCCH period number
    p = p + 1;

end

if (measureBLERForSCI)
    controlBLER = 100*mean(controlErrors,2);
    controlRxsAvg = mean(controlRxs,2);
else
    controlBLER = [];
    controlRxsAvg = [];
end

if (measureBLERForSLSCH)
    sharedBLER = 100*mean(sharedErrors,2);
    sharedRxsAvg = mean(sharedRxs,2);
else
    sharedBLER = [];
    sharedRxsAvg = [];
end

if (measureBLERForSCI || measureBLERForSLSCH)
    plotResults(channel,SNRIn,controlBLER,sharedBLER,controlRxsAvg,sharedRxsAvg);
end

% Calculate the perfect channel estimate and the noise estimate
function [hest,nest] = perfectChannelEstimate(rxConfig,channel,noise,frameOffset)
    % Use the uplink perfect channel estimator to calculate
    % the sidelink estimate as both use SC-FDMA
    rxConfig.NTxAnts = 1;
    rxConfig.NULRB = rxConfig.NSLRB;
    rxConfig.CyclicPrefixUL = rxConfig.CyclicPrefixSL;
    rxConfig.TotSubframes = 1;
    hest = lteULPerfectChannelEstimate(rxConfig,channel,frameOffset);
    noiseGrid = lteSLSCFDMADemodulate(rxConfig,noise(1+frameOffset:end,:));
    nest = var(noiseGrid(:));
end

function plotResults(channel,SNRIn,controlBLER,sharedBLER,controlRxsAvg,sharedRxsAvg)

    fadingDescription = sprintf('%s%s',channel.DelayProfile,num2str(channel.DopplerFreq));

    figure;
    hold on;
    legends = {};
    if (~isempty(controlBLER))
        plot(SNRIn,controlBLER,'ro-');
        legends = [legends 'PSCCH/SCI'];
    end
    if (~isempty(sharedBLER))
        plot(SNRIn,sharedBLER,'bx-');
        legends = [legends 'PSSCH/SL-SCH'];
    end
    legend(legends);
    title(sprintf('Sidelink BLER (%%) in %s fading and AWGN',fadingDescription));
    ylabel('BLER (%)');
    xlabel('SNR (dB)');
    axis([SNRIn(1)-1 SNRIn(end)+1 -10 110]);
    grid on

    figure;
    hold on;
    if (~isempty(controlBLER))
        plot(SNRIn,controlRxsAvg,'ro-');
    end
    if (~isempty(sharedBLER))
        plot(SNRIn,sharedRxsAvg,'bx-');
    end
    legend(legends);
    title(sprintf('Average number of sidelink transmissions\ncombined in %s fading and AWGN',fadingDescription));
    ylabel('Average number of transmissions');
    xlabel('SNR (dB)');
    axis([SNRIn(1)-1 SNRIn(end)+1 0.75 4.25])
    grid on

end