% Set the number of frames to simulate
NFrames = 4;
% Set a range of SNR points to cover both high and low BLER operating
% conditions
SNRIn = [-5 0 5];

sfGap = 8; % Time in subframes between initial and retransmission
tti = 4;   % Time in subframes between different HARQ processes
if tti > sfGap
   error('tti cannot be greater than sfGap');
end

% Number of allocated resource blocks (NPRB) and Transport Block Size (TBS)
% as given by TS 36.101 Table A.8.3-1. Note that TBS must be chosen
% according to TS 36.213 section 14.1.1 and NPRB according to TS 36.213
% section 14.1.1.4C. If an invalid TBS/NPRB is specified, the transmitted
% ModCoding value in the SCI message would be incorrect and this could
% result in the SL-SCH decoding to fail. An invalid NPRB could also result
% in an SCI message and corresponding PSSCH transmission where the
% allocated resources are different from the NPRB specified here.
NPRB = 48;        % Number of allocated resource blocks (NPRB)
TBS = 3496;       % The transport block size
% Define the starting RB for the resource allocations. The initial and
% subsequent transmission can have different allocations
rbStart = [0 0];

% Define the UE configuration parameters
ueConfig = struct('SidelinkMode','V2X');% Release 14 V2X mode
ueConfig.NSLRB = 50;                    % 10MHz bandwidth
ueConfig.DuplexMode = 'FDD';            % Duplex mode
ueConfig.CyclicPrefixSL = 'Normal';     % Cyclic prefix length
ueConfig.Modulation = 'QPSK';           % Symbol modulation ('QPSK','16QAM')

channel = struct;                    % Channel config structure
channel.Seed = 6;                    % Channel seed
channel.NRxAnts = 2;                 % Number of receive antennas
channel.DelayProfile ='EVA';         % Delay profile
channel.DopplerFreq = 500;           % Doppler frequency in Hz
channel.MIMOCorrelation = 'Low';     % Multi-antenna correlation
channel.NTerms = 16;                 % Oscillators used in fading model
channel.ModelType = 'GMEDS';         % Rayleigh fading model type
channel.InitPhase = 'Random';        % Random initial phases
channel.NormalizePathGains = 'On';   % Normalize delay profile power
channel.NormalizeTxAnts = 'On';      % Normalize for transmit antennas
% The channel sampling rate depends on the FFT size used in the OFDM
% modulator. This can be obtained using the function lteSLSCFDMAInfo.
ofdmInfo = lteSLSCFDMAInfo(ueConfig);
channel.SamplingRate = ofdmInfo.SamplingRate;

% Perfect channel estimator flag
perfectChanEstimator = false;

% Define the practical channel estimator configuration structure. Note that
% depending on the channel delay profile and Doppler frequency, the
% operating SNR, PSSCH modulation order and the number of allocated
% resource blocks, different channel estimation parameters may give better
% performance.
cec = struct;                        % Channel estimation config structure
cec.PilotAverage = 'UserDefined';    % Type of pilot symbol averaging
cec.FreqWindow = 27;                 % Frequency window size
cec.TimeWindow = 1;                  % Time window size
cec.InterpType = 'Cubic';            % 2D interpolation type

measureBLERForSCI = true;
measureTputForSLSCH = true;

sciAssumed = false;

displaySimulationInformation = true;

resourcePool = V2XSidelinkResourcePool;
resourcePool.Config.NSLRB = ueConfig.NSLRB;
resourcePool.Config.DuplexMode = ueConfig.DuplexMode;
if displaySimulationInformation
    resourcePool.displayPeriod;
    drawnow;
end

% Initialize variables used in the simulation and analysis
maxThroughputSLSCH = zeros(length(SNRIn),1);
simThroughputSLSCH = zeros(length(SNRIn),1);
simBLERSCI = zeros(length(SNRIn),1);

% Get the number of HARQ processes required
nHARQProcesses = floor(sfGap/tti);
% Calculate the HARQ ID sequence for each of the transmitting subframes
harqProcessSequence = zeros(1,NFrames*10);
for h = 1:nHARQProcesses
    harqProcessSequence(1+tti*(h-1):sfGap:NFrames*10) = h;
end

