clear all;

M = 6;
N = 3;
K = 57;
snap = 50;

path = 3;
theta = [40 90 120];
phi = [60 90 135];
toff = [12 26 36];
dcps = 0;
snr = 8;
[H_a,H_ad,H_at,H_adt,H_adtp]=data_sim(M,N,K,path,theta,phi,toff,dcps,snr);

S=randn(path,snap); 
A = reshape(H_ad,M*N,1);
X = repmat(A,1,path);
X1 = X * S;

Rxx=X1*X1'/snap;
% 特征值分?
[EV,D]=eig(Rxx);                   %特征值分?
EVA=diag(D)';                      %将特征?矩阵对角线提取并转为一?
[EVA,I]=sort(EVA);                 %将特征?排? 从小到大
EV=fliplr(EV(:,I));                % 对应特征矢量排序
En=EV(:,path+1:M*N);                   % 取矩阵的第M+1到N列组成噪声子空间

%%
derad = pi/180;      %角度->弧度
cc = 3e8;
ff = 5.8e9;

ex1 = 0:M-1;
ex2 = 0:N-1; 
ex3 = 0:K-1;

d = 0.027;
d1 = d*ex1;
d2 = d*ex2;

for aoa = 0:180
    for aod = 0:180
        a1 = exp(-1i*2*pi*d1*cos(derad*aoa)* ff /cc);% 1 * M
        a2 = exp(-1i*2*pi*d2*cos(derad*aod)* ff /cc);
        a = reshape(a1'*a2,1,M*N)';
        Pmusic(aoa+1,aod+1)=1/(a'*En*En'*a);
        %                 Pmusic(tof+1)=1/(repmat(a2,1,M)*En*En'*repmat(a2,1,M)');
    end
end

Pmusic=abs(Pmusic);
Pmmax=max(max(Pmusic))
% Pmusic = Pmusic/Pmmax;
% Pmusic=10*log10(Pmusic/Pmmax);            % 归一化处?

plot2D(0:180,'AoA/',0:180,'AoD/',Pmusic);