function iW=DSF3DTMA_weight(f,fc,S,N,c,T,p,v,Qn,Qfc,Qs,wghIdc)
% iW=DSF3DTMA_weight(f,fc,S,N,c,T,p,v,Qn,Qfc,Qs,wghIdc)
%
% Weight generation for estimating the initial position and velocity 
% for TMA.
%
% Input:
% f:      (MN x 1), DSF measurement vector, 
%         (M=num. of sensors, N=num. of time measurements).
% fc:     scalar, center frequency.
% S:      (3 x M), sensor poition matrix.
% N:      scalar, number of time measurements.
% c:      scalar, signal propagation speed.
% T:      scalar, time measurement interval.
% p:      (3 x 1), object position.
% v:      (3 x 1), object velocity.
% Qn:     (MN x MN), 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:
% iW:  (MN x MN), inverse of weighting matrix.
%
% 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);
p_all=p*ones(1,N)+v*(0:N-1)*T;

dn=sqrt(sum((repelem(p_all',M,1)-repmat(S,N,1)).^2,2));
r=c*(1-f/fc);

% -- reconstruct DSFs for weights with sensor position errors --
f=fc-fc/c*(((repelem(p_all',M,1)-repmat(S,N,1))./repmat(dn,1,3))*v);

B1=-2*diag((r.*dn.^2));
Ds1=-fc/c*v(1)./dn-(f-fc)./dn.^2.*(repelem(p_all(1,:)',M,1)-repmat(S(:,1),N,1));
Ds2=-fc/c*v(2)./dn-(f-fc)./dn.^2.*(repelem(p_all(2,:)',M,1)-repmat(S(:,2),N,1));
Ds3=-fc/c*v(3)./dn-(f-fc)./dn.^2.*(repelem(p_all(3,:)',M,1)-repmat(S(:,3),N,1));
for n=0:N-1
    Dsa=Ds1(n*M+1:M+n*M);Dsa=diag(Dsa);
    Dsb=Ds2(n*M+1:M+n*M);Dsb=diag(Dsb);
    Dsc=Ds3(n*M+1:M+n*M);Dsc=diag(Dsc);
    Dsd= Dsc(:,[1;1;1]*(1:size(Dsc,2)));Dsd(:,1:3:end) = Dsa;Dsd(:,2:3:end) = Dsb;
    Ds(n*M+1:M+n*M,:)=Dsd;
end
Dfo=-f/fc;

switch wghIdc
    case 0
        iW=eye(N*M);
    case 1
        iW=B1*Qn*B1';
    case 2
        iW=B1*(Ds*(Qs)*Ds'+Qn)*B1';
    case 3
        iW=B1*(Dfo*Qfc*Dfo'+Qn)*B1';
    otherwise
        iW=B1*(Ds*Qs*Ds'+Dfo*Qfc*Dfo'+Qn)*B1';
end

iW=iW/max(iW(:));

end
