function theta=DSF3DLoc_CFS(f,fc,S,c,Qn,Qfc,Qs,wghIdc)
% theta=DSF3DLoc_CFS(f,fc,S,c,Qn,Qfc,Qs,wghIdc)
%
% Closed-form solution (CFS) for obtaining the object position
% and velocity.
%
% Input:
% f:      (M x 1), DSF measurement vector, M=num. of sensors.
% fc:     scalar, center frequency.
% S:      (3 x M), sensor poition matrix.
% c:      scalar, signal propagation speed.
% Qn:     (M x M), covariance matrix of DSF measurements.
% Qfc:    scalar, variance of center frequency.
% Qs:     (3M x 3M), covariance matrix of sensor positions.
% wghIdc: scalar, weighting matrix indicator for different mix of errors
%         wghIdc=0   No weighting matrix
%         wghIdc=1   weighting matrix for measurement errors only
%         wghIdc=2   weighting matrix for measurement and sensor pos. errors
%         wghIdc=3   weighting matrix for measurement and source freq. errors
%         otherwise  Full weighting matrix
%
% Output:
% theta:  (6 x 1), solution, [objPos;objVel].
%
% Reference: M. H. Ahmed, K. C. Ho, and G. Wang, "3-D target localization
% and motion analysis based on Doppler shifted frequencies," IEEE Trans.
% Aerosp. Electron. Syst., vol. 58, no. 2, pp. 815-833, Apr. 2022.
%
% M. H. Ahmed, K. C. Ho     10-28-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

S=S';
M=length(S);
r=c*(1-f/fc);

