function [HBB,CRB,HCR,HBBxy,CRBxy,HCRxy] = AOATDOA_HBB_3D(SensorPositions,SourceLocation,Q)
% [HBB,CRB,HCR,HBBxy,CRBxy,HCRxy] = AOATDOALoc_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,
%                   the sensors should not lie in a plane or a line.
% SourceLocation:	2M 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 TDOA and 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*0);
s = SensorPositions;
S = s;
[N,M] = size(SensorPositions);
uo = SourceLocation(1:N);
ro = sqrt(sum((uo*ones(1,M)-S).^2))';
rdo=ro(2:end)-ro(1);
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
mo=[rdo;thetamo;phimo];

iQ=inv(Q);

% ----- CRB component -----
dtdoa_du = (S(:,2:end)-uo*ones(1,M-1))'./(ro(2:end)*ones(1,size(S,1))) ...
    -ones(M-1,1)*((S(:,1)-uo)'/ro(1));
dtdoa_du=-dtdoa_du;

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
dthetam_du=[dthetamdx dthetamdy dthetamdz;];
dphim_du=[dphimdx dphimdy dphimdz];

d_u = [dtdoa_du;dthetam_du;dphim_du];

K = d_u'*iQ*d_u;    % FIM

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

for rpt=1:Rpt,
    % ----- HCR component -----
    qo=mo'*iQ*mo;
    L=10000*10;     % number of test points
    L1=4;
    tstLoc=(rand(N,L)-0.5)*max(abs(uo))*200+repmat(uo,1,L);
    mse=zeros(1,L);
    
    for ii=1:L,
        uTst=tstLoc(:,ii);
        rTilde = sqrt(sum((uTst*ones(1,M)-S).^2))';
        rdTilde=rTilde(2:end)-rTilde(1);
        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
        mTilde=[rdTilde; thetamTilde; phimTilde];
        JTilde=exp(-0.5*(2*mTilde'*iQ*mTilde-qo-(2*mTilde-mo)'*iQ*(2*mTilde-mo)))-1;
        mse(ii)=trace((uTst-uo)*inv(JTilde)*(uTst-uo)');
    end;
    
    [tmp,idx]=sort(mse,'descend');  % select test points giving large bound values
    for ii=1:L1,
        uTst=tstLoc(:,idx(ii));
        rt(:,ii) = sqrt(sum((uTst*ones(1,M)-S).^2))';
        rd(:,ii)=rt(2:end,ii)-rt(1,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
        m=[rd; thetam; phim];
    end
    
    for ii=1:L1,
        for jj=1:L1,
            J(ii,jj)=exp(-0.5*(m(:,ii)'*iQ*m(:,ii)+m(:,jj)'*iQ*m(:,jj)-qo ...
                -(m(:,ii)+m(:,jj)-mo)'*iQ*(m(:,ii)+m(:,jj)-mo)))-1;
        end;
    end;
    
    tstLoc=tstLoc(:,idx(1:L1));
    
    % ----- Cross term -----
    L=d_u'*iQ*(m-mo*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))];
    
    % ----- HBB -----
    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;
