% doadec1, simplified version from doadec.m for online access
% DOA detection using fewer sensors

clear
% Parameter setup &^%$$$$ - start
T=100;         % no. of snapshots
snr=0;         % signal to nosie ratio
thres=25;       % t in (3.12) of Chen -IEEE-T-SP-1991
thr_lr=0.985;   % alpha for the LR algorithm
% Parameter setup &^%$$$$ - end

N=6;            % no. of sensors
M=8;            % no. of DOAs
Nd=11;          % maximum value of the half-wavelength distance 
Nsim=1000;      % no. of simulation runs
tol=0.2;        % maximum increment on the norm of eigenvalues of G matrix
seps=1; 

Rs_p_est=zeros(12,12);  rtrmv=zeros(Nsim,1); rtrv=zeros(Nsim,1);
rt01=zeros(Nsim,1);     % T0 
rt0=zeros(Nsim,1);      % loaded
rt01_opt=zeros(Nsim,1); 
lrtr=zeros(Nsim,1); lr0=zeros(Nsim,1);  lr01=zeros(Nsim,1);  % for loaded T 
lrmlu=zeros(Nsim,1); lrmlu1=zeros(Nsim,1); lru=zeros(Nsim,1);  lrul=zeros(Nsim,1);
lr0_opt=zeros(Nsim,1); lrml=zeros(Nsim,1); lrml1=zeros(Nsim,1); lrmlc=zeros(Nsim,1);
lrr=zeros(Nsim,1); lrmlmu=zeros(Nsim,11);
mval=zeros(1000,12);  % matrix for condition numbers

% T0 determined from linear programming and possibly from negative eigenvalue extraction
rtmlmu=zeros(Nsim,1);  % effective rank of Tml
niter_0=zeros(Nsim,1); niter_mlmu=zeros(Nsim,1);
rtmlu=zeros(Nsim,1); niter_mlu=zeros(Nsim,1); rtmlu1=zeros(Nsim,1); niter_mlu1=zeros(Nsim,1);
rtml=zeros(Nsim,1); niter_ml=zeros(Nsim,1); rtml1=zeros(Nsim,1); niter_ml1=zeros(Nsim,1);
rtmlc=zeros(Nsim,1);
rtu=zeros(Nsim,1);  niter_0_opt=zeros(Nsim,1);

rgmlu=0; rgmlu1=0; rgml=0; rgml1=0;