% -- Stage 1 --
h1=zeros(M,1);A1=zeros(M,14);
r2=r.^2;
for i=1:M
    h1(i,1)=r2(i)*(S(i,:)*S(i,:)');
    A1(i,:)=[2*r2(i)*S(i,:) S(i,:).^2 -r2(i) 2*S(i,1)*S(i,2) 2*S(i,1)*S(i,3) 2*S(i,2)*S(i,3) -2*S(i,:) 1];
end
phi_1=(A1'*A1)\A1'*h1;

% -- resolve velocity sign --
dn=sqrt(phi_1(7)+sum(S.^2,2)-2*S*phi_1(1:3));
f_est1=fc-fc/c*(((repmat(phi_1(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 -1 0;0 0 -1]*sqrt(phi_1(4:6)));
f_est2=fc-fc/c*(((repmat(phi_1(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 -1 0;0 0 -1]*sqrt(phi_1(4:6)));
f_est3=fc-fc/c*(((repmat(phi_1(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 1 0;0 0 -1]*sqrt(phi_1(4:6)));
f_est4=fc-fc/c*(((repmat(phi_1(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 1 0;0 0 -1]*sqrt(phi_1(4:6)));

f_est5=fc-fc/c*(((repmat(phi_1(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 -1 0;0 0 1]*sqrt(phi_1(4:6)));
f_est6=fc-fc/c*(((repmat(phi_1(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 -1 0;0 0 1]*sqrt(phi_1(4:6)));
f_est7=fc-fc/c*(((repmat(phi_1(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 1 0;0 0 1]*sqrt(phi_1(4:6)));
f_est8=fc-fc/c*(((repmat(phi_1(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 1 0;0 0 1]*sqrt(phi_1(4:6)));

cost1=(f-f_est1)'/Qn*(f-f_est1);
cost2=(f-f_est2)'/Qn*(f-f_est2);
cost3=(f-f_est3)'/Qn*(f-f_est3);
cost4=(f-f_est4)'/Qn*(f-f_est4);
cost5=(f-f_est5)'/Qn*(f-f_est5);
cost6=(f-f_est6)'/Qn*(f-f_est6);
cost7=(f-f_est7)'/Qn*(f-f_est7);
cost8=(f-f_est8)'/Qn*(f-f_est8);

code=[-1 -1 -1;1 -1 -1;-1 1 -1;1 1 -1;-1 -1 1;1 -1 1;-1 1 1;1 1 1];

[~,idx]=min([cost1 cost2 cost3 cost4 cost5 cost6 cost7 cost8]);
L=diag(code(idx,:));

iW1=DSF3DLoc_Weight(f,fc,S',c,phi_1(1:3),L*sqrt(phi_1(4:6)),Qn,Qfc,Qs,wghIdc);
phi_1=(A1'/iW1*A1)\A1'/iW1*h1;
% [iW1,tp]=DSF3DLoc_Weight(f,fc,S',c,phi_1(1:3),L*sqrt(phi_1(4:6)),Qn,Qfc,Qs,wghIdc);
% phi_1=(A1'*tp*A1)\A1'*tp*h1;


% -- Stage 2 --
h2=[phi_1(1:7);phi_1(8:10).^2;phi_1(11:14)];

A2=zeros(14,6);
A2(1:6,1:6)=eye(6);
A2(7,1:3)=phi_1(1:3)';
A2(8,4:6)=0.5*[phi_1(5) phi_1(4) 0];
A2(9,4:6)=0.5*[phi_1(6) 0 phi_1(4)];
A2(10,4:6)=0.5*[0 phi_1(6) phi_1(5)];
A2(11,1:3)=[phi_1(4) phi_1(8) phi_1(9)];
A2(12,1:3)=[phi_1(8) phi_1(5) phi_1(10)];
A2(13,1:3)=[phi_1(9) phi_1(10) phi_1(6)];
A2(14,1:3)=phi_1(11:13)';

for t=1:3
    B2=eye(14);
    B2(7,1:3)=-phi_1(1:3)';
    B2(8,4:8)=[-0.5*phi_1(5) -0.5*phi_1(4) 0 0 2*phi_1(8)];
    B2(9,4:9)=[-0.5*phi_1(6) 0 -0.5*phi_1(4) 0 0 2*phi_1(9)];
    B2(10,5:10)=[-0.5*phi_1(6) -0.5*phi_1(5) 0 0 0 2*phi_1(10)];
    B2(11,4:9)=[-phi_1(1) 0 0 0 -phi_1(2) -phi_1(3)];
    B2(12,5:10)=[-phi_1(2) 0 0 -phi_1(1) 0 -phi_1(3)];
    B2(13,6:10)=[-phi_1(3) 0 0 -phi_1(1) -phi_1(2)];
    B2(14,11:13)=-phi_1(1:3)';
    iW2=B2/(A1'/iW1*A1)*B2';iW2=iW2/max(iW2(:));
%     iW2=B2/(A1'*tp*A1)*B2';iW2=iW2/max(iW2(:));
    phi_2=(A2'/iW2*A2)\(A2'/iW2*h2);
    phi_1(1:6)=phi_2;
end

% -- resolve velocity sign --
dn=sqrt(sum((repmat(phi_2(1:3)',M,1)-S).^2,2));

f_est1=fc-fc/c*(((repmat(phi_2(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 -1 0;0 0 -1]*sqrt(phi_2(4:6)));
f_est2=fc-fc/c*(((repmat(phi_2(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 -1 0;0 0 -1]*sqrt(phi_2(4:6)));
f_est3=fc-fc/c*(((repmat(phi_2(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 1 0;0 0 -1]*sqrt(phi_2(4:6)));
f_est4=fc-fc/c*(((repmat(phi_2(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 1 0;0 0 -1]*sqrt(phi_2(4:6)));
f_est5=fc-fc/c*(((repmat(phi_2(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 -1 0;0 0 1]*sqrt(phi_2(4:6)));
f_est6=fc-fc/c*(((repmat(phi_2(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 -1 0;0 0 1]*sqrt(phi_2(4:6)));
f_est7=fc-fc/c*(((repmat(phi_2(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 1 0;0 0 1]*sqrt(phi_2(4:6)));
f_est8=fc-fc/c*(((repmat(phi_2(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 1 0;0 0 1]*sqrt(phi_2(4:6)));

cost1=(f-f_est1)'/Qn*(f-f_est1);
cost2=(f-f_est2)'/Qn*(f-f_est2);
cost3=(f-f_est3)'/Qn*(f-f_est3);
cost4=(f-f_est4)'/Qn*(f-f_est4);
cost5=(f-f_est5)'/Qn*(f-f_est5);
cost6=(f-f_est6)'/Qn*(f-f_est6);
cost7=(f-f_est7)'/Qn*(f-f_est7);
cost8=(f-f_est8)'/Qn*(f-f_est8);

code=[-1 -1 -1;1 -1 -1;-1 1 -1;1 1 -1;-1 -1 1;1 -1 1;-1 1 1;1 1 1];

[~,idx]=min([cost1 cost2 cost3 cost4 cost5 cost6 cost7 cost8]);
L=diag(code(idx,:));

theta=[phi_2(1:3);L*sqrt(phi_2(4:6))];

end

