function ang = bleAngleEstimate(iqSamples,cfg)
%bleAngleEstimate Estimate AoA or AoD of Bluetooth LE Signal
%
%   ANG = bleAngleEstimate(IQSAMPLES,CFG) estimates the angle of arrival
%   (AoA) or angle of departure (AoD), ANG, for the given in-phase and
%   quadrature (IQ) samples, IQSAMPLES, and Bluetooth(R) low energy (LE)
%   angle estimation configuration object, CFG.
%
%   ANG is a 1x2 vector or a scalar. The size of ANG equals to the size of
%   <a href="matlab:help('bleAngleEstimateConfig.ArraySize')">ArraySize</a> property or a 1x2 vector if <a
%   href="matlab:help('bleAngleEstimateConfig.EnableCustomArray')">EnableCustomArray</a> is true.
%   If ANG is a 1x2 vector, then the first value represents the estimated
%   azimuth angle (in degrees) and the second value represents the
%   estimated elevation angle (in degrees). If ANG is a scalar, then this
%   value represents the estimated broadside angle. If elevation angle is
%   zero, then broadside angle becomes azimuth angle.
%
%   IQSAMPLES is a complex-valued column vector of type double or single.
%   This value consists of IQ samples corresponding to 8 microseconds
%   reference period and sample slots.
%
%   CFG is a configuration object of type <a
%   href="matlab:help('bleAngleEstimateConfig')">bleAngleEstimateConfig</a>
%   that configures the Bluetooth LE angle estimation function.
%
%   % Example:
%
%   % Estimate the AoA of Bluetooth LE signal received by a 2-element
%   % uniform linear array (ULA) using LE1M physical layer transmission
%   % mode for connection-oriented scenario. The actual direction of the
%   % Bluetooth LE signal is 70 degrees in azimuth and 0 degrees in
%   % elevation.
%
%   % Create and configure the Bluetooth LE angle estimation configuration
%   % object
%   cfg = bleAngleEstimateConfig('ArraySize',2,'SlotDuration',2,...
%                   'SwitchingPattern',[1 2]);
%   % Valid IQ samples for connection data channel PDU for AoA constant
%   % tone extension (CTE) with 2 microseconds slots, CTE time of 16
%   % microseconds and rotated with 70 degrees in azimuth.
%   iqSamples = [0.8507+0.5257i;-0.5257 + 0.8507i;-0.8507 - 0.5257i;...
%                0.5257 - 0.8507i;0.8507+0.5257i;-0.5257 + 0.8507i;...
%               -0.8507 - 0.5257i;0.5257 - 0.8507i;-0.3561 + 0.9345i];
%   ang = bleAngleEstimate(iqSamples,cfg); % Estimate the angle
%
%   See also bleAngleEstimateConfig, bleWaveformGenerator,
%   bleIdealReceiver, blePositionEstimate, bleCTEIQSample.

%   Copyright 2020-2022 The MathWorks, Inc.

%#codegen

% Check the number of input arguments
narginchk(2,2)

% Validate the configuration object
validateattributes(cfg,{'bleAngleEstimateConfig'},{'scalar'},'bleAngleEstimate','cfg',2);
validateConfig(cfg);

% Validate the IQ samples
validateattributes(iqSamples,{'double','single'},{'nonnan','finite','column'},...
                        mfilename,'IQSamples');
refSampleLength = 8;
coder.internal.errorIf(length(nonzeros(iqSamples))<(refSampleLength+getNumElements(cfg)-1),...
                        'bluetooth:bleAngleEstimate:InputLength',...
                        refSampleLength+getNumElements(cfg)-1,length(nonzeros(iqSamples)));

% Adjust switching pattern as per the Bluetooth Core Specification, version
% 5.1, volume 6, part A, section 5. During antenna switching, if the
% specified switching pattern is exhausted before the last sample slot,
% then the pattern is restarted from the beginning. If the pattern has not
% been completely used at the end of CTE, any remaining values shall be
% ignored.
expectedSPLen = length(iqSamples)-(refSampleLength-1); % Expected switching pattern length
spLength = length(cfg.SwitchingPattern); % Given switching pattern length
if expectedSPLen >= spLength
    repFactor = ceil(expectedSPLen/spLength);
    switchingPatternRep =  repmat(cfg.SwitchingPattern,1,repFactor);
    switchingPattern = switchingPatternRep(1:expectedSPLen);
else
    switchingPattern = cfg.SwitchingPattern(1:expectedSPLen);
end

% Derive phase difference for four microseconds duration, from 8 reference
% samples, which only has GFSK effect as all the reference samples are
% collected using same antenna.
phaseRef = unwrap(angle(iqSamples(1:refSampleLength)));
deltaFreq = diff(phaseRef)./(2*pi);
avgDeltaFreq = mean(abs(deltaFreq))*1e6;
if avgDeltaFreq < 375e3 % For LE1M
    phDiff = 1;
else % For LE2M
    phDiff = 2;
end

% Consider 1 sample from first antenna by taking an average over 8
% reference samples
tRef = (0:refSampleLength-1)'; % Reference time in microseconds
iqRef = iqSamples(1:refSampleLength).*exp(-1i*tRef*phDiff*pi/2); % Remove phase shift caused due to GFSK
% Average over 8 reference samples and shift the sample phase corresponding
% to 8th reference sample
iqRefSample = mean(iqRef).*exp(1i*(phaseRef(8)-phaseRef(1)));
iqSamples = [iqRefSample;iqSamples(refSampleLength+1:end)].'; % Append reference sample with remaining samples

% Remove GFSK effect for all the IQ Samples
gfskAngles = exp(-1i*(0:cfg.SlotDuration*pi*phDiff:cfg.SlotDuration*pi*phDiff*(length(iqSamples)-1)));
IQGFSKRemoved = iqSamples.*gfskAngles;
% Sort the switching pattern and re-order the samples
[switchingPattern,sortedInd]=sort(switchingPattern);
IQGFSKRemovedSort = IQGFSKRemoved(sortedInd);

% Average the IQSamples which are collected from the same antenna because
% same antenna can be used multiple times
u = unique(switchingPattern); % Accept the unique values
n = histc(switchingPattern,u); %#ok<HISTC> % Returns number of values repeated at each index
d = u(n>1); % Extract repetitive indices
removeRepIdx = zeros(1,0);
if ~isempty(d) % If there is any repetitive index
    for j = 1:length(d) % Check for any repetitive index
        ind = find(ismember(switchingPattern,d(j))); % Exact indices for repetitive index
        IQGFSKRemovedSort(ind(1)) = mean(IQGFSKRemovedSort(ind)); % Average
        removeRepIdx = [removeRepIdx ind(2:end)]; %#ok<AGROW> % Remove repetitive indices
    end
   IQGFSKRemovedSort(removeRepIdx) = 0;
end
IQSamplesAveraged = nonzeros(IQGFSKRemovedSort);

% Estimate AoA or AoD using MUSIC algorithm
ang = ble.internal.musicEstimate(IQSamplesAveraged.',cfg);

end