function [HBB,CRB,HCR,HBBxy,CRBxy,HCRxy] = AOALoc_HHB_3D(SensorPositions,SourceLocation,Q)
% [HBB,CRB,HCR,HBBxy,CRBxy,HCRxy] = AOALoc_HHB_3D(SensorPositions,SourceLocation,Q)
%
% SensorPositions:  (Dim x M) matrix, each column is a sensor position and
%                   first column is for the reference sensor,
%                   M is the number of sensors and should be at least Dim+2,
% SourceLocation:	M x 1 (2-D case) or 2M x 1 (3-D case), AOA measurement vector
% Q:                M x M (2-D case) or 2M x 2M (3-D case) covariance matrix of the AOA
%                   measurement vector
% HBB:  HBB bound in MPR
% CRB:  CRB in MPR
% HCR:  HCR bound in MPR
% HBBxy:    HBB bound in Cartesian
% CRBxy:    CRB in Cartesian
% HCRxy:    HCR bound in Cartesian
%
% The routine evaluates the HBB bound for AOA positioning, works for 2-D and 3-D
%
% Reference: J. S. Abel, “A bound on mean-square-estimate error,” IEEE Trans. Inf. 
% Theory, vol. 39, no. 5, pp. 1675–1680, Sep. 1993.
%
% Reference: Y. Wang and K. C. Ho, “Unified near-field and far-field localization for
% AOA and hybrid AOA-TDOA positionings,” IEEE Trans. Wireless Commun., vol. 17, no. 2,
% pp. 1242-1254, Feb. 2018.
%
% Yue Wang, K. C. Ho    03-18-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

rand('state',17^5*2);
M = length(Q);

S = SensorPositions;
uo = SourceLocation;

[N,M] = size(SensorPositions);

for i=1:M
    [thetamo(i,1) phimo(i,1) ro(i,1)]=cart2sph(uo(1)-S(1,i),uo(2)-S(2,i),uo(3)-S(3,i));
end
anglemo=[thetamo;phimo];
iQ=inv(Q);

% ----- CRB component -----
for i=1:M
    dd(i)=norm(uo-S(:,i),2);
    rr(i)=norm(uo(1:2)-S(1:2,i),2);
end
for i=1:M
    dthetamdx(i,1)=-(uo(2)-S(2,i))/rr(i)^2;
    dthetamdy(i,1)=(uo(1)-S(1,i))/rr(i)^2;
    dthetamdz(i,1)=0;
    dphimdx(i,1)=-(uo(1)-S(1,i))*(uo(3)-S(3,i))/(rr(i)*dd(i)^2);
    dphimdy(i,1)=-(uo(2)-S(2,i))*(uo(3)-S(3,i))/(rr(i)*dd(i)^2);
    dphimdz(i,1)=rr(i)^2/(rr(i)*dd(i)^2);
end
d_u=[dthetamdx dthetamdy dthetamdz;
    dphimdx dphimdy dphimdz];

K = d_u'*iQ*d_u;    % FIM

Rpt=10;
HBBAllxy=0;
HCRAllxy=0;
HBBAll=0;
HCRAll=0;

for rpt=1:Rpt,
    
    % ----- HCR component -----
    qo=anglemo'*iQ*anglemo;
    
    L=10000;     % number of test points
    L1=4;
    tstLoc=(rand(N,L)-0.5)*(200*max(abs(uo)))+repmat(uo,1,L);
    mse=zeros(1,L);
    
    for ii=1:L,
        uTst=tstLoc(:,ii);
        for i=1:M
            [thetamTilde(i,1) phimTilde(i,1) rmTilde(i,1)]=cart2sph(uTst(1)-S(1,i),uTst(2)-S(2,i),uTst(3)-S(3,i));
        end
        anglemTilde=[thetamTilde;phimTilde];
        JTilde=exp(-0.5*(2*anglemTilde'*iQ*anglemTilde-qo-(2*anglemTilde-anglemo)'*iQ*(2*anglemTilde-anglemo)))-1;
        mse(ii)=trace((uTst-uo)*inv(JTilde)*(uTst-uo)');
    end;
    
    [tmp,idx]=sort(mse,'descend');  % select test points giving large bound values
    
    r=zeros(M,L1); rd=zeros(M-1,L1);
    J=zeros(L1,L1);
    for ii=1:L1,
        uTst=tstLoc(:,idx(ii));
        for i=1:M
            [thetam(i,ii) phim(i,ii) rm(i,ii)]=cart2sph(uTst(1)-S(1,i),uTst(2)-S(2,i),uTst(3)-S(3,i));
        end
    end;
    anglem=[thetam;phim];
    for ii=1:L1,
        for jj=1:L1,
            J(ii,jj)=exp(-0.5*(anglem(:,ii)'*iQ*anglem(:,ii)+anglem(:,jj)'*iQ*anglem(:,jj)-qo ...
                -(anglem(:,ii)+anglem(:,jj)-anglemo)'*iQ*(anglem(:,ii)+anglem(:,jj)-anglemo)))-1;
        end;
    end;
    
    tstLoc=tstLoc(:,idx(1:L1));
    
    % ----- Cross term -----
    L=d_u'*iQ*(anglem-anglemo*ones(1,L1));
    % ----- Phi matrix -----
    Phixy=[eye(N), (tstLoc-uo*ones(1,L1))];
    % ----- HHB -----
    H=[ K, L; L', J];
    HBBxy=Phixy*inv(H)*Phixy';
    % ----- CRB -----
    CRBxy=Phixy(:,1:N)*inv(K)*Phixy(:,1:N)';
    % ---- HCR -----
    HCRxy=Phixy(:,N+1:end)*inv(J)*Phixy(:,N+1:end)';
    
    if (~isfinite(trace(HBBxy))) HBBxy=CRBxy; end;
    
    HBBAllxy=HBBAllxy+HBBxy;
    HCRAllxy=HCRAllxy+HCRxy;
    
    rgo=norm(uo);
    thetao=angle(uo(1)+sqrt(-1)*uo(2));
    phio=angle(norm(uo(1:2))+sqrt(-1)*uo(3));
    cThetao=cos(thetao); sThetao=sin(thetao); cPhio=cos(phio); sPhio=sin(phio);
    A=[rgo*cThetao*cPhio^2, rgo*sThetao*cPhio^2, rgo*sPhio*cPhio;
        -sThetao, cThetao,0;
        -cThetao*sPhio*cPhio, -sThetao*sPhio*cPhio, cPhio^2]/(rgo*cPhio);
    A=diag([-1/rgo^2,1,1])*A;
    irgBg=[1./sqrt(sum(tstLoc.^2)); angle(tstLoc(1,:)+sqrt(-1)*tstLoc(2,:));
        angle(sqrt(tstLoc(1,:).^2+tstLoc(2,:).^2)+sqrt(-1)*tstLoc(3,:))];
    irgBgo=[1/rgo, thetao, phio]';
    Phi=[A,(irgBg-irgBgo*ones(1,L1))];
    
    % ----- HHB -----
    H=[ K, L; L', J];
    HBB=Phi*inv(H)*Phi';    
    % ----- CRB -----
    CRB=Phi(:,1:N)*inv(K)*Phi(:,1:N)';   
    % ---- HCR -----
    HCR=Phi(:,N+1:end)*inv(J)*Phi(:,N+1:end)';
    
    if (~isfinite(trace(HBB))) HBB=CRB; end;
    
    HBBAll=HBBAll+HBB;
    HCRAll=HCRAll+HCR;
end
HBBxy=HBBAllxy/Rpt;
HCRxy=HCRAllxy/Rpt;
HBB=HBBAll/Rpt;
HCR=HCRAll/Rpt;
