function uTilde0=AOA_SDRMPR_3D(s,a,Q)
% uTilde0=AOA_SDRMPR_3D(s,a,Q)
%
% s:    (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,
% a:    2M x 1, AOA measurement vector
% Q:    2M x 2M, covariance matrix of the AOA
%       measurement vector
% uTilde0:  solution in MPR
%
% The routine uses SDR to obtain an initial coarse solution in MPR,
% works for 3-D
%
% 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
%
%   12-19-2019: bug fixed on lines 61, 63, 115, 117, giving better
%               SDR-MPR results in Figs. 5 & 6 than those reported in the paper.
%               (thanks to Dr. Gang Wang and his student from Ningbo Univ.)
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

thetam=a(1:length(a)/2);
phim=a(1+length(a)/2:end);

[Dim,M]=size(s);

n1=4;

for i=1:M
    G1(i,:)=[-sin(thetam(i)) cos(thetam(i)) 0 s(1,i)*sin(thetam(i))-s(2,i)*cos(thetam(i))];
    G2(i,:)=[sin(phim(i))*cos(thetam(i)) sin(phim(i))*sin(thetam(i)) -cos(phim(i)) -(s(1,i)*sin(phim(i))*cos(thetam(i))+s(2,i)*sin(phim(i))*sin(thetam(i))-s(3,i)*cos(phim(i)))];
end
G=[G1;G2];
M1=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 0];

% -- preliminary solution to estimate weighting matrix W --
W1 =eye(2*M);

cvx_begin quiet;
variable s1
variable V1(4,4) symmetric
minimize(trace(G'*W1*G*V1))
subject to
1-trace(M1*V1)==0
V1==semidefinite(n1);
cvx_end;
[V,D]=eig(V1);
[d,IX]=sort(sum(D));
u1=V(:,IX(end))*sqrt(D(IX(end),IX(end)));

up1=[u1(4) angle(u1(1)+sqrt(-1)*u1(2)), angle(sqrt(1-u1(3)^2)+sqrt(-1)*u1(3))]';
u2=-u1;
up2=[u2(4) angle(u2(1)+sqrt(-1)*u2(2)), angle(sqrt(1-u2(3)^2)+sqrt(-1)*u2(3))]';

invrg=up1(1);
thetag=up1(2);
phig=up1(3);
for i=1:M
    b01(i,:)=[cos(thetag)*cos(phig)-s(1,i)*invrg   sin(thetag)*cos(phig)-s(2,i)*invrg   sin(phig)-s(3,i)*invrg];
    thetamg1(i,1)=angle(cos(phig)*cos(thetag)-invrg*s(1,i)+sqrt(-1)*(cos(phig)*sin(thetag)-invrg*s(2,i)));
    phimg1(i,1)=angle(norm(b01(i,1:2),2)+sqrt(-1)*(sin(phig)-invrg*s(3,i)));
end

invrg=up2(1);
thetag=up2(2);
phig=up2(3);
for i=1:M
    b02(i,:)=[cos(thetag)*cos(phig)-s(1,i)*invrg   sin(thetag)*cos(phig)-s(2,i)*invrg   sin(phig)-s(3,i)*invrg];
    thetamg2(i,1)=angle(cos(phig)*cos(thetag)-invrg*s(1,i)+sqrt(-1)*(cos(phig)*sin(thetag)-invrg*s(2,i)));
    phimg2(i,1)=angle(norm(b01(i,1:2),2)+sqrt(-1)*(sin(phig)-invrg*s(3,i)));
end

e1=norm([thetam phim]-[thetamg1 phimg1],2);
e2=norm([thetam phim]-[thetamg2 phimg2],2);
if e1<e2
    up=up1;
    b0=b01;
else
    up=up2;
    b0=b02;
end

Bt=zeros(M,M);
Bp=zeros(M,M);
for i=1:M
    Bt(i,i)=norm(b01(i,1:2),2);
    Bp(i,i)=norm(b01(i,:),2);
end
B=[Bt,zeros(M,M);zeros(M,M),Bp];
W1=inv(B*Q*B');

% -- solution with approximated weighting matrix W --
cvx_begin quiet;
variable s1
variable V1(4,4) symmetric
minimize(trace(G'*W1*G*V1))
subject to
1-trace(M1*V1)==0
V1==semidefinite(n1);
cvx_end;
[V,D]=eig(V1);
[d,IX]=sort(sum(D));
u1=V(:,IX(end))*sqrt(D(IX(end),IX(end)));

up1=[u1(4) angle(u1(1)+sqrt(-1)*u1(2)), angle(sqrt(1-u1(3)^2)+sqrt(-1)*u1(3))]';
u2=-u1;
up2=[u2(4) angle(u2(1)+sqrt(-1)*u2(2)), angle(sqrt(1-u2(3)^2)+sqrt(-1)*u2(3))]';

invrg=up1(1);
thetag=up1(2);
phig=up1(3);
for i=1:M
    b01(i,:)=[cos(thetag)*cos(phig)-s(1,i)*invrg   sin(thetag)*cos(phig)-s(2,i)*invrg   sin(phig)-s(3,i)*invrg];
    thetamg1(i,1)=angle(cos(phig)*cos(thetag)-invrg*s(1,i)+sqrt(-1)*(cos(phig)*sin(thetag)-invrg*s(2,i)));
    phimg1(i,1)=angle(norm(b01(i,1:2),2)+sqrt(-1)*(sin(phig)-invrg*s(3,i)));
end

invrg=up2(1);
thetag=up2(2);
phig=up2(3);
for i=1:M
    b02(i,:)=[cos(thetag)*cos(phig)-s(1,i)*invrg   sin(thetag)*cos(phig)-s(2,i)*invrg   sin(phig)-s(3,i)*invrg];
    thetamg2(i,1)=angle(cos(phig)*cos(thetag)-invrg*s(1,i)+sqrt(-1)*(cos(phig)*sin(thetag)-invrg*s(2,i)));
    phimg2(i,1)=angle(norm(b01(i,1:2),2)+sqrt(-1)*(sin(phig)-invrg*s(3,i)));
end

e1=norm([thetam phim]-[thetamg1 phimg1],2);
e2=norm([thetam phim]-[thetamg2 phimg2],2);
if e1<e2
    up=up1;
else
    up=up2;
end

uTilde0=up([2,3,1]);