% Create the partial SCI message with the parameters that have fixed values
% for the simulation. Other parameters that have variable values will be
% set further down in the simulation loop
sciMessage = struct('SCIFormat','Format1');
sciMessage.TimeGap = sfGap;
% Set the PSSCH MCS according to the TBS and modulation scheme
[itbs,modn] = lteMCS(0:28,'PUSCH');
% According to TS 36.213 Section 14.1.1, for IMCS (0 to 28), the modulation
% order is set to Q' = min(4,Qm') where Qm' can be 2, 4 or 6. So change the
% cases where Qm' = 6 ('64QAM') to Qm' = 4 ('16QAM')
modn(strcmpi(modn, '64QAM')) = {'16QAM'};
% Filter valid transport block sizes according to modulation scheme used
possibleTBS = lteTBS(NPRB,itbs);
possibleTBS(~strcmpi(modn,ueConfig.Modulation)) = 0;
% Now set the MCS index according to the TBS
sciMessage.ModCoding = find(possibleTBS==TBS,1) - 1;

% Create new figure for displaying resource allocation, if required
if displaySimulationInformation
    figure;
end

% 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');
for snrIdx = 1:numel(SNRIn)

    % Set the random number generator seed depending on the loop variable
    % to ensure independent random streams
    rng(snrIdx);

    % Initialize the state of all HARQ buffers
    harqProcess = struct('RVIdx',1,'data',[],'decState',[]);
    harqProcesses(1:nHARQProcesses) = harqProcess;
    RVSeq = [0 2];    % RV for initial transmission and retransmission

    % Set up variables for the main loop
    lastOffset = 0;       % Initialize overall frame timing offset
    frameOffset = 0;      % Initialize frame timing offset
    sharedbitTput = [];   % Number of successfully received bits per subframe
    txedTrBlkSizes = [];  % Number of transmitted bits per subframe
    controlErrors = [];   % Number of SCI block errors

    for subframeNo = 0:(NFrames*10-1)
        harqID = harqProcessSequence(subframeNo+1);
        if  harqID == 0
            % If there is no HARQ process transmitting in the current
            % subframe, continue to the next
            continue
        end

        % Update current HARQ process with new transport data and reset the
        % receive buffer if it is the initial transmission. The PSSCH is
        % transmitted twice with a gap of 'sfGap' subframes irrespective of
        % the success of the first transmission
        if harqProcesses(harqID).RVIdx == 1
            harqProcesses(harqID).data = randi([0 1], TBS, 1);
            harqProcesses(harqID).decState = [];
        end
        % Set the RV
        ueConfig.RV = RVSeq(harqProcesses(harqID).RVIdx);
        % Set the subframe number
        ueConfig.NSubframePSSCH = subframeNo;

        % Display run time information
        if displaySimulationInformation
            fprintf('Subframe: %d. HARQ process index: %d. Redundancy version: %d\n',subframeNo,harqID,ueConfig.RV)
        end

        % Channel time for the current subframe
        channel.InitTime = subframeNo/1000;

        % create an empty resource grid
        slgrid = lteSLResourceGrid(ueConfig);

        % Update the transmission config with the starting RB
        startingRB = rbStart(harqProcesses(harqID).RVIdx);
        resourcePool.Config.startRB_Subchannel_r14 = startingRB;

        % Calculate the RIV from the number of allocated subchannels and
        % the starting subchannel
        ueConfig.FirstSubchannelIdx = 0;
        beta = 0;
        if strcmpi(resourcePool.Config.adjacencyPSCCH_PSSCH_r14,'On')
            beta = 2;
        end
        lsubCH = ceil((NPRB+beta)/resourcePool.Config.sizeSubchannel_r14);
        sciMessage.RIV = resourcePool.encodeRIV(lsubCH,ueConfig.FirstSubchannelIdx);

        % Set the retransmission index
        sciMessage.RetransmissionIdx = harqProcesses(harqID).RVIdx - 1;

        % Create the SCI and message bits
        sciConfig = ueConfig;
        sciConfig.PSSCHNSubchannels = resourcePool.Config.numSubchannel_r14;
        [sciMessage, sciBits] = lteSCI(sciConfig,sciMessage);
        % Perform SCI encoding
        [cw,crc] = lteSCIEncode(sciConfig, sciBits);
        % NXID is the 16 bit CRC associated with the PSCCH SCI grant and is
        % used as the PSSCH scrambling identity
        ueConfig.NXID = crc;
        % Store NXID value in 'expectedNXID' so that the receiver won't
        % have to access 'sciMessage' at all
        expectedNXID = ueConfig.NXID;

        % Display the subframes and resource blocks for transmission. Note
        % that the allocations are the same for all HARQ processes and that
        % the plot does not show the subframe offset for HARQ processes
        % other than 1
        if (displaySimulationInformation) && (harqID==1)
            % Specify the additional parameters required for the resource
            % pool display
            sciMessage.FirstSubchannelIdx = ueConfig.FirstSubchannelIdx;
            sciMessage.SLIndex = 0;
            resourcePool.displayPeriod(sciMessage,10);
            drawnow;
        end

        % Create the PSCCH codeword
        pscchSymbols = ltePSCCH(cw);
        % Get the resource allocation for the PSCCH
        [pscchsf,pscchrb] = resourcePool.getPSCCHResources(setfield(sciMessage,'FirstSubchannelIdx',ueConfig.FirstSubchannelIdx)); %#ok<SFLD>
        % Select the resource for this transmission
        sciConfig.PRBSet = pscchrb(:,harqProcesses(harqID).RVIdx);
        % Get the PSCCH symbol indices
        pscchIndices = ltePSCCHIndices(sciConfig);
        % Map the PSCCH symbols on to the grid
        slgrid(pscchIndices) = pscchSymbols;

        % Define a random cyclic shift from the set {0,3,6,9} for each
        % PSCCH transmission
        sciConfig.CyclicShift = randi([0 3])*3;
        % Create the DM-RS symbols and indices
        pscchdrssymbols = ltePSCCHDRS(sciConfig);
        pscchdrsindices = ltePSCCHDRSIndices(sciConfig);
        % Map the PSCCH DRS symbols on to the grid
        slgrid(pscchdrsindices) = pscchdrssymbols;

        % Get the resource allocations for the two transmissions
        [psschsf,psschrb] = resourcePool.getPSSCHResources(setfield(sciMessage,'FirstSubchannelIdx',ueConfig.FirstSubchannelIdx)); %#ok<SFLD>
        % Now select the resource for this transmission
        ueConfig.PRBSet = psschrb(:,harqProcesses(harqID).RVIdx);
        % Calculate the PSSCH resource indices
        [psschIndices,psschinfo] = ltePSSCHIndices(ueConfig);

        % SL-SCH and PSSCH
        cw = lteSLSCH(ueConfig,psschinfo.G,harqProcesses(harqID).data);
        psschSymbols = ltePSSCH(ueConfig,cw);
        slgrid(psschIndices) = psschSymbols;

        % PSSCH DRS and DRS indices
        psschdrssymbols = ltePSSCHDRS(ueConfig);
        psschdrsindices = ltePSSCHDRSIndices(ueConfig);
        slgrid(psschdrsindices) = psschdrssymbols;

        % The SC-FDMA modulation will not use windowing due to the
        % piece-wise nature of the waveform construction
        windowing = 0;
        % perform sidelink SC-FDMA modulation
        [txWaveform,scfdmaInfo] = lteSLSCFDMAModulate(ueConfig,slgrid,windowing);

        % Add 25 sample padding. This is to cover the range of delays
        % expected from channel modeling (a combination of
        % implementation delay and channel delay spread)
        ntxants = size(txWaveform,2);
        txWaveform = [txWaveform; zeros(25, ntxants)]; %#ok<AGROW>

        % Pass data through channel model
        rxNoiselessWaveform = lteFadingChannel(channel,txWaveform);

        % Calculate noise gain
        SNR = 10^((SNRIn(snrIdx))/20);

        % Normalize noise power to take account of sampling rate,
        % which is a function of the IFFT size used in OFDM
        % modulation, and the number of antennas
        N0 = 1/(sqrt(2.0*ntxants*double(ofdmInfo.Nfft))*SNR);

        % Create AWGN
        noise = N0*complex(randn(size(rxNoiselessWaveform)),...
            randn(size(rxNoiselessWaveform)));

        % Add AWGN to the received time domain waveform
        rxWaveform = rxNoiselessWaveform + noise;

        %------------------------------------------------------------------
        %            Receiver
        %------------------------------------------------------------------

        % Define the receiver top-level configuration
        rxConfig = [];
        rxConfig.SidelinkMode = ueConfig.SidelinkMode;
        rxConfig.CyclicPrefixSL = ueConfig.CyclicPrefixSL;
        rxConfig.FirstSubchannelIdx = ueConfig.FirstSubchannelIdx;
        rxConfig.NSLRB = resourcePool.Config.NSLRB;
        rxConfig.DuplexMode = resourcePool.Config.DuplexMode;
        rxConfig.TDDConfig = resourcePool.Config.TDDConfig;
        rxConfig.PSSCHNSubchannels = resourcePool.Config.numSubchannel_r14;
        rxConfig.PSSCHSubchannelSize = resourcePool.Config.sizeSubchannel_r14;
        rxConfig.PSSCHAdjacency = resourcePool.Config.adjacencyPSCCH_PSSCH_r14;
        rxConfig.PSSCHSubchannelPRBStart = resourcePool.Config.startRB_Subchannel_r14;
        rxConfig.NSubframePSSCH = subframeNo; % Set the subframe number

        % If SCI BLER measurement is configured:
        sciDecoded = false;
        if measureBLERForSCI

            % Blind decoding of the control channel
            % We know that the current subframe contains control and data,
            % but do not know the location. Check for the SCI message in
            % the resource pool for all cyclic shifts till it is found or
            % the whole resource pool is searched
            pscchPool = resourcePool.PSCCHResourceBlockPool;
            % Re-arrange the resources into the possible PRB pairs
            pscchPool = reshape(pscchPool,2,numel(pscchPool)/2);

            p = 1;
            while p <= size(pscchPool,2) && ~sciDecoded

                % PSCCH DRS for V2X is transmitted on two consecutive RBs
                rxConfig.PRBSet = pscchPool(:,p);
                for cs = [0 3 6 9]
                    rxConfig.CyclicShift = cs;
                    [frameOffset,corr] = lteSLFrameOffsetPSCCH(rxConfig,rxWaveform);

                    % Perform subframe synchronization
                    if (frameOffset > 25) || (frameOffset < 0)
                        frameOffset = lastOffset;
                    end
                    lastOffset = frameOffset;

                    % Perform SC-FDMA demodulation on the received data to obtain
                    % the resource grid
                    rxSubframe = lteSLSCFDMADemodulate(rxConfig,rxWaveform(1+frameOffset:end,:));

                    % Perform channel estimation
                    if(perfectChanEstimator)
                        [hest,nest] = perfectChannelEstimate(rxConfig,channel,noise,frameOffset);
                    else
                        [hest,nest] = lteSLChannelEstimatePSCCH(rxConfig,cec,rxSubframe);
                    end
                    % Get the PSCCH candidate for the current PRBSet,
                    % extract the corresponding received symbols and
                    % channel estimate and perform equalization
                    pscchCandidate = ltePSCCHIndices(rxConfig);
                    [pscchRx,pscchHest] = lteExtractResources(pscchCandidate,rxSubframe,hest);
                    pscchSymbols = lteEqualizeMMSE(pscchRx,pscchHest,nest);

                    % Demodulate the PSCCH
                    codedsciBits = ltePSCCHDecode(pscchSymbols);

                    % Get the payload size for the Release 14 V2X SCI message
                    scilengthinfo = lteSCIInfo(rxConfig);
                    % Decode the SCI and recover the message
                    [rxinfo,rxSCIErr,rxcrc] = lteSCIDecode(scilengthinfo.Format1,codedsciBits);
                    if  ~rxSCIErr && (rxcrc==expectedNXID)
                        sciMessageRx = lteSCI(rxConfig,rxinfo);
                        rxConfig.NXID = rxcrc;
                        sciDecoded = true;
                        if (displaySimulationInformation)
                            fprintf('   SCI decoded\n');
                        end
                        break;
                    end

                end
                % Increment the pool index
                p = p+1;
            end
            if ~sciDecoded
                 if (displaySimulationInformation)
                    fprintf('   SCI decoding failed\n');
                 end
            end

            % Store values needed to calculate BLER
            controlErrors = [controlErrors ~sciDecoded];  %#ok<AGROW>
        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. Also set the NXID to the expected value
        if (~sciDecoded) && (sciAssumed)
            sciMessageRx = sciMessage;
            rxConfig.NXID = expectedNXID;
        end

        % If SL-SCH throughput measurement is configured and if the SCI was
        % successfully decoded or if SCI decoding success is assumed,
        % perform PSSCH reception and SL-SCH decoding
        if measureTputForSLSCH
            if (sciDecoded || sciAssumed)

                % Get the PSSCH resource allocation from the decoded SCI
                % message and pool configuration
                rxConfig.PRBSet = lteSCIResourceAllocation(rxConfig,sciMessageRx);

                % Perform subframe synchronization
                frameOffset = lteSLFrameOffsetPSSCH(rxConfig,rxWaveform);
                if (frameOffset > 25) || (frameOffset < 0)
                    frameOffset = lastOffset;
                end
                lastOffset = frameOffset;

                % Perform SC-FDMA demodulation on the received data to obtain
                % the resource grid
                rxSubframe = lteSLSCFDMADemodulate(rxConfig,rxWaveform(1+frameOffset:end,:));

                % Perform channel estimation, extract the received PSSCH
                % symbols and the corresponding channel estimate, and
                % perform equalization
                if(perfectChanEstimator)
                    [hest,nest] = perfectChannelEstimate(rxConfig,channel,noise,frameOffset);
                else
                    [hest,nest] = lteSLChannelEstimatePSSCH(rxConfig,cec,rxSubframe);
                end
                [psschRx,psschHest] = lteExtractResources(psschIndices,rxSubframe,hest);
                psschSymbols = lteEqualizeMMSE(psschRx,psschHest,nest);

                % Demodulate the PSSCH
                [~,rxConfig.Modulation] = lteMCS(sciMessageRx.ModCoding,'PUSCH');
                if (strcmpi(rxConfig.Modulation,'64QAM'))
                    rxConfig.Modulation = '16QAM';
                end
                codedSlschBits = ltePSSCHDecode(rxConfig,psschSymbols);

                % Decode the SL-SCH, including soft combining into the
                % receiver buffer and check the CRC
                rxConfig.NTurboDecIts = 5;  % Turbo decoder iterations
                rxConfig.RV = RVSeq(harqProcesses(harqID).RVIdx);
                [~,slschCRC,harqProcesses(harqID).decState] = ...
                        lteSLSCHDecode(rxConfig,TBS,codedSlschBits, ...
                        harqProcesses(harqID).decState);
                slschDecoded = ~slschCRC;

            else
                % If SCI decoding failed, SL-SCH decoding also failed
                slschDecoded = false;
            end
            % Store values needed to calculate throughput
            sharedbitTput = [sharedbitTput TBS*slschDecoded]; %#ok<AGROW>
            txedTrBlkSizes = [txedTrBlkSizes TBS]; %#ok<AGROW>
            if (displaySimulationInformation)
                if  slschDecoded
                    fprintf('   SL-SCH decoded\n');
                else
                    fprintf('   SL-SCH decoding failed\n');
                end
            end

        end

        % Update the RV sequence index for the next transmission
        harqProcesses(harqID).RVIdx = mod(harqProcesses(harqID).RVIdx,size(RVSeq,2))+1;

    end

    if measureBLERForSCI

        % Calculate the SCI BLER
        simBLERSCI(snrIdx) = 100*mean(controlErrors,2);

        % Display the results dynamically in the command window
        fprintf('\nSNR = %.2f dB. SCI BLER(%%) for %d Frame(s) = %.4f %%\n',...
        SNRIn(snrIdx),NFrames,simBLERSCI(snrIdx));
    end

    if measureTputForSLSCH

        % Calculate the maximum and simulated throughput
        maxThroughputSLSCH(snrIdx) = sum(txedTrBlkSizes); % Max possible throughput
        simThroughputSLSCH(snrIdx) = sum(sharedbitTput);  % Simulated throughput

        % Display the results dynamically in the command window
        fprintf('\nSNR = %.2f dB. SL-SCH Throughput for %d Frame(s) = %.4f Mbps\n',...
             SNRIn(snrIdx),NFrames,1e-6*simThroughputSLSCH(snrIdx)/(NFrames*10e-3));
        fprintf('SNR = %.2f dB. SL-SCH Throughput(%%) for %d Frame(s) = %.4f %%\n',...
        SNRIn(snrIdx),NFrames,simThroughputSLSCH(snrIdx)*100/maxThroughputSLSCH(snrIdx));
    end

end

if measureBLERForSCI
    % Plot PSCCH/SCI BLER
    plotResults('SCI', 'BLER',channel,SNRIn,simBLERSCI);
end

if measureTputForSLSCH
    % Plot PSSCH/SL-SCH throughput
    plotResults('SL-SCH', 'Throughput',channel,SNRIn,simThroughputSLSCH*100./maxThroughputSLSCH);
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;
    hest = lteULPerfectChannelEstimate(rxConfig,channel,frameOffset);
    noiseGrid = lteSLSCFDMADemodulate(rxConfig,noise(1+frameOffset:end ,:));
    nest = var(noiseGrid(:));
end

% Plot the SCI BLER or SL-SCH Throughput
function plotResults(chName,simOpt,channel,xvalues,yvalues)
    fadingDescription = sprintf('%s%s',channel.DelayProfile,num2str(channel.DopplerFreq));
    figure;
    plot(xvalues, yvalues,'*-.');
    v = axis;
    axis([v(1) v(2) -10 110])
    title(sprintf('V2X Sidelink %s %s (%%) in %s fading and AWGN',chName,simOpt,fadingDescription));
    xlabel('SNR (dB)');
    ylabel([simOpt ' (%)']);
    grid on;
end