motionModel = "2-D Constant Velocity";       % Linear motion model

dfMethod = "AoA";          % direction-finding method
dfPacketType = "ConnectionCTE";      % direction-finding packet type

% PHY transmission mode, must be LE1M or LE2M (for ConnectionCTE) and LE1M
% (for ConnectionlessCTE)
phyMode = "LE1M";

% Antenna array size, must be a scalar (represents ULA) or a vector
% (represents URA) for 2-D or 3-D positioning, respectively
arraySize = 16;

% Normalized spacing between the antenna elements with respect to
% wavelength
elementSpacing = 0.5;

% Antenna switching pattern, must be a 1xM row vector and M must be in the
% range [2, 74/slotDuration+1]
switchingPattern = 1:prod(arraySize);

slotDuration = 2;      % Slot duration in microseconds
sps = 4;               % Samples per symbol
channelIndex = 17; % Channel index
crcInit = '555551';           % Cyclic redundancy check (CRC) initialization
accessAddress = '01234567';     % Access address
payloadLength = 1;     % Payload length in bytes

% Length of constant tone extension (CTE) in microseconds, must be in the
% range [16, 160] with 8 microseconds step size
cteLength = 160;

% Sample offset, must be a positive integer in the range [sps/8,7*sps/8]
% for LE1M and [sps/4,7*sps/4] for LE2M
sampleOffset =  2;

EbNo = 25;                                        % Eb/No in dB
environment = "Outdoor";                                 % Environment
txPower =0;                                % Transmit power in dBm

% Configure the bluetoothRangeConfig object
rangeConfig = bluetoothRangeConfig;
rangeConfig.SignalPowerType = "ReceivedSignalPower";
rangeConfig.Environment = environment;
rangeConfig.TransmitterPower = txPower;
rangeConfig.TransmitterCableLoss = 0;                               % Transmitter cable loss 
rangeConfig.ReceiverCableLoss = 0                                   % Receiver cable loss

numDimensions = 2+(strcmp(motionModel,"3-D Constant Velocity") || strcmp(motionModel,"3-D Constant Acceleration"));
if numDimensions == 2 && size(arraySize,2) ~= 1
    error("The arraySize must be a scalar for 2-D position estimation");
elseif numDimensions == 3 && size(arraySize,2) ~= 2
    error("The arraySize must be a 1-by-2 vector for 3-D position estimation");
end
if strcmp(dfPacketType,"ConnectionCTE") && payloadLength ~= 1
    error("The payloadLength must be 1 byte for direction-finding packet type of ConnectionCTE");
elseif strcmp(dfPacketType,"ConnectionlessCTE") && payloadLength < 3
    error("The payloadLength must be greater than or equal to 3 bytes for direction-finding packet type of ConnectionlessCTE");
end

if numDimensions == 3 && isscalar(elementSpacing)
    elementSpacing = [elementSpacing elementSpacing];
end
cfg = bleAngleEstimateConfig("ArraySize",arraySize,"SlotDuration",slotDuration,"SwitchingPattern", ...
    switchingPattern,"ElementSpacing",elementSpacing);
validateConfig(cfg); % Validate the configuration object
pos = getElementPosition(cfg); % Element positions of an array

% Derive type of CTE based on slot duration and direction-finding method
if strcmp(dfMethod,"AoA")
    cteType = [0;0];
else
    cteType = [0;1];
    if slotDuration == 1
        cteType = [1;0];
    end
end

% Create and configure preamble detector System object
accessAddBitsLen = 32;
accessAddBits = int2bit(hex2dec(accessAddress),accessAddBitsLen,false);
refSamples = helperBLEReferenceWaveform(phyMode,accessAddBits,sps);
prbDet = comm.PreambleDetector(refSamples,"Detections","First");

% Create and configure coarse frequency compensator System object
phyFactor = 1+strcmp(phyMode,"LE2M");
sampleRate = 1e6*phyFactor*sps;
coarsesync = comm.CoarseFrequencyCompensator("Modulation","OQPSK",...
    "SampleRate",sampleRate,...
    "SamplesPerSymbol",2*sps,...
    "FrequencyResolution",100);

% Create and configure CRC detector System object
crcLen = 24; % CRC length
crcDet = comm.CRCDetector("x^24 + x^10 + x^9 + x^6 + x^4 + x^3 + x + 1", "DirectMethod",true,...
    "InitialConditions", int2bit(hex2dec(crcInit),crcLen).');

rng('default'); % Initialize the random number generator stream
snr = EbNo - 10*log10(sps); % Signal to noise ratio (SNR) in dB
headerLen = 16+8*strcmp(dfPacketType,"ConnectionCTE"); % Header length
preambleLen = 8*phyFactor; % Preamble length

% Derive initial state for whitening based on channel index
dewhitenStateLen = 6;
chanIdxBin = int2bit(channelIndex,dewhitenStateLen).';
initState = [1 chanIdxBin];

% Consider one node and 15 locators in a network. Generate random locator
% positions and generate node positions based on the motion model.
numLocators = 15; % Number of locators in a network
numNodePositions = 30; % Number of node positions in a network
[posNode,posLocators] = helperBLEPositions(motionModel,numNodePositions,numLocators);

% Simulate motion of the node by looping over the generated node positions.
% For each node position, find active locators.
posNodeEst = zeros(numDimensions,numNodePositions);
for inumNode = 1:numNodePositions

    % Consider active locators that are within 80 meters of range to the node
    [posActiveLocators,angleActive,distanceActive] = helperBLEActiveLocators(posNode(:,inumNode),posLocators);
    posLocatorBuffer{:,inumNode} = posActiveLocators; %#ok<SAGROW>
    numActiveLocators = size(posActiveLocators,2); % Number of active locators

    % Estimate the pathloss between the transmitter and the receiver based on the distance and the environment
    plLin = helperBluetoothEstimatePathLoss(rangeConfig.Environment,distanceActive);
    angleEst = zeros(numActiveLocators,numDimensions-1); % Estimated angles
    [pathLossdB,linkFailFlag] = deal(zeros(numActiveLocators,1)); % Estimated path loss and flag to indicate link fail

    % Loop over number of active locators
    for i = 1:numActiveLocators

        % Generate direction-finding packet
        data = helperBLEGenerateDFPDU(dfPacketType,cteLength,cteType,payloadLength,crcInit);

        % Generate Bluetooth LE waveform
        bleWaveform = bleWaveformGenerator(data,"Mode",phyMode,"SamplesPerSymbol",sps,...
            "ChannelIndex",channelIndex,"DFPacketType",dfPacketType,"AccessAddress",accessAddBits);

        % If the direction-finding method is AoD, perform antenna switching
        if strcmp(dfMethod,"AoD")
            bleWaveform = helperBLESteerSwitchAntenna(bleWaveform,angleActive(i,:),...
                phyMode,sps,dfPacketType,payloadLength,cfg);
        end

        % Add frequency offset
        freqOffset = randsrc(1,1,-10e3:100:10e3);
        freqWaveform = helperBLEFrequencyOffset(bleWaveform,sampleRate,freqOffset);

        % Add timing offset
        timingoff = randsrc(1,1,1:0.2:20);
        timingOffWaveform = helperBLEDelaySignal(freqWaveform,timingoff);

        % Add DC offset
        dcValue = (5/100)*max(timingOffWaveform);
        dcWaveform = timingOffWaveform + dcValue;

        % Apply transmitter power and path loss
        dBmConverter = 30;
        attenuatedWaveform  = 10^((rangeConfig.TransmitterPower-dBmConverter)/20)*dcWaveform/plLin(i);

        % Perform antenna steering if the direction-finding method is AoA
        if strcmp(dfMethod,"AoA")
            steerVec = helperBLESteeringVector(angleActive(i,:),pos);
            steeredWaveform = attenuatedWaveform .* steerVec.';
        else
            steeredWaveform = attenuatedWaveform;
        end

        % Initialize the variables used for receiver processing
        packetDetect = 0; % Packet detect flag
        headerFlag = 1; % Header flag to perform header decoding
        pduCRCFlag = 0; % PDU and CRC flag to perform CRC detection
        samplingFlag = 0; % CTE IQ sampling flag
        crcError = 1; % Checksum flag
        samplesPerFrame = 8*sps; % Samples considered for packet detection
        samplesPerModule = samplesPerFrame; % Samples considered for header, PDU and CRC, CTE IQ sampling
        numTimes = ceil(length(timingOffWaveform)/samplesPerFrame)+1; % Divide the waveform as per the samples considered
        countpacketDetect = 0; % Counter for packet detection
        moduleStartInd = 0; % Start index for each module
        % Append zeros to the waveform
        rxWaveformBuffer = [steeredWaveform;zeros(samplesPerFrame*numTimes-length(steeredWaveform)+samplesPerFrame,size(steeredWaveform,2))];
        for j = 1:numTimes
            % Retrieve the waveform corresponding to each module
            if (j-1)*samplesPerFrame + moduleStartInd + samplesPerModule <= length(rxWaveformBuffer)
                rxChunkWaveform = rxWaveformBuffer((j-1)*samplesPerFrame+moduleStartInd+(1:samplesPerModule),:);
            else
                rxChunkWaveform = rxWaveformBuffer((j-1)*samplesPerFrame+moduleStartInd+1:end,:);
            end

            if ~samplingFlag
                rxNoisyWaveform = awgn(rxChunkWaveform(:,1),snr,"measured"); % Add AWGN
            else
                if strcmp(dfMethod,"AoA") % Perform antenna switching
                    rxSwitchWaveform = helperBLESwitchAntenna(rxChunkWaveform,phyMode,sps,slotDuration,switchingPattern);
                else
                    rxSwitchWaveform = rxChunkWaveform;
                end
                rxNoisyWaveform = awgn(rxSwitchWaveform,snr,"measured"); % Add AWGN

                % Perform IQ sampling on the CTE
                cteSamples = rxNoisyWaveform(1:cteTime*8*sps*phyFactor);
                iqSamples = bleCTEIQSample(cteSamples,"Mode",phyMode,...
                    "SamplesPerSymbol",sps,"SlotDuration",slotDuration,"SampleOffset",sampleOffset);
                samplingFlag = 0;
                break;
            end

            if packetDetect == 0
                countpacketDetect = countpacketDetect+1;
                rcvSigBuffer((countpacketDetect-1)*samplesPerFrame+1:(countpacketDetect)*samplesPerFrame) = rxNoisyWaveform;
                if countpacketDetect >= (preambleLen+accessAddBitsLen+headerLen)*sps/samplesPerFrame

                    % Perform timing synchronization
                    [~, dtMt] = prbDet(rcvSigBuffer.');
                    release(prbDet)
                    prbDet.Threshold = max(dtMt);
                    prbIdx = prbDet(rcvSigBuffer.');
                    release(prbDet)
                    if prbIdx >=length(refSamples)
                        rcvTrim = rcvSigBuffer(1+prbIdx-length(refSamples):end).';
                    else
                        rcvTrim = rcvSigBuffer.';
                    end

                    % Estimate and compensate DC offset
                    estimatedDCOffset = mean(rcvTrim(1:length(refSamples)))-mean(refSamples)*sqrt(var(rcvTrim));
                    rcvDCFree = rcvTrim-estimatedDCOffset;

                    % Estimate and compensate frequency offset
                    [~,freqoff] = coarsesync(rcvDCFree);
                    release(coarsesync)
                    [rcvFreqOffsetFree,iniFreqState] = helperBLEFrequencyOffset(rcvDCFree,sampleRate,-freqoff);

                    % Perform GMSK demodulation
                    x = rem(length(rcvFreqOffsetFree),sps);
                    if x
                        remsad = sps - x;
                    else
                        remsad = x;
                    end
                    remNumSamples = x;
                    remSamples = rcvFreqOffsetFree(end-remNumSamples+1:end);
                    [demodSoftBits,demodInitPhase] = helperBLEGMSKDemod(rcvFreqOffsetFree(1:end-remNumSamples),sps,0);
                    demodBits = demodSoftBits>0;

                    % Check for access address match
                    if length(demodBits) >= preambleLen+accessAddBitsLen
                        accessAddress = int8(demodBits(preambleLen+(1:accessAddBitsLen))>0);
                        decodeData = int8(demodBits(accessAddBitsLen+preambleLen+1:end)>0);
                        if isequal(accessAddBits,accessAddress)
                            packetDetect = 1;
                            samplesPerModule = headerLen*sps-length(decodeData)*sps+remsad;
                            rcvSigBuffer = [];
                        end
                    end
                end
            end

            if packetDetect
                if headerFlag && (length(rxNoisyWaveform) ~= samplesPerFrame || samplesPerModule <= 0)

                    % Remove DC offset
                    rxDCFreeWaveform = rxNoisyWaveform-estimatedDCOffset;

                    % Frequency offset correction
                    [rxNoisyWaveform,iniFreqState] = helperBLEFrequencyOffset(rxDCFreeWaveform,sampleRate,-freqoff,iniFreqState);

                    % Perform GMSK demodulation on the header bits
                    if (length(rxNoisyWaveform) ~= samplesPerFrame)
                        rcvSigHeaderBuffer = [remSamples;rxNoisyWaveform];
                        [headerSoftBits,demodInitPhase] = helperBLEGMSKDemod(rcvSigHeaderBuffer,sps,demodInitPhase);
                        decodeDataHeader = [decodeData;headerSoftBits>0];
                    elseif samplesPerModule <= 0
                        decodeDataHeader = decodeData;
                    end

                    % Perform data dewhitening
                    [dewhitenedBits,initState1] = helperBLEWhiten(decodeDataHeader,initState);

                    % Extract PDU length
                    pduLenField = double(dewhitenedBits(9:16)); % Second byte of PDU header
                    pduLenInBits = bi2de(pduLenField')*8;

                    % Retrieve CTE time and CTE type information from bits
                    if strcmp(dfPacketType,"ConnectionCTE")
                        [cteTime,cteTypeDec] = helperBLECTEInfoExtract(dewhitenedBits,dfPacketType);
                        % Slot duration based on CTE type
                        if cteTypeDec == 1 || cteTypeDec == 2
                            slotDuration = cteTypeDec;
                        else
                            slotDuration = cfg.SlotDuration;
                        end
                    end

                    % Update the samples per frame, start index to
                    % perform CRC detection
                    if samplesPerModule
                        moduleStartInd = samplesPerModule-samplesPerFrame;
                    else
                        moduleStartInd = 0;
                    end
                    samplesPerModule = (pduLenInBits+crcLen-length(dewhitenedBits)+headerLen)*sps;
                    pduCRCFlag = 1; % Enable the flag for CRC detection
                    headerFlag = 0; % Disable the header recovery flag
                    rcvSigHeaderVar = rxNoisyWaveform; % Load the waveform for pathloss estimation
                    rxNoisyWaveform = [];
                end

                if pduCRCFlag && ~isempty(rxNoisyWaveform)

                    % Remove DC offset
                    rxDCFreeWaveform = rxNoisyWaveform-estimatedDCOffset;

                    % Frequency offset correction
                    [rxNoisyWaveform,iniFreqState] = helperBLEFrequencyOffset(rxDCFreeWaveform,sampleRate,-freqoff,iniFreqState);

                    % Perform GMSK demodulation on the PDU and CRC bits
                    x = rem(length(rxNoisyWaveform),sps);
                    if x
                        remNumSamples = sps - x;
                    else
                        remNumSamples = x;
                    end
                    demodPDUCRC = helperBLEGMSKDemod([rxNoisyWaveform;zeros(remNumSamples,1)],sps,demodInitPhase)>0;

                    % Perform data dewhitening
                    dewhitenedPDUCRC = helperBLEWhiten(demodPDUCRC,initState1);

                    % Retrieve CTE time and CTE type information from bits
                    headerPDUCRC = [double(dewhitenedBits);dewhitenedPDUCRC];
                    if strcmp(dfPacketType,"ConnectionlessCTE")
                        [cteTime,cteTypeDec] = helperBLECTEInfoExtract(headerPDUCRC,dfPacketType);
                        % Slot duration based on CTE type
                        if cteTypeDec == 1 || cteTypeDec == 2
                            slotDuration = cteTypeDec;
                        else
                            slotDuration = cfg.SlotDuration;
                        end
                    end

                    % Perform CRC detection
                    [dfPDU,crcError] = crcDet(headerPDUCRC);
                    if crcError
                        break;
                    end

                    % Update the samples per frame, start index to
                    % perform IQ sampling
                    moduleStartInd = samplesPerModule-samplesPerFrame+moduleStartInd;
                    samplesPerModule = cteTime*8*sps*phyFactor;
                    samplingFlag = 1; % Enable the flag to perform IQ sampling
                    pduCRCFlag = 0; % Disable the PDU and CRC flag
                    rcvSigPDUVar = rxNoisyWaveform; % Load the waveform for pathloss estimation
                end
            end
        end

        % IQ samples must contain at least eight samples from reference period and
        % one sample from each antenna
        refSampleLength = 8; % Reference samples length
        minIQSamples = refSampleLength+getNumElements(cfg)-1;
        if ~crcError && length(nonzeros(iqSamples)) >= minIQSamples  % If packet detection is successful
            angleEst(i,:) = bleAngleEstimate(iqSamples,cfg);
            rangeConfig.ReceivedSignalPower = 10*log10(var([rcvFreqOffsetFree;rcvSigHeaderVar;rcvSigPDUVar]));
            pathLossdB(i) = pathLoss(rangeConfig);
        else
            linkFailFlag(i) = 1; % If packet detection fails, enable the link fail flag
        end
    end

    if any(linkFailFlag == 1)
        idx = find(linkFailFlag == 1);
        posActiveLocators(:,idx) = [];
        angleEst(idx,:) = [];
        pathLossdB(idx) = [];
    end
    if (numActiveLocators-nnz(linkFailFlag)) >= 2 && ~all(angleEst(:,1) == angleEst(1,1))

        % Estimate the node position by using the angulation method
        posNodeEst(:,inumNode) = blePositionEstimate(posActiveLocators,"angulation",angleEst.');
    elseif (numActiveLocators-nnz(linkFailFlag)) == 1 || (~all(linkFailFlag == 1) && all(angleEst(:,1) == angleEst(1,1)))

        % Estimate the distance between the transmitter and the receiver based on the environment
        range = bluetoothRange(rangeConfig);
        distanceValues = min(range):max(range);
        idx = randi(length(distanceValues),1);
        distanceEst = distanceValues(idx)

        % Estimate the node position by using the distance-angle method
        posNodeEst(:,inumNode) = blePositionEstimate(posActiveLocators,"distance-angle",distanceEst.',angleEst.');
    else
        posNodeEst(:,inumNode) = NaN(numDimensions,1);
    end
end

posErr = sqrt(sum((posNodeEst-posNode).^2)); % Compute the position error
disp(["Positioning error in meters = ", num2str(posErr)])

if ~all(isnan(posNodeEst(1,:)))
    helperBLEVisualizeNodeTracking(posLocators,posNode,posLocatorBuffer,posNodeEst)
end

% if all(isnan(posNodeEst(1,:)))
%     disp("All direction-finding packet transmissions failed")
% else
%     posNodeTrackEst = helperBLEKalmanFilter(motionModel,posNodeEst);
%     posTrackErr = sqrt(sum((posNodeTrackEst-posNode).^2)); % Compute the position error with Kalman filter
%     disp(["Positioning error with Kalman filter in meters = ", num2str(posTrackErr)])
% end