function [waveformDiffFreq,varargout] = helperBLEModulationTestMeasurements(inputWaveform,txTestID,testParams)
%helperBLEModulationTestMeasurements returns BLE test measurements and FM
%demodulated waveform for each BLE transmitter test corresponding to
%modulation characteristics, carrier frequency offset and drift.
%   [WAVEFORMDIFFFREQ,VARARGOUT] =
%   helperBLEModulationTestMeasurements(INPUTWAVEFORM,TXTESTID,TESTPARAMS)
%   returns FM demodulated waveform, WAVEFORMDIFFFREQ and test measurements
%   as part of VARARGOUT.
%   
%   INPUTWAVEFORM is a complex valued column vector of size Ns-by-1, where Ns
%   represents the number of time-domain samples.
%   
%   TXTESTID is a character vector or string specifying the transmitter test
%   ID. It must be one of the following:
%   {'RF-PHY/TRM/BV-09-C','RF-PHY/TRM/BV-11-C',
%    'RF-PHY/TRM/BV-13-C','RF-PHY/TRM/BV-06-C',
%    'RF-PHY/TRM/BV-12-C','RF-PHY/TRM/BV-14-C'}.
%   
%   TESTPARAMS, a structure containing the following test parameters:
%   SPS             - Samples per symbol
%   SYMBOLRATE      - Symbol rate
%   BITSPERBYTE     - Bits per byte
%   PHYMODE         - Physical layer transmission mode
%   TESTSEQIDS      - Type of test sequence(s)
%   TESTSEQID       - Test sequence
%   NONPDULEN       - Length of BLE packet excluding PDU
%   DISCARDSTARTLEN - Number of bits to discard at the start of the packet
%   DISCARDENDLEN   - Number of bits to discard at the end of the packet
%   PREAMBLELEN     - Length of the preamble
%   CODINGFACTOR    - FEC Coding scheme
%   NUMOFTESTSEQS   - Number of test sequence
%   SAMPLERATE      - Sampling rate
%   
%   WAVEFORMDIFFFREQ, is a column vector containing FM demodulated waveform.
%   
%   VARARGOUT, contains test measurements specific to test ID, TXTESTID.
%   For test IDs: {'RF-PHY/TRM/BV-09-C','RF-PHY/TRM/BV-11-C','RF-PHY/TRM/BV-13-C'}
%             VARARGOUT{1}, Measured average frequency deviation(s) for the
%             test sequences.
%             VARARGOUT{2}, Measured average sequence center frequencies
%             for the test sequences.
%             VARARGOUT{3}: Measured maximum frequency deviation for the
%             second test sequence for 'RF-PHY/TRM/BV-09-C', 
%             'RF-PHY/TRM/BV-11-C' and first test sequence for
%             'RF-PHY/TRM/BV-13-C'.
%   For test IDs: {'RF-PHY/TRM/BV-06-C','RF-PHY/TRM/BV-12-C','RF-PHY/TRM/BV-14-C'}
%             VARARGOUT{1}, Measured initial frequency offset.
%             VARARGOUT{2}, Measured carrier frequency drift.

%   Copyright 2019-2020 The MathWorks, Inc.

discardStartLen = testParams.discardStartLen;
sps = testParams.sps;
discardEndLen = testParams.discardEndLen;
sampleRate = testParams.sampleRate;
waveformDiffFreq = zeros(size(inputWaveform,1)-1,size(inputWaveform,2));
for ind = 1:size(inputWaveform,2)
    % Waveform after discarding bits
    trimWaveform = inputWaveform(1:end,ind);
    % Obtain instantaneous frequencies for each sample
    waveformDiffFreqNorm = diff(unwrap(angle(trimWaveform)))./(2*pi);
    waveformDiffFreq(:,ind) = waveformDiffFreqNorm*sampleRate;
end

if any(strcmp(txTestID,{'RF-PHY/TRM/BV-09-C','RF-PHY/TRM/BV-11-C','RF-PHY/TRM/BV-13-C'}))
    % Modulation characteristics tests
    waveformDiffFreq = waveformDiffFreq(discardStartLen*sps:end-discardEndLen*sps,:);
    [varargout{1},varargout{2},varargout{3}] = ...
    helperModulationCharacteristicsTest(waveformDiffFreq,sps);
else % Carrier frequency offset and drift tests
    phyMode = testParams.phyMode;
    intervalDuration = testParams.intervalDuration;
    preambleLen = testParams.preambleLen;
    % Obtain instantaneous frequencies for each sample
    timeScale  = (0:1:length(waveformDiffFreq)-1).'/sampleRate;
    % Obtain payload from the samples 
    payloadFreq = waveformDiffFreq(discardStartLen*sps+1:end-discardEndLen*sps);
    payloadTime = timeScale(discardStartLen*sps+1:end-discardEndLen*sps);    
    % Obtain preamble from the samples
    if(strcmp(phyMode,'LE125K'))
        preambleIndices = (sps*2)+1:(preambleLen-14)*sps;
        preambleFreq = waveformDiffFreq(preambleIndices(1:intervalDuration*sps));
        preambleTime = timeScale(preambleIndices(1:intervalDuration*sps));
        payloadFreq = [waveformDiffFreq(preambleIndices((intervalDuration*sps)+1:end)); payloadFreq];
        payloadTime = [timeScale(preambleIndices((intervalDuration*sps)+1:end)); payloadTime];
    else
        preambleIndices = 0.5*sps:(preambleLen+0.5)*sps;
        preambleFreq = waveformDiffFreq(preambleIndices);
        preambleTime = timeScale(preambleIndices);
    end
    % Calculate initial frequency offset 
    preambleDuration = (length(preambleFreq))*(1/sampleRate);
    f0 = trapz(preambleTime,preambleFreq)./preambleDuration;
    % Calculate Frequency drift for each interval 
    numBins = floor(length(payloadFreq)/(intervalDuration*sps));
    f = zeros(numBins,1);
    for ii = 1:numBins
        rangeOfIntegration = (intervalDuration*sps*(ii-1))+1:(intervalDuration*sps*(ii));
        payloadFreqTemp = payloadFreq(rangeOfIntegration);
        payloadTimeTemp = payloadTime(rangeOfIntegration);
        payloadDuration = length(payloadFreqTemp)*(1/sampleRate);
        f(ii) = trapz(payloadTimeTemp,payloadFreqTemp)./payloadDuration;
    end
    varargout{1} = f0;
    varargout{2} = f;
    varargout{3} = [];
end
end