doa=[-65,-45,-30,0,15,30,45,60]
theta=(2*pi/2)*sin(doa*pi/180);
dist=[1,2,3,4,8,12];
A=exp(j*dist.'*theta);

Nsq=N*N; Nsq2=2*Nsq; Nsqh=N*(N-1)/2; L=Nsq/2+N-1; Lent=(L+1)/2;
theta_est_dec=zeros(1,11);
doa_est=zeros(Nsim,M); doa_est_ls=zeros(Nsim,M); doa_est_iqls=zeros(Nsim,M);
doa_est_daa=zeros(Nsim,M); doa_est_aml=zeros(Nsim,M); doa_est_sst=zeros(Nsim,M);
rmse=zeros(M,1); rmse_ls=zeros(M,1); rmse_iqls=zeros(M,1); rmse_daa=zeros(M,1);
rmse_aml=zeros(M,1);  rmse_sst=zeros(M,1);
mdl=zeros(11,1); mdl_p=zeros(11,1);
% mdl_ss=zeros(11,1);     % Second-order Statistics
Rs_est=zeros(11,11); Rs_p_est=zeros(11,11); 
   
Mest_th=zeros(Nsim,1); Mest_mdl=zeros(Nsim,1); Mest_ss=zeros(Nsim,1);
Mest_mdl_p=zeros(Nsim,1); Mest_lr=zeros(Nsim,1);
Mest_mdl_nla=zeros(Nsim,1); Mest_mdl_p_nla=zeros(Nsim,1);
ratio_mest=zeros(Nsim,11);
sig2est=zeros(Nsim,1);

sig2=10^(-snr/10)

Iden=eye(N);
R=A*A'+sig2*eye(N);
Rvec=zeros(N*N,1);  Idenvec=zeros(N*N,1);
for k=1:N
    Rvec(1+(k-1)*N:N+(k-1)*N)=R(:,k);
    Idenvec(1+(k-1)*N:N+(k-1)*N)=Iden(:,k);
end

Adot=zeros(Nsq,M);
for k=1:M
    Adot(:,k)=kron(conj(A(:,k)),A(:,k));
end

angdif=zeros(Nsq,1);
for k=1:N
    angdif(1+(k-1)*N:N+(k-1)*N)=dist-dist(k);
end
[angdif_sort,ind_sort]=sort(angdif);
angdif_sort_r=zeros(23,1); ind_sort_r=zeros(23,1);
ind=1;
angdif_sort_r(1)=angdif_sort(1); ind_sort_r(1)=ind_sort(1);
for k=2:Nsq
    if (angdif_sort(k)>angdif_sort(k-1))
      %  disp0=[k,ind,angdif_sort(k),angdif_sort(ind)]
        ind=ind+1;
        angdif_sort_r(ind)=angdif_sort(k); ind_sort_r(ind)=ind_sort(k);
    end
end

Rvec_r=zeros(23,1);
for k=1:23
    Rvec_r(k)=Rvec(ind_sort_r(k));
end
Rvec_r(8)=(Rvec_r(8)+Rvec(ind_sort(9)))/2;
Rvec_r(10)=(Rvec_r(10)+Rvec(ind_sort(12)))/2;
Rvec_r(11)=(Rvec_r(11)+Rvec(ind_sort(14))+Rvec(ind_sort(15)))/3;
med=0;
for kk=17:21
    med=med+Rvec(ind_sort(kk));
end
Rvec_r(12)=(Rvec_r(12)+med)/6;
Rvec_r(13)=(Rvec_r(13)+Rvec(ind_sort(23))+Rvec(ind_sort(24)))/3;
Rvec_r(14)=(Rvec_r(14)+Rvec(ind_sort(26)))/2;
Rvec_r(16)=(Rvec_r(16)+Rvec(ind_sort(29)))/2;

Idenvec_r=zeros(23,1); Idenvec_r(12)=1;
Aexp=exp(j*angdif_sort_r*theta);
% Rvec_r_test=Aexp*ones(M,1)+0*sig2*Idenvec_r;
% disp3=[Rvec_r-sig2*Idenvec_r,Rvec_r_test]

Rss=zeros(12,12);
for k=1:12
    Rss=Rss+Rvec_r(12-k+1:23-k+1)*Rvec_r(12-k+1:23-k+1)';
end
[Urss,Srss,Vrss]=svd(Rss);
% disp4=diag(Srss).'
Un=Urss(:,9:12);

acoe=zeros(23,1);
Run=Un*Un';
for k=1:11
    for m=1:k
        acoe(k)=acoe(k)+Run(12-k+m,m);
    end
    acoe(23-k+1)=conj(acoe(k));
end
for k=1:12
    acoe(12)=acoe(12)+Run(k,k);
end
root_a=sort(roots(acoe));
root_a_mag=abs(root_a); root_a_phase=angle(root_a);
% disp5=[root_a,root_a_mag,root_a_phase]
theta_1=sort(-root_a_phase(11-M+1:11));
% disp=[theta; theta_1.']

% Check for IQLS
sig2_est_0=sum(diag(sqrt(Srss(M+1:12,M+1:12))))/(12-M);
Rvec_r_0=Rvec_r; Rvec_r_0(12)=Rvec_r(12)-sig2_est_0;
L=Nsq/2+N-1;
% Paperp=eye(L)-Aexp*inv(Aexp'*Aexp)*Aexp';
% test5=norm(Paperp*Rvec_r_0)
Gz=zeros(L-M,M+1); 
for k=1:L-M
    Gz(k,1:M+1)=Rvec_r_0(k:k+M,1).';
end
Rz=Gz'*Gz;
[Urz,Sig_rz,Vrz]=svd(Rz);
root_rz0=sort(roots(Urz(:,M+1)));
root_rz0_mag=abs(root_rz0); root_rz0_phase=angle(root_rz0);
% disp5=[root_rz0,root_rz0_mag,root_rz0_phase]

B1=zeros(L,L-M);
for k=1:L-M
    B1(k:k+M,k)=Urz(:,M+1);
end
% testB=norm(B1.'*Aexp)

% theta_rz=sort(-root_a_phase(11-M+1:11));

% DAA method - start *&^%$
Lent=(L+1)/2;
Tr=zeros(Lent,Lent);
for k=1:Lent
    for ki=1:Lent-k+1
        disp=[k,ki,ki+k-1];
        Tr(ki,ki+k-1)=Rvec_r(12-k+1); Tr(ki+k-1,ki)=conj(Rvec_r(12-k+1));
    end
end
[Utr,Str,Vtr]=svd(Tr);
% disp_rank_tr=diag(Str);
% test_tr=norm(Utr(:,9:12)'*Aexp(1:12,:))

acoe=zeros(23,1);
Run=Utr(:,9:12)*Utr(:,9:12)';
for k=1:11
    for m=1:k
        acoe(k)=acoe(k)+Run(12-k+m,m);
    end
    acoe(23-k+1)=conj(acoe(k));
end
for k=1:12
    acoe(12)=acoe(12)+Run(k,k);
end
root_a=sort(roots(acoe));
root_a_mag=abs(root_a); root_a_phase=angle(root_a);
% disp5=[root_a,root_a_mag,root_a_phase]
%  theta_daa=sort(-root_a_phase(11-M+1:11));
%  disp_daa=[theta; theta_daa.']

% DAA method - end *&^%$


% AML method ** start ^^^^^
Jm=zeros(N,N,Nd+1); 
Jm(:,:,1)=eye(N);   
Jm(1,2,2)=1; Jm(2,3,2)=1; Jm(3,4,2)=1;
Jm(1,3,3)=1; Jm(2,4,3)=1;   
Jm(1,4,4)=1; 
Jm(4,5,5)=1; Jm(5,6,5)=1;   
Jm(3,5,6)=1;     
Jm(2,5,7)=1; 
Jm(1,5,8)=1; 
Jm(4,6,9)=1; 
Jm(3,6,10)=1; 
Jm(2,6,11)=1; 
Jm(1,6,12)=1; 
rho=zeros(12,1);
for k=1:12
    rho(k)=Rvec_r(12-k+1); 
end
R_aml_t=zeros(N,N);
R_aml_t=R_aml_t+Jm(:,:,1)*rho(1);
for k=1:11
    R_aml_t=R_aml_t+Jm(:,:,k+1)*rho(k+1)+(Jm(:,:,k+1)*rho(k+1))';
end
% test__aml_t=norm(R-R_aml_t)
Sigm=zeros(Nsq,23);
for k1=1:N
    Sigm(1+(k1-1)*N:k1*N,1)=Jm(1:N,k1,1);
end
for k=1:11
    for k1=1:N
        Sigm(1+(k1-1)*N:k1*N,1+1+(k-1)*2)=Jm(1:N,k1,k+1);
        Sigm(1+(k1-1)*N:k1*N,1+k*2)=Jm(k1,1:N,k+1).';
    end
end
Omega=zeros(23,23);
Omega(1,1)=1; 
for k=1:11
    Omega(1+1+(k-1)*2:1+k*2,1+1+(k-1)*2:1+k*2)=[1 j; 1 -j];
end
Psi=Sigm*Omega;
rho_e=zeros(23,1);
rho_e(1)=rho(1);
for k=1:11
    rho_e(1+1+(k-1)*2:1+k*2)=[real(rho(k+1)); imag(rho(k+1))]; 
end
Rvec_aml_t=Psi*rho_e;
% test__aml_t=norm(Rvec-Rvec_aml_t)
Cz1=kron(conj(R),R);
phi_est=inv(Psi'*Cz1*Psi)*Psi'*Cz1*Rvec;
% test__aml_phi=norm(rho_e-phi_est)
rho_est=zeros(12,1);
rho_est(1)=phi_est(1);
for k=1:11
    rho_est(k+1)=phi_est(2+(k-1)*2)+j*phi_est(1+k*2);
end
% test__aml_rho=norm(rho-rho_est)

% AML method ** end ^^^^^



% Covariance matrix of 4th order statistics

Cz1=kron(conj(R),R);
B=zeros(Nsq,Nsq);
for k=1:N
    ks=1+(k-1)*N; kf=N+(k-1)*N;
    for l=1:N
        ls=1+(l-1)*N; lf=N+(l-1)*N;
        B(ks:kf,ls:lf)=Iden(:,l)*Iden(:,k).';
    end
end
Cz2=kron(Iden,R)*B*kron(Iden,conj(R));
[eb,sb]=eig(B);
disp00=diag(sb).';

P0=zeros(N,Nsq); P1=zeros(Nsqh,Nsq);
for k=1:N-1
    ks=1+(k-1)*N; kf=N+(k-1)*N;
    rowsh=(N-k/2)*(k-1); rows=rowsh+1; rowf=rowsh+N-k;
    P0(k,ks:kf)=Iden(k,:);
    P1(rows:rowf,ks:kf)=Iden(k+1:N,:);
    rowsh=(N-(k+1)/2)*k; 
end
for k=N:N
    ks=1+(k-1)*N; kf=N+(k-1)*N;
    P0(k,ks:kf)=Iden(k,:);
end
Q=zeros(Nsq2,Nsq2);
Q(1:Nsq,1:Nsq)=real(Cz1+Cz2)/2; Q(1+Nsq:Nsq2,1+Nsq:Nsq2)=real(Cz1-Cz2)/2; 
Q(1:Nsq,1+Nsq:Nsq2)=imag(Cz1-Cz2)/2; Q(1+Nsq:Nsq2,1:Nsq)=-imag(Cz1+Cz2)/2;
P=zeros(Nsq,Nsq2);
P(1:N,1:Nsq)=P0; 
P(N+1:N+Nsqh,1:Nsq)=P1; 
P(N+Nsqh+1:Nsq,1+Nsq:Nsq2)=P1; 
C=P*Q*P.';
[Uc,Sc,Vc]=svd(C);
% diag(Sc).'



%=============Simulation======

Rhatv=zeros(Nsq,1);
J=zeros(12,12);
for k=1:12
    J(k,12-k+1)=1;
end
randn('seed',0)
for in=1:Nsim
    
    sm=(randn(M,T)+j*randn(M,T))/sqrt(2); 
    nm=sqrt(sig2/2)*(randn(N,T)+j*randn(N,T));
    X=A*sm+nm;
    Rhat=X*X'/T;  % only works for non-correlated sources
    for k=1:N
        Rhatv(1+(k-1)*N:N+(k-1)*N)=Rhat(:,k);
        %  Rhatv(1+(k-1)*N:N+(k-1)*N)=R(:,k);
    end
%test2=norm(R-Rhat);

Rhatv_r=zeros(23,1);
for k=1:23
    Rhatv_r(k)=Rhatv(ind_sort_r(k));
end
Rhatv_r(8)=(Rhatv_r(8)+Rhatv(ind_sort(9)))/2;
Rhatv_r(10)=(Rhatv_r(10)+Rhatv(ind_sort(12)))/2;
Rhatv_r(11)=(Rhatv_r(11)+Rhatv(ind_sort(14))+Rhatv(ind_sort(15)))/3;
med=0;
for kk=17:21
    med=med+Rhatv(ind_sort(kk));
end
Rhatv_r(12)=(Rhatv_r(12)+med)/6;
Rhatv_r(13)=(Rhatv_r(13)+Rhatv(ind_sort(23))+Rhatv(ind_sort(24)))/3;
Rhatv_r(14)=(Rhatv_r(14)+Rhatv(ind_sort(26)))/2;
Rhatv_r(16)=(Rhatv_r(16)+Rhatv(ind_sort(29)))/2;

% Subspace method  - start &&^%
Rss=zeros(12,12);
for k=1:12
   Rss=Rss+Rhatv_r(12-k+1:23-k+1)*Rhatv_r(12-k+1:23-k+1)';
  %   Rss=Rss+Rvec_r(12-k+1:23-k+1)*Rvec_r(12-k+1:23-k+1)';   % Accurate covariance matrix
end
[Urss,Srss,Vrss]=svd(Rss);
p0=sqrt(Srss(12,12));

Un=Urss(:,9:12);
test8=norm(Aexp(1:12,:)'*Un);

acoe_ss=zeros(23,1);
Run=Un*Un';
for k=1:11
    for m=1:k
        acoe_ss(k)=acoe_ss(k)+Run(12-k+m,m);
    end
    acoe_ss(23-k+1)=conj(acoe_ss(k));
end
for k=1:12
    acoe_ss(12)=acoe_ss(12)+Run(k,k);
end
root_hat=sort(roots(acoe_ss));
root_hat_mag=abs(root_hat); root_hat_phase=angle(root_hat);
% disp9=[root_hat,root_hat_mag,root_hat_phase]
theta_est=sort(-root_hat_phase(11-M+1:11));
% disp_hat=[theta; theta_est.']
doa_est(in,:)=asin(theta_est/pi)*180/pi;

% Subspace method  - end &&^%

% DAA method - start *&^%$
Lent=(L+1)/2;
Tr=zeros(Lent,Lent);
for k=1:Lent
    for ki=1:Lent-k+1
     %   disp=[k,ki,ki+k-1];
        Tr(ki,ki+k-1)=Rhatv_r(12-k+1); Tr(ki+k-1,ki)=conj(Rhatv_r(12-k+1));
    end
end
[Utr,Str,Vtr]=svd(Tr);
% disp_rank=[diag(Str).'; diag(Srss).']
% test_tr=norm(Utr(:,9:12)'*Aexp(1:12,:))
acoe_daa=zeros(23,1);
Run_daa=Utr(:,9:12)*Utr(:,9:12)';
for k=1:11
    for m=1:k
        acoe_daa(k)=acoe_daa(k)+Run_daa(12-k+m,m);
    end
    acoe_daa(23-k+1)=conj(acoe_daa(k));
end
for k=1:12
    acoe_daa(12)=acoe_daa(12)+Run_daa(k,k);
end
root_a=sort(roots(acoe_daa));
root_a_mag=abs(root_a); root_a_phase=angle(root_a);
% disp5=[root_a,root_a_mag,root_a_phase]
theta_est_daa=sort(-root_a_phase(11-M+1:11));
% disp_daa=[theta; theta_est_daa.']
 doa_est_daa(in,:)=asin(theta_est_daa/pi)*180/pi;
% disp_coe=[acoe_ss.'; acoe_daa.']
% test_daa_ss=norm(Tr*Tr-Rss)
 
% DAA method - end *&^%$

Ndiff=10;   % no. of t-loop 
Neps=11;    % no. of epsilon-loop 
[Urh,Srh,Vrh]=svd(Rhat); 
Rh=Urh*inv(sqrt(Srh))*Urh';  % squart root of Rh 
Rhi=Urh*sqrt(Srh)*Urh';  % inverse squart root of Rh 

% generation of initial Toeplitz matrix T0 
fv1=0;
mintr=min(min(abs(Tr))); ep1=seps*mintr; 
Trnew=Tr; diff=1; kdiff=0; 
Tr_tt=zeros(12,12,Ndiff); lr_t=zeros(Ndiff,1);
while (diff>0.1 & kdiff<Ndiff)   % t-loop
    [Utrn,Strn,reff_n]=evd(Trnew,Lent); reff_0=reff_n;
        Rest1=decttor(Trnew(1,:)); G=Rh*Rest1*Rh'; [Uge,Sge,reff_g]=evd(G,N);
        sgev=diag(Sge); lr_tloop=declr(sgev,N,T);
  % disp88_ini_tr0_before_eps0=[kdiff, diag(Strn).']
    [fv1,Tr_d1, ratio]=ini(Utrn,Strn,p0,ep1);
  % disp88_ini_tr0_before_eps1=[kdiff,fv1,ep1,ratio]
    k=1;
    while ( ratio>tol & k<Neps)   % epsilon loop
        [fv1,Tr_d1,ratio]=ini(Utrn,Strn,p0,ep1);
        % disp_ini_tr0_in_eps=[k,fv1,ep1,ratio]
        ep1=ep1/2;  k=k+1;
    end
    Tr_t=Trnew; % if ratio>tol, no update will be accepted.
    if (ratio<tol) 
        Tr_t=Trnew+Tr_d1; % if ratio<tol, no update will be accepted.
    end
    diff=norm(Tr_t-Trnew)/norm(Trnew); kdiff=kdiff+1; Tr_tt(:,:,kdiff)=Tr_t; 
% disp_99_tr0=[diff,fv1, ep1, ep1/mintr, kdiff]
    Trnew=Tr_t; ep1=seps*mintr;
        Rest1=decttor(Trnew(1,:)); G=Rh*Rest1*Rh'; [Uge,Sge,reff_g]=evd(G,N);
        sgev=diag(Sge); lr_t(kdiff)=declr(sgev,N,T);
 % [U1_0,S1_0,reff_0]=evd(Tr_t,Lent);
 %  disp88_ini_tr0_after_eps0=[in, kdiff, diff, diag(S1_0).']
end
niter_0(in)=kdiff;

for ki=1:kdiff
    if (lr_t(ki)>=1 | lr_t(ki)<=0) lr_t(ki)=0; end
end
[lr_max,k_max]=max(lr_t); Tr_01=Tr_tt(:,:,k_max); lr01(in)=lr_max;

[U1_0,S1_0,reff_0]=evd(Tr_01,Lent); rt01(in)=reff_0;
if (reff_0<12)
    Tr_01=Tr_01-(S1_0(12,12)-p0)*eye(12);
end
[U1_0,S1_0,reff_0]=evd(Tr_01,Lent); rt01(in)=reff_0;
% disp_rank_tr_0=reff_0
% If Tr_d1 is too small, Tr_0 still contains negative eigenvalues;
% However, if Tr_d1 is too large, the purterbation does NOT hold.
% A larger ep1 easily makes fv1>0. There are a few cases when the minimum
% eigenvalue is still negative. This is why reff_0<12 is tested as well.
Rtr_0=decttor(Tr_01(1,:)); G=Rh*Rtr_0*Rh'; [Uge,Sge,reff_g]=evd(G,N); 
sgev=diag(Sge); lr01(in)=declr(sgev,N,T);
% disp_lr_g_01=diag(Sge).'
% disp_lr_tr_01=diag(S1_0).'

[Utre,Stre,rtr]=evd(Tr,Lent);
rt0(in)=rtr; Tr_0=Tr;
if (rtr<12)
    Tr_0=Utre*(Stre-(Stre(12,12)-p0)*eye(12))*Utre';
end
[U1_0,S1_0,reff_0]=evd(Tr_0,Lent);
Rtr_0=decttor(Tr_0(1,:)); G=Rh*Rtr_0*Rh'; [Uge,Sge,reff_g]=evd(G,N); 
sgev=diag(Sge); lr0(in)=declr(sgev,N,T);
% disp_lr_g_0=diag(Sge).'
% disp_lr_tr_0=diag(S1_0).'

% sprintf('end of TR0 ini')

% Unsconstrained ML Toeplitz matrix Tml - "direct" optimization
mintr=min(min(abs(Tr_0))); ep1=seps*mintr;
Rest1=decttor(Tr_0(1,:)); G=Rh*Rest1*Rh'; [Uge,Sge,reff_g]=evd(G,N);
lambda0=Sge(6,6); lambda1=1/Sge(1,1); 
Trnew=Tr_0; diff=1; kdiff=1; 
Tr_tt=zeros(12,12,Ndiff); lr_t=zeros(Ndiff,1);
while (diff>0.1 & kdiff<Ndiff)   % t-loop
    [Utrn,Strn,reff_n]=evd(Trnew,Lent);
    Rest1=decttor(Trnew(1,:)); G=Rh*Rest1*Rh'; [Uge,Sge,reff_g]=evd(G,N);
    sgev=diag(Sge); lr_tloop=declr(sgev,N,T);
 %disp_EV_Trnew_before_eps_mlu=[in,kdiff,diag(Strn).']
 %disp_EV_g_before_eps_mlu=[in,kdiff,diag(Sge).']
 %disp_mlu_Tr_t_min_evalue=[S1_ml(11,11),S1_ml(12,12)]
    k=1; ratio=1;
    while (ratio>tol & k<Neps)   % epsilon loop
        [fv1,Tr_d1,R_d1,ratio]=decmlu(lambda0,Utrn,Strn,Uge,Sge,ep1,Rh);
        % Rtr_t=Rest1+R_d1; Tr_t=decrtot(Rtr_t,dist,Lent);
        Tr_t=Trnew+Tr_d1; Rtr_t=decttor(Tr_t(1,:)); 
        %disp_mlu_g_88_in_eps=[in,kdiff,k,ratio,ep1]
        ep1=ep1/2;  k=k+1;
    end
    G=Rh*Rtr_t*Rh'; [Uge,Sge,reff_g]=evd(G,N); 
  %disp_EV_g_after_eps_mlu=[in,kdiff,k,ratio,ep1, diag(Sge).']
     [Utrt,Strt,reff_t]=evd(Tr_t,Lent);
  %disp_EV_Trt_after_eps_mlu=[diag(Strt).']
    % disp_mlu_99=[diff,fv1, ep1, ep1/mintr, kdiff]
% finding the maximum likelihood ratio for the matrices in t-loop -start
    diff=0;     
    % t-loop stops if the last run of esp-loop still offers an increment more than 10%
    if (ratio<tol) 
        sgev=diag(Sge); lr_eps=declr(sgev,N,T); lr_t(kdiff)=lr_eps;
        diff=norm(Tr_t-Trnew)/norm(Trnew);
        Tr_tt(:,:,kdiff)=Tr_t; 
        if (lr_eps<=lr_tloop)
            Tr_tt(:,:,kdiff)=Trnew; lr_t(kdiff)=lr_tloop;  
        end
        Trnew=Tr_t;
    end
% finding the maximum likelihood ratio for the matrices in t-loop -end
   % disp_mlu_g_88_in_tloop=[in,kdiff,ratio,ep1, diff]
    % disp_mlu_lr_99=[kdiff,k,lr_eps.',lr_tloop,lr0(in)]
    ep1=seps*mintr; kdiff=kdiff+1;
end

for ki=1:kdiff
    if (lr_t(ki)>=1 | lr_t(ki)<=0) lr_t(ki)=0; end
end
[lr_max,k_max]=max(lr_t); Tr_mlu=Tr_tt(:,:,k_max) ; lrmlu(in)=lr_t(k_max);
[U1_ml,S1_ml,reff_ml]=evd(Tr_mlu,Lent);
% disp_ev_mlu=diag(S1_ml).';
rtmlu(in)=reff_ml; niter_mlu(in)=kdiff;


%  Unsconstrained ML Toeplitz matrix Tml - "inverse" optimization
mintr=min(min(abs(Tr_0))); ep1=seps*mintr; 
Trnew=Tr_0; diff=1; kdiff=1; 
Tr_tt=zeros(12,12,Ndiff); lr_t=zeros(Ndiff,1);
while (diff>0.1 & kdiff<Ndiff)   % t-loop
    [Utrn,Strn,reff_n]=evd(Trnew,Lent);
    Rest1=decttor(Trnew(1,:)); G=Rh*Rest1*Rh'; [Uge,Sge,reff_g]=evd(G,N); 
    sgev=diag(Sge); lr_tloop=declr(sgev,N,T);
    % disp_mlu1_g_before_eps1=[kdiff,diag(Sge).']
    k=1; ratio=1; 
    while (ratio>tol & k<Neps)   % epsilon loop
        [fv1,Tr_d1,R_d1,ratio]=decmlu1(lambda1,Utrn,Strn,Uge,inv(Sge),ep1,Rhat,Rhi);
        % disp88=[k,fv1,ep1, ep1/mintr]
        Tr_t=Trnew+Tr_d1; Rtr_t=decttor(Tr_t(1,:)); 
        ep1=ep1/2;  k=k+1;
    end
    G=Rh*Rtr_t*Rh'; [Uge,Sge,reff_g]=evd(G,N);
    % disp_EV_g_after_eps_mlu1=[in,kdiff,k,ratio,ep1, diag(Sge).']
    % [Utrt,Strt,reff_t]=evd(Tr_t,Lent); disp_EV_Trt_after_eps_mlu1=[diag(Strt).']
    % disp_mlu1_99=[diff,fv1, ep1, ep1/mintr, kdiff]
% finding the maximum likelihood ratio for the matrices in t-loop -start
    diff=0;     
    % t-loop stops if the last run of esp-loop still offers an increment more than 10%
    if (ratio<tol) 
        sgev=diag(Sge); lr_eps=declr(sgev,N,T); lr_t(kdiff)=lr_eps;
        diff=norm(Tr_t-Trnew)/norm(Trnew); Tr_tt(:,:,kdiff)=Tr_t; 
        if (lr_eps<=lr_tloop)
            Tr_tt(:,:,kdiff)=Trnew; lr_t(kdiff)=lr_tloop;  
        end
        Trnew=Tr_t;
    end
% finding the maximum likelihood ratio for the matrices in t-loop -end
    % disp_mlu_g_88_in_tloop=[in,kdiff,ratio,ep1, diff]
    % disp_mlu_lr_99=[kdiff,k,lr_eps.',lr_tloop,lr0(in)]
    ep1=seps*mintr; kdiff=kdiff+1;
end

for ki=1:kdiff
    if (lr_t(ki)>=1 | lr_t(ki)<=0) lr_t(ki)=0; end
end
[lr_max,k_max]=max(lr_t); Tr_mlu1=Tr_tt(:,:,k_max); lrmlu1(in)=lr_t(k_max);
[U1_ml,S1_ml,reff_ml]=evd(Tr_mlu1,Lent);
% disp_ev_mlu1=diag(S1_ml).'
rtmlu1(in)=reff_ml; niter_mlu1(in)=kdiff;

lru(in)=lrmlu1(in); rtu(in)=rtmlu1(in); Tr_u=Tr_mlu1;
if (lrmlu1(in)<lrmlu(in)) lru(in)=lrmlu(in); rtu(in)=rtmlu(in); Tr_u=Tr_mlu; end
lr_max=max(lrmlu(in),lrmlu1(in));
%if (lr0(in)>lr_max) 
%    Tr_u=Tr_0; lru(in)=lr0(in);
% end

[U1_0,S1_0,reff_0]=evd(Tr_u,Lent); 
if (reff_0<12)
    Tr_u=Tr_u-(S1_0(12,12)-p0)*eye(12);
end
Rtr_t=decttor(Tr_u(1,:)); G=Rh*Rtr_t*Rh'; [Uge,Sge,reff_g]=evd(G,N); 
sgev=diag(Sge); lrul(in)=declr(sgev,N,T);  % loaded unconstrained


% LR calculation of R matrix and Tr matrix
G=Rh*R*Rh'; [Uge,Sge,reff_g]=evd(G,N); sgev=diag(Sge);
lrr(in)=declr(sgev,N,T);
Rtr=decttor(Tr(1,:)); G=Rh*Rtr*Rh'; [Uge,Sge,reff_g]=evd(G,N); sgev=diag(Sge);
[Utt,Stt,reff_tt]=evd(Rtr,N);
lrtr(in)=declr(sgev,N,T);

% ML Toeplitz matrix Tmlmu
evdif=zeros(11,1);  

mintr=min(min(abs(Tr_0))); ep1=seps*mintr; 
Trnew=Tr_u;
for kmu=1:10
    mu=12-kmu-1;
    p01=0;
    for k=mu+1:12
        p01=p01+sqrt(Srss(k,k));
    end
    p01=p01/(12-mu);
    
    diff=1; kdiff=1; 
    Tr_tt=zeros(12,12,Ndiff); lr_t=zeros(Ndiff,1); 
    while (diff>0.1 & kdiff<Ndiff)   % t-loop
        [Utrn,Strn,reff_n]=evd(Trnew,Lent);
        Rtr_t=decttor(Trnew(1,:)); G=Rh*Rtr_t*Rh'; [Uge,Sge,reff_g]=evd(G,N); 
        sgev=diag(Sge); lr_t(kdiff)=declr(sgev,N,T);
        k=1;    ratio=1;
    while (ratio>tol & k<Neps)   % epsilon loop
        [fv1,Tr_d1,ratio]=decmlmu(Utrn,Strn,ep1,mu);
        Tr_t=Trnew+Tr_d1; Rtr_t=decttor(Tr_t(1,:)); 
        ep1=ep1/2; k=k+1;
        G=Rh*Rtr_t*Rh'; [Uge,Sge,reff_g]=evd(G,N); 
        sgev=diag(Sge); lr_tmp=declr(sgev,N,T);
    end
    
    Tr_tt(:,:,kdiff)=Trnew;  % if ratio>tol, select the intial matrix for the t-loop
    if (ratio<tol)
        [Utrn,Strn,reff_n]=evd(Tr_t,Lent); 
        if (reff_n<12)
            Tr_t=Tr_t-(Strn(12,12)-p0)*eye(12);
        end
        Rtr_t=decttor(Tr_t(1,:));     
        G=Rh*Rtr_t*Rh'; [Uge,Sge,reff_g]=evd(G,N); sgev=diag(Sge);
        lr_t(kdiff)=declr(sgev,N,T); Tr_tt(:,:,kdiff)=Tr_t;    
    end
  
    diff=norm(Tr_t-Trnew)/norm(Trnew); kdiff=kdiff+1;
    Trnew=Tr_t; ep1=seps*mintr;

end

 for ki=1:kdiff
     if (lr_t(ki)>=1 || lr_t(ki)<=0) lr_t(ki)=0; end
 end
 [lrmlmu(in,mu),k_max]=max(lr_t);
 Trnew=Tr_tt(:,:,k_max);
        
[Utrn,Strn,reff_n]=evd(Trnew,Lent); % determination of eigenvalue difference
evdif(mu)=Strn(mu+1,mu+1)-Strn(12,12);
 
end
lrmlmu(in,11)=lr0(in);

for mu=1:11
    if (evdif(mu)>p01) lrmlmu(in,mu)=0; end
end

for mu=1:11   % MATLAB does not allow iteration from 11 to 1.
    if (lrmlmu(in,12-mu)>thr_lr*lr0(in)) Mest_lr(in)=12-mu; end
end
ratio_mest(in,:)=lrmlmu(in,:)/lr0(in);

% disp_doa=[doa; doa_est(in,:); doa_est_ls(in,:); doa_est_iqls(in,:); doa_est_daa(in,:); doa_est_aml(in,:)]
disp_doa=[doa; doa_est(in,:); doa_est_daa(in,:); doa_est_aml(in,:); doa_est_sst(in,:)]

% === theoretical threshold method

lsv=zeros(12,1);
for k=1:12
    for ki=k:12
        lsv(k)=lsv(k)+sqrt(Srss(ki,ki));
    end
    lsv(k)=lsv(k)/(12-k+1);
end
sig2est(in)=(lsv(9)-sig2)/sig2;

dsvu=zeros(12,1);
for k=1:11
    coesv=(k+1)*(1+thres/sqrt(T*(k+1)))/(1-thres/sqrt(T*k))-k;
    dsvu(12-k)=coesv*lsv(12-k+1);
end
diff=sqrt(diag(Srss))-dsvu;
ind=1;
for k1=1:11
    if (diff(k1)>=0) ind=k1; end
end
Mest_th(in)=ind;


% === log-likelihod function and MDL criterion

theta_est_dec=zeros(1,11);
theta_est_dec_esprit=zeros(1,11);
theta_est_dec_r=zeros(1,11);
Rs_est=zeros(11,11);
[Urh,Srh,Vrh]=svd(Rhat);
 
% mval(in,1)=in;
for Md=1:11

Un=Urss(:,Md+1:12);
acoe=zeros(23,1);
Run=Un*Un';
for k=1:11
    for m=1:k
        acoe(k)=acoe(k)+Run(12-k+m,m);
    end
    acoe(23-k+1)=conj(acoe(k));
end
for k=1:12
    acoe(12)=acoe(12)+Run(k,k);
end
root_hat=sort(roots(acoe));
root_hat_mag=abs(root_hat); root_hat_phase=angle(root_hat);
theta_est_dec(1:Md)=sort(-root_hat_phase(11-Md+1:11));  % MUSIC angle estimates

V1=Urss(1:11,1:Md); V2=Urss(2:12,1:Md);   % ESPRIT angle estimates
V=inv(V1'*V1)*V1'*V2;
[Uv,D]=eig(V);
theta_est_dec_esprit(1:Md)=sort(angle(diag(D).'));

Qr=zeros(Md+1,Md+1);
for k1=1:12-Md
    Qr=Qr+Urss(k1:k1+Md,1:Md)*Urss(k1:k1+Md,1:Md)';
end
[Uqr,Sqr,Vqr]=svd(Qr);
root_hat_r=sort(roots(Uqr(:,Md+1)));
root_hat_r_mag=abs(root_hat_r); root_hat_r_phase=angle(root_hat_r);
theta_est_dec_r(1:Md)=sort(root_hat_r_phase(1:Md));

sig2_est=0;
    for k=Md+1:12
        sig2_est=sig2_est+sqrt(Srss(k,k));
    end
sig2_est=sig2_est/(12-Md);
Rsssqr=Urss*sqrt(Srss)*Urss';

% estimated phases
%Ass_est=exp(j*angdif_sort_r(12:23)*theta_est_dec(1:Md));  % A11, used to calculate source covariance matrix
%[Uaexp,Saexp,Vexp]=svd(Ass_est'*Ass_est);
%ratio1=Saexp(Md,Md)/Saexp(1,1);

%Aexp_est=exp(j*angdif_sort_r*theta_est_dec(1:Md));  % A1, used to calculate source power vector
%IN_v=zeros(23,1); IN_v(12)=1;
%rho_est=real( inv(Aexp_est'*Aexp_est)*Aexp_est'*(Rhatv_r-sig2_est*IN_v) );
%if (ratio1>0.000000001)   
% If close DOAs are chosen, Rs estimate will be zero and R estimate will 
% very far from the true one, and the estimation error is thus large.
%    AL=inv(Ass_est'*Ass_est)*Ass_est';
%    Rs_p_est(1:Md,1:Md)=AL*(Rsssqr-sig2_est*eye(12))*AL';
%    Rs_est(1:Md,1:Md)=diag(rho_est);
% end

% A_est=exp(j*dist.'*theta_est_dec(1:Md));    % A, used to calculate array covariance matrix
% A_p_est=zeros(N,Md);
%for k1=1:N
%   A_p_est(k1,:)=(root_hat(11-Md+1:11)').^dist(k1); % estimated poles
%end
% R_est=A_est*Rs_est(1:Md,1:Md)*A_est'+sig2_est*eye(N);
% R_p_est=A_est*Rs_p_est(1:Md,1:Md)*A_est'+sig2_est*eye(N);
% R_estv=zeros(Nsq,1); R_p_estv=zeros(Nsq,1);
% for k=1:N
%    R_estv(1+(k-1)*N:N+(k-1)*N)=R_est(:,k); 
%     R_p_estv(1+(k-1)*N:N+(k-1)*N)=R_p_est(:,k);
% end


%66656676
% estimated phases
Ass_est=exp(j*angdif_sort_r(12:23)*theta_est_dec_esprit(1:Md)); % A1, used to calculate source power vector
[Uaexp,Saexp,Vexp]=svd(Ass_est'*Ass_est);
ratio1=Saexp(Md,Md)/Saexp(1,1);

Aexp_est=exp(j*angdif_sort_r*theta_est_dec(1:Md));  % A1, used to calculate source power vector
IN_v=zeros(23,1); IN_v(12)=1;
rho_est=real( inv(Aexp_est'*Aexp_est)*Aexp_est'*(Rhatv_r-sig2_est*IN_v) );
if (ratio1>0.01)   
% If close DOAs are chosen, Rs estimate will be zero and R estimate will 
% very far from the true one, and the estimation error is thus large.
    AL=inv(Ass_est'*Ass_est)*Ass_est';
    Rs_est(1:Md,1:Md)=diag(rho_est);        % diagonal estimated source covariance matrix
    Rs_p_est(1:Md,1:Md)=AL*(Rsssqr-sig2_est*eye(12))*AL'; % non-diagonal estimated source covariance matrix
end

% mval(in,Md+1)=ratio1;
% if (in==114)     mval(1,Md)=ratio1; end
% if (in==333)     mval(2,Md)=ratio1; end
% if (in==713)     mval(3,Md)=ratio1; end
% if (in==880)     mval(4,Md)=ratio1; end

A_est=exp(j*dist.'*theta_est_dec(1:Md));  % A, used to calculate array covariance matrix
R_est=A_est*Rs_est(1:Md,1:Md)*A_est'+sig2_est*eye(N);
R_p_est=A_est*Rs_p_est(1:Md,1:Md)*A_est'+sig2_est*eye(N);
R_estv=zeros(Nsq,1); R_p_estv=zeros(Nsq,1);
for k=1:N
    R_estv(1+(k-1)*N:N+(k-1)*N)=R_est(:,k); 
    R_p_estv(1+(k-1)*N:N+(k-1)*N)=R_p_est(:,k);
end

% ^^^^^^

Cz1=kron(conj(R_est),R_est);
Cz2=kron(Iden,R_est)*B*kron(Iden,conj(R_est));
Cz1_p=kron(conj(R_p_est),R_p_est);
Cz2_p=kron(Iden,R_p_est)*B*kron(Iden,conj(R_p_est));

Qest=zeros(Nsq2,Nsq2);
Qest(1:Nsq,1:Nsq)=real(Cz1+Cz2)/2; 
Qest(1+Nsq:Nsq2,1+Nsq:Nsq2)=real(Cz1-Cz2)/2; 
Qest(1:Nsq,1+Nsq:Nsq2)=imag(Cz1-Cz2)/2; 
Qest(1+Nsq:Nsq2,1:Nsq)=-imag(Cz1+Cz2)/2;
C=P*Qest*P.';
Rv_tilde=[real(Rhatv-R_estv); imag(Rhatv-R_estv)];  % vectorized covariance matrix

Qest_p=zeros(Nsq2,Nsq2);
Qest_p(1:Nsq,1:Nsq)=real(Cz1_p+Cz2_p)/2; 
Qest_p(1+Nsq:Nsq2,1+Nsq:Nsq2)=real(Cz1_p-Cz2_p)/2; 
Qest_p(1:Nsq,1+Nsq:Nsq2)=imag(Cz1_p-Cz2_p)/2; 
Qest_p(1+Nsq:Nsq2,1:Nsq)=-imag(Cz1_p+Cz2_p)/2;
C_p=P*Qest_p*P.';
Rv_p_tilde=[real(Rhatv-R_p_estv); imag(Rhatv-R_p_estv)];  % vectorized covariance matrix

mdl(Md)=1000; mdl_p(Md)=1000; % for Md=11, T=1000 and snr=5dB, C is rank decificient because very close DOAs are chosen.
if (ratio1>0.01) 
    difv=real(P*Rv_tilde);
    difv_p=real(P*Rv_p_tilde);
    neglog_p=real( (T/2)*difv_p'*inv(C_p)*difv_p+(1/2)*log(det(C_p/T)) ) ;
        neglog=real( (T/2)*difv'*inv(C)*difv+(1/2)*log(det(C/T)) ) ;
    neglog_p_nla=T*real( trace(inv(R_p_est)*Rhat)+log(det(R_p_est)) );
        neglog_nla=T*real( trace(inv(R_est)*Rhat)+log(det(R_est)) );
    mdl_p(Md)=neglog_p+((Md+Md^2+1)/2)*log(T);
        mdl(Md)=neglog+((Md+Md+1)/2)*log(T);
    mdl_p_nla(Md)=neglog_p_nla+((Md+Md^2+1)/2)*log(T);
        mdl_nla(Md)=neglog_nla+((Md+Md+1)/2)*log(T);
end


end

[valmdl,Mest_mdl(in)]=max(-mdl);
[valmdl1,Mest_mdl_p(in)]=min(mdl_p);
[valmdl,Mest_mdl_nla(in)]=min(mdl_nla);
[valmdl,Mest_mdl_p_nla(in)]=min(mdl_p_nla);

disp5=[in,Mest_th(in),Mest_mdl(in),Mest_mdl_p(in),Mest_lr(in)]

end % in

for k=1:M
    for i=1:Nsim
        rmse(k)=rmse(k)+(doa_est(i,k)-doa(k))^2;
        rmse_ls(k)=rmse_ls(k)+(doa_est_ls(i,k)-doa(k))^2;
        rmse_iqls(k)=rmse_iqls(k)+(doa_est_iqls(i,k)-doa(k))^2;
        rmse_daa(k)=rmse_daa(k)+(doa_est_daa(i,k)-doa(k))^2;
        rmse_aml(k)=rmse_aml(k)+(doa_est_aml(i,k)-doa(k))^2;
        rmse_sst(k)=rmse_sst(k)+(doa_est_sst(i,k)-doa(k))^2;
    end
    rmse(k)=rmse(k)/Nsim; rmse_ls(k)=rmse_ls(k)/Nsim; rmse_iqls(k)=rmse_iqls(k)/Nsim;
    rmse_daa(k)=rmse_daa(k)/Nsim; rmse_aml(k)=rmse_aml(k)/Nsim; rmse_sst(k)=rmse_sst(k)/Nsim;
end
% disp_mse=[rmse,rmse_ls,rmse_iqls, rmse_daa]
disp_mse=[rmse, rmse_daa, rmse_aml, rmse_sst]

prob_th=0; prob_mdl=0; prob_mdl_p=0; prob_mdl_nla=0; prob_mdl_p_nla=0;
prob_mdl_o=0;  prob_mdl_u=0; prob_mdl_p_o=0;  prob_mdl_p_u=0;
prob_th_o=0; prob_th_u=0; prob_lr=0; prob_lr_o=0;  prob_lr_u=0;
for k=1:Nsim
    if (Mest_th(k)==M) prob_th=prob_th+1; end        % Eigenvalue threshold method
    if (Mest_th(k)>M) prob_th_o=prob_th_o+1; end     % over-estimation probability
    if (Mest_th(k)<M) prob_th_u=prob_th_u+1; end     % under-estimation probability
    if (Mest_mdl_p(k)==M) prob_mdl_p=prob_mdl_p+1; end  % mdl with logT, Md+Md^2+1 (non-identical estimated source covariance matrix)
    if (Mest_mdl_p(k)>M) prob_mdl_p_o=prob_mdl_p_o+1; end     % over-estimation probability
    if (Mest_mdl_p(k)<M) prob_mdl_p_u=prob_mdl_p_u+1; end     % under-estimation probability
    if (Mest_mdl(k)==M) prob_mdl=prob_mdl+1; end        % mdl with logT, Md+Md+1 (identical estimated source covariance matrix)
    if (Mest_mdl(k)>M) prob_mdl_o=prob_mdl_o+1; end     % over-estimation probability
    if (Mest_mdl(k)<M) prob_mdl_u=prob_mdl_u+1; end     % under-estimation probability
    if (Mest_mdl_p_nla(k)==M) prob_mdl_p_nla=prob_mdl_p_nla+1; end  % mdl1 with Md+Md^2+1 based on non-uniform linear array
    if (Mest_mdl_nla(k)==M) prob_mdl_nla=prob_mdl_nla+1; end  % mdl1 with Md+Md+1 based on non-uniform linear array
    if (Mest_lr(k)==M) prob_lr=prob_lr+1; end        % likelihood ratio method
    if (Mest_lr(k)>M) prob_lr_o=prob_lr_o+1; end     % over-estimation probability
    if (Mest_lr(k)<M) prob_lr_u=prob_lr_u+1; end     % over-estimation probability
end

lrmax7=max(ratio_mest(:,7)) ;
lrmin8=min(ratio_mest(:,8)) ;

%  Display  *^%$#### - start
prob_th=prob_th/Nsim                             % EV algorithm
prob_th_o=prob_th_o/Nsim;
prob_th_u=prob_th_u/Nsim;
prob_mdl_p=prob_mdl_p/Nsim                       % MDL algorithm
prob_mdl_p_o=prob_mdl_p_o/Nsim;
prob_mdl_p_u=prob_mdl_p_u/Nsim;
prob_mdl=prob_mdl/Nsim;                           
prob_mdl_o=prob_mdl_o/Nsim;
prob_mdl_u=prob_mdl_u/Nsim;
prob_lr=prob_lr/Nsim                             % likelihood ratio algorithm
prob_lr_o=prob_lr_o/Nsim;
prob_lr_u=prob_lr_u/Nsim;
prob_mdl_p_nla=prob_mdl_p_nla/Nsim               % MDL1 algorithm
prob_mdl_nla=prob_mdl_nla/Nsim;
%  Display  *^%$#### - end


% disp_doadec1=[Mest_mdl, Mest_mdl_p, Mest_mdl_nla, Mest_mdl_p_nla]

% for ii=1:Nsim
%     if (Mest_mdl_p(ii)~=8)
%         disp=[ii, Mest_mdl_p(ii)]
%     end
% end

% for ii=1:Nsim
%     if (Mest_mdl(ii)~=8)
%         disp=[ii, Mest_mdl(ii)]
%     end
% end

% mval