function [uQ,ut] = AOARBL3D_BRPLE(Ain,Cin,thetam0,phim0,Qn0,Loc)
% [uQ,ut] = AOARBL3D_BRPLE(Ain,Cin,thetam0,phim0,Qn0,Loc)
%
% Estimate the rotation matrix and the translation vector for the localization 
% of a rigid body in 3-D using the BRPLE method from the second reference
% paper below
%
% Input:
%   Ain:    (3 x M), anchor positions in global coord, M = number of anchors
%   Cin     (3 x N), sensor positions in local coord., N = number of sensors
%   thetam0:(N x M), azimuths of AOA measurements 
%   phim0:  (N x M), elevations of AOA measurements 
%   Qn0:    (2MN x 2MN), AOA covariance matrix
%   Loc:    scalar, Self-Localization (Loc=0), Localization(Loc=1) 
%
% Output:
%   uQ:     (3 x 3), rotation matrix estimate
%   ut:     (3 x 1), translation vector estimate
%
% References:
% Y. Wang, G. Wang, S. Chen, K. C. Ho and L. Huang, "An investigation and 
% solution of angle based rigid body localization," IEEE Trans. Signal 
% Process., vol. 68, pp. 5457-5472, 2020.
% Y. Wang and K. C. Ho, ¡°An asymptotically efficient estimator in closed-form 
% for 3D AOA localization using a sensor network,¡± IEEE Trans. Wireless 
% Commun., vol. 14, no. 12, pp. 6524-6535, Dec. 2015.
%
% Yue Wang, K. C. Ho   07-18-2021
%
%       Copyright (C) 2020
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

if Loc==0
    a=Ain; c=Cin;
else
    a=Cin; c=Ain;
end;

thetam0=reshape(thetam0',prod(size(thetam0)),1);  
phim0=reshape(phim0',prod(size(phim0)),1);      
[thetam,phim,Qn]=reorder(a,c,thetam0,phim0,Qn0);

% N=size(c,1);
N=size(c,2);
M=size(a,2);
thetamm=reshape(thetam,M,N);
phimm=reshape(phim,M,N);
idxm1=reshape([1:M*N]',M,N);
idxm2=reshape([M*N+1:2*M*N]',M,N);
idxm=[idxm1,idxm2];

for i=1:M
    theta1=thetamm(i,:)';
    phi1=phimm(i,:)';
    for j=1:2*N
        for k=1:2*N
            Q1(j,k)=Qn(idxm(i,j),idxm(i,k));
        end
    end
    u=AOA3DLoc_BRPLE(theta1,phi1,c,Q1);
    a_beta(:,i)=u;
end
a_rel= a - mean(a')'*ones(1,M);
a_betar_rel = a_beta - mean(a_beta')'*ones(1,M);
[U Sigma V]= svd( a_rel * a_betar_rel');
SigmaModified = diag( [ones(size(Sigma,1)-1,1); sign(det(V*U'))] );
uQ = V*SigmaModified*U';
ut = mean(a_beta')' - uQ*mean(a')';

% ---------------------------------------------------
function [thetam,phim,Qn]=reorder(a,c,thetam0,phim0,Qn0)

N=size(c,2);
M=size(a,2);

thetamm=reshape(thetam0,N,M);
thetam=reshape(thetamm',M*N,1);
phimm=reshape(phim0,N,M);
phim=reshape(phimm',M*N,1);

idxm1=reshape([1:M*N]',N,M);
idx1=reshape(idxm1',M*N,1);
idxm2=reshape([M*N+1:2*M*N]',N,M);
idx2=reshape(idxm2',M*N,1);
idx=[idx1;idx2];
Qn=zeros(2*M*N,2*M*N);
for i=1:2*M*N
    for j=1:2*M*N
        Qn(i,j)=Qn0(idx(i),idx(j));
    end
end

% -------------------------------------------------
function [uBR]=AOA3DLoc_BRPLE(theta,phi,s,Q)

M=size(s,2); 
K=size(s,1); 
iterN=1; 
for i=1:M
    a0(i,:)=[-sin(theta(i)),cos(theta(i)),0];
    a1(i,:)=[-sin(phi(i))*cos(theta(i)),-sin(phi(i))*sin(theta(i)),cos(phi(i))];
    
    b0(i)=a0(i,:)*s(:,i);
    b1(i)=a1(i,:)*s(:,i);
        
    wtn(i,:)=[-cos(theta(i)),-sin(theta(i)),0];
    wto(i,:)=zeros(1,3);
    wpn(i,:)=[sin(phi(i))*sin(theta(i)), -sin(phi(i))*cos(theta(i)),0];
    wpo(i,:)=[-cos(phi(i))*cos(theta(i)), -cos(phi(i))*sin(theta(i)), -sin(phi(i))];
    
    u0(i,:)=[-wtn(i,:) wtn(i,:)*s(:,i)];
    v0(i,:)=[-wto(i,:) wto(i,:)*s(:,i)];
    u1(i,:)=[-wpn(i,:) wpn(i,:)*s(:,i)];
    v1(i,:)=[-wpo(i,:) wpo(i,:)*s(:,i)];
end
A1=[a0;a1];
B1=[b0,b1]';
A=[-A1,B1];

X0=[u0;v0];
X1=[u1;v1];
X=[X0;X1];

for n=1:iterN+1,
    if (n==1)
        W=eye(2*M);
    else
        d1=sqrt(sum((uBR*ones(1,M)-s).*(uBR*ones(1,M)-s)));
        D0n=diag(d1'.*cos(phi));
        D0w=zeros(M,M);
        D1n=zeros(M,M);
        D1w=diag(d1);
        D=[D0n,D0w
            D1n,D1w];
        W=inv(D*Q*D');
    end
    
    W11=W(1:M,1:M);
    W12=W(1:M,M+1:2*M);
    W21=W(M+1:2*M,1:M);
    W22=W(M+1:2*M,M+1:2*M);
    Q11=Q(1:M,1:M);
    Q12=Q(1:M,M+1:2*M);
    Q21=Q(M+1:2*M,1:M);
    Q22=Q(M+1:2*M,M+1:2*M);
    k1=K-1;                                                           
    QW=kron(ones(k1,k1),Q).*[kron(ones(k1,k1),W11) kron(ones(k1,k1),W12);kron(ones(k1,k1),W21) kron(ones(k1,k1),W22)];
    R=A'*W*A;
    Omega=X'*QW*X;
    
    [V,D2]=eig(R,Omega);
    [d,IX]=sort(sum(D2));
    uBR=V(1:3,IX(1))/V(4,IX(1));
end

% ---------------------------------------------
