%proposed

close all;
clear all;
clc;

J=sqrt(-1);
source_number=2;
source_elevation=[20 60]*pi/180;
source_azimuth=[40 30]*pi/180;
sensor_number=16;
snapshort_number=200;
snr=10;


k=0:sensor_number-1;
 for i=1:source_number
   A(:,i)=exp(J*pi*k*cos(source_elevation(i))*sin(source_azimuth(i)));
 end
 A1=A(1:source_number,:);
 A2=A(source_number+1:sensor_number,:);

M=[exp(J*pi*cos(source_elevation(1))*sin(source_azimuth(1))),exp(J*pi*cos(source_elevation(2))*sin(source_azimuth(2)))];
M=diag(M);
N=[exp(J*pi*sin(source_elevation(1))*sin(source_azimuth(1))),exp(J*pi*sin(source_elevation(2))*sin(source_azimuth(2)))];
N=diag(N);

B2=[A2.', (A1*M).', (A2*M).', (A1*N).' ,(A2*N).'].';

 
Fs=10e6;%采樣周期
Ts=1/Fs;

y1=10*Ts;
y2=20*Ts;
y3=30*Ts;%延時
  
t=linspace(Ts,snapshort_number*Ts,snapshort_number);
for n=1:snapshort_number
    
   S1(1,n)=s(t(1,n)-y1) ;
   S2(1,n)=s(t(1,n)-y2) ;
end
 
s=[S1;S2];%%s=[S1.',S2.']'
%s=(randn(source_number,snapshort_number)+J*randn(source_number,snapshort_number))/sqrt(2);
x=A*s;
x=awgn(x,snr);
y=A*M*s;
y=awgn(y,snr);
z=A*N*s;
z=awgn(y,snr);

Q=[x.' y.' z.'].'; 
R=Q*Q'/snapshort_number;
R1=R(:,1:source_number);
R2=R(:,source_number+1:3*sensor_number);
%Q1=Q(1:source_number,:);
%Q2=Q(source_number+1:3*sensor_number,:);

P=inv(R1'*R1)*R1'*R2;
%P=inv(Q1*Q1')*Q1*Q2';
P=P';
[m,n]=size(A2);
P1=P(1:m,:);
[a,b]=size(A1*M);
P2=P(m+1:m+a,:);
[c,d]=size(A2*M);
P3=P(m+a+1:m+a+c,:);
[e,f]=size(A1*N);
P4=P(m+a+c+1:m+a+c+e,:);
[g,h]=size(A2*N);
P5=P(m+a+c+e+1:m+a+c+e+g,:);
 
Ty=P3*pinv(P1);
[Uy,Sy]=eig(Ty);
 
Tz=P5*pinv(P1);
[Uz,Sz]=eig(Tz);

for i=1:source_number
    o(i)=angle(Sz(i,i))/angle(Sy(i,i));
    %o(i)=log(Sz(i,i))/log(Sy(i,i));
    elevation(i)=atan(o(i))*180/pi;
   k(i)= sqrt((angle(Sy(i,i)))^2+(angle(Sz(i,i)))^2)/pi;
 % k(i)=sqrt((log(Sy(i,i)))^2+(log(Sz(i,i)))^2)/pi;
 azimuth(i)=asin(k(i))*180/pi;
end
disp('elevation=');
disp(elevation);
disp('azimuth=');
disp(azimuth);