tic
clear
format short;
%c=3*10.^8;
m=4;
L=2*m+1;
signum=1;
N=128*2;
fm1=0.25;%fc1=0.275;
fm2=0.32;%fc2=0.349;
Km1=0.025;
Km2=0.033;
f0=0.32;
%lamta=c/f0;
%len=lamta/2;
dt=0.5;
%len=1/2;
theta1=30*pi/180;
theta2=60*pi/180;
fai1=20*pi/180;
fai2=50*pi/180;
snr=20;
Amp=sqrt(2*10^(snr/10));
%----s---
t=1:N;
sig1=Amp.*exp(j*2*pi*(Km1/2*(t.^2)/N+fm1*t));
f1=1*Km1*t/N+fm1;
sig2=Amp.*exp(j*2*pi*(Km2/2*(t.^2)/N+fm2*t));
f2=1*Km2*t/N+fm2;
s=[sig1;sig2];
%----a---
  for i=1:N; 
      p1=exp(j*2*pi*dt*sin(theta1)*cos(fai1)*f1(1,i));
      q1=exp(j*2*pi*dt*sin(theta1)*sin(fai1)*f1(1,i));
      p2=exp(j*2*pi*dt*sin(theta2)*cos(fai2)*f2(1,i));
      q2=exp(j*2*pi*dt*sin(theta2)*sin(fai2)*f2(1,i));
      l=1:m; 
      ap1=p1.^l;
      aq1=q1.^l;
      a1=[1;ap1.';aq1.'];
      ap2=p2.^l;
      aq2=q2.^l;
      a2=[1;ap2.';aq2.'];
      a=[a1,a2];
      S(:,i)=a*s(:,i);
  end   
%----noise---
noise=randn(L,N)+j*randn(L,N);
  
z=S+noise;

%z1=z(:,1:N-1);
%z2=z(:,2:N);
 [tfrf,t,f]=tfrsp(z(1,:).'); 
 rou=round(0.3/0.5*N/2);
 [Mm1 Nn1]=max(tfrf(1:rou,:));
 ff01=mean(Nn1(N/2-5:N/2-5))./N;
 %lamta01=c/ff01;
 [Mm2 Nn2]=max(tfrf((rou+1):N,:));
 ff02=(mean(Nn2(N/2-5:N/2-5))+rou)./N;
 %lamta02=c/ff02;
 %contour(t,f,tfrf)
 rou1=round(0.3/0.5*N);
 for i=1:1*L
    zz=z(i,:).';
[tfr,t,f]=tfrspwv([z(1,:).',zz]);
%figure(i);
%contour(t,f,tfr);
y1=max(tfr(1:rou1,(N/2-5:N/2-5)));
%y1=max(tfr(1:rou1,:));
%m1=max(y1);
yy1(i,:)=y1;
y2=max(tfr(rou1+1:N,(N/2-5:N/2-5)));
%y2=max(tfr(rou1+1:N,:));
%m2=max(y2);
yy2(i,:)=y2;
end;
%yy=(yy(1,:)+yy(1,:))/2;

Rzz1=(yy1*yy1')/(N-1);
[uRzz,lamdazz]=eig(Rzz1);
if lamdazz(1*L,1*L)>lamdazz(1,1)
uRzz=fliplr(uRzz);
%lamdazz=J*lamdazz*J;
end
Eszz=uRzz(:,1:signum);

%********************* p ***********************
Ep1=Eszz(1:m,:);
Ep2=Eszz(2:m+1,:);

psaip=(Ep1'*Ep2)/(Ep1'*Ep1);
[up,faip]=eig(psaip);

%********************* q ***********************
Eq1(1,:)=Eszz(1,:);
Eq1(2:m,:)=Eszz(m+2:L-1,:);
Eq2=Eszz(m+2:L,:);

psaiq=(Eq1'*Eq2)/(Eq1'*Eq1);
[uq,faiq]=eig(psaiq);

%faip=faip(1,1);
%faiq=faiq(1,1);
%----caculate the DOA and p----
theta11=asin((sqrt((angle(faip)*angle(faip))+(angle(faiq)*angle(faiq))))/ff01/pi/2/dt)*180/pi
fai11=atan(angle(faiq)/angle(faip))*180/pi




Rzz2=(yy2*yy2')/(N-1);
[uRzz,lamdazz]=eig(Rzz2);
if lamdazz(1*L,1*L)>lamdazz(1,1)
uRzz=fliplr(uRzz);
%lamdazz=J*lamdazz*J;
end
Eszz=uRzz(:,1:signum);

%********************* p ***********************
Ep1=Eszz(1:m,:);
Ep2=Eszz(2:m+1,:);

psaip=(Ep1'*Ep2)/(Ep1'*Ep1);
[up,faip]=eig(psaip);

%********************* q ***********************
Eq1(1,:)=Eszz(1,:);
Eq1(2:m,:)=Eszz(m+2:L-1,:);
Eq2=Eszz(m+2:L,:);

psaiq=(Eq1'*Eq2)/(Eq1'*Eq1);
[uq,faiq]=eig(psaiq);

%faip=faip(1,1);
%faiq=faiq(1,1);
%----caculate the DOA and p----
theta11=asin((sqrt((angle(faip)*angle(faip))+(angle(faiq)*angle(faiq))))/ff02/pi/2/dt)*180/pi
fai11=atan(angle(faiq)/angle(faip))*180/pi

toc