function uTilde0=  AOATDOA_SDRMPR_3D(s,rd,a,Q)
% uTilde0 = AOATDOA_SDRMPR_3D(s,rd,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,
%       the sensors should not lie in a plane or a line.
% rd:   (M-1) x 1, TDOA measurement vector
% 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
%
%       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);

%M = length(rd) + 1;

[Dim,M]=size(s);

Q_tdoa = Q(1:M-1,1:M-1);
Q_thetam = Q(M:2*M-1,M:2*M-1);
Q_phim = Q(2*M:3*M-1,2*M:3*M-1);
Q_angelm = Q(M:3*M-1,M:3*M-1);
n1=9;

h1 = rd;
h2=zeros(M,1);
h3=zeros(M,1);
for i=2:M
    sd(:,i-1)=s(:,i)-s(:,1);
    [alpha(i-1), beta(i-1) R(i-1)]=cart2sph(sd(1,i-1),sd(2,i-1),sd(3,i-1));
end
for i=1:M-1
    G1(i,:) = [-R(i)*cos(alpha(i))*cos(beta(i)) -R(i)*sin(alpha(i))*cos(beta(i)) -R(i)*sin(beta(i)) 0.5*R(i)^2*(1-cos(alpha(i))^2*cos(beta(i))^2)...
        0.5*R(i)^2*(1-sin(alpha(i))^2*cos(beta(i))^2) 0.5*R(i)^2*cos(beta(i))^2 -R(i)^2*sin(alpha(i))*cos(alpha(i))*sin(beta(i))*cos(beta(i))...
        -R(i)^2*cos(alpha(i))*sin(beta(i))*cos(beta(i)) -R(i)^2*sin(alpha(i))*sin(beta(i))*cos(beta(i))];
end
for i=1:M
    G2(i,:)=[-sin(thetam(i)), cos(thetam(i)), 0, s(1,i)*sin(thetam(i))-s(2,i)*cos(thetam(i)), s(1,i)*sin(thetam(i))-s(2,i)*cos(thetam(i)),...
        s(1,i)*sin(thetam(i))-s(2,i)*cos(thetam(i)), 0, 0, 0];
    G3(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))), ...
        -(s(1,i)*sin(phim(i))*cos(thetam(i))+s(2,i)*sin(phim(i))*sin(thetam(i))-s(3,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))), 0, 0, 0];
end
h=[h1;h2;h3];
G=[G1;G2;G3];
W = eye(3*M-1);
M1=zeros(9,9); M1(1,1)=1; M1(2,2)=1; M1(3,3)=1;
M2=zeros(9,9); M2(1,9)=1; M2(9,1)=1; M2(2,8)=-1; M2(8,2)=-1;
M3=zeros(9,9); M3(1,9)=1; M3(9,1)=1; M3(3,7)=-1; M3(7,3)=-1;
M4=zeros(9,9); M4(4,5)=0.5; M4(5,4)=0.5; M4(7,7)=-1;
M5=zeros(9,9); M5(1,6)=1; M5(6,1)=1; M5(3,8)=-1; M5(8,3)=-1;
M6=zeros(9,9); M6(2,6)=1; M6(6,2)=1; M6(3,9)=-1; M6(9,3)=-1;

cvx_begin quiet;
variables s1 s2 s3
variable V1(9,9) symmetric
minimize(trace(G'*W*G*V1)-2*pow_p(trace(G'*W*h*h'*W*G*V1),0.5))
subject to
1-trace(M1*V1)==0
-trace(M2*V1)==0
-trace(M3*V1)==0
-trace(M4*V1)==0
-trace(M5*V1)==0
-trace(M6*V1)==0
V1==semidefinite(n1);
cvx_end;
[V,D]=eig(V1);
[d,IX]=sort(sum(D));
u1=V(:,IX(end));
up1=[u1(4)+u1(5)+u1(6),angle(u1(1)/sqrt(1-u1(3)^2)+sqrt(-1)*u1(2)/sqrt(1-u1(3)^2)), angle(sqrt(1-u1(3)^2)+sqrt(-1)*u1(3))]';
u2=-u1;
up2=[u2(4)+u2(5)+u2(6),angle(u2(1)/sqrt(1-u2(3)^2)+sqrt(-1)*u2(2)/sqrt(1-u2(3)^2)), angle(sqrt(1-u2(3)^2)+sqrt(-1)*u2(3))]';
rd1=G1*u1;
rd2=G1*u2;

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];
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];
end

e1=norm(rd1-rd,2);
e2=norm(rd2-rd,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=[eye(M-1),zeros(M-1,M),zeros(M-1,M);zeros(M,M-1),Bt,zeros(M,M);zeros(M,M-1),zeros(M,M),Bp];
W1=inv(B*Q*B');

cvx_begin quiet;
variables s1 s2 s3
variable V1(9,9) symmetric
minimize(trace(G'*W*G*V1)-2*pow_p(trace(G'*W*h*h'*W*G*V1),0.5))
subject to
1-trace(M1*V1)==0
-trace(M2*V1)==0
-trace(M3*V1)==0
-trace(M4*V1)==0
-trace(M5*V1)==0
-trace(M6*V1)==0
V1==semidefinite(n1);
cvx_end;
[V,D]=eig(V1);
[d,IX]=sort(sum(D));
u1=V(:,IX(end));
up1=[u1(4)+u1(5)+u1(6),angle(u1(1)/sqrt(1-u1(3)^2)+sqrt(-1)*u1(2)/sqrt(1-u1(3)^2)), angle(sqrt(1-u1(3)^2)+sqrt(-1)*u1(3))]';
u2=-u1;
up2=[u2(4)+u2(5)+u2(6),angle(u2(1)/sqrt(1-u2(3)^2)+sqrt(-1)*u2(2)/sqrt(1-u2(3)^2)), angle(sqrt(1-u2(3)^2)+sqrt(-1)*u2(3))]';
rd1=G1*u1;
rd2=G1*u2;
e1=norm(rd1-rd,2);
e2=norm(rd2-rd,2);
if e1<e2
    up=up1;
else
    up=up2;
end

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