%加上冗余線陣
clear all
clc
N=1024;
d=2;
M=4;
a=[60,90];
f=1/20;
h=[0,1,3,4];

%SOI signal
for k=1:d
      s1=randsn1(N);   
   for n=1:N
      ss(k,n)=s1(n)*cos(2*pi*f*n);          %BPSK
   end
end

SS=zeros(M,N);
for k=1:d
   S{k}=zeros(M,N);
      mov=zeros(M,1);
      for m=1:M
          mov(m,1)=floor(h(m)/(2*f)*cos(a(k)*pi/180));
      end
    
   for i=1:M
     if mov(i,1)>=0 
        for n=1:N-mov(i,1) 
            S{k}(i,n)=ss(k,n+mov(i,1));
        end
     else 
        for n=-mov(i,1)+1:N
             S{k}(i,n)=ss(k,n+mov(i,1));
        end
     end 
   end
SS=SS+S{k};   
end
W=normrnd(0,0.1,M,N);
X=SS;

b=2*f;   %cyclic frequency
LL=20;
R=zeros(9,LL);
R0=zeros(9,1);
for t=1:LL
     for p=1:M
         for q=1:M
             i=h(p)+h(q)+1;
           for n=1:N-t 
              R(i,t)=R(i,t)+X(p,n)*(X(q,(n+t)))*exp(-j*2*pi*b*(n));
           end
        R(i,t)=R(i,t)/N;
        R0(i,1)=R0(i,1)+R(i,t);
         end
     end 
 end
 R0=R0/LL;
 

 mm=3; %mm reprents the size of subarray
L=10-mm;    %L reprents the number of subarray
RCCf=zeros(mm,mm);   
%RCCB=zeros(mm,mm);
for l=1:L
   %RCCf=RCCf+Y([l:l+mm-1],[l:l+mm-1]);
   RCCf=RCCf+R0(l:l+mm-1)*(R0(l:l+mm-1))'
%RCCB=RCCB+conj(R0(10-l:-1:11-l-mm))*(conj(R0(10-l:-1:11-l-mm)))';
end
   RCCf=RCCf/L;
 % RCCB=RCCB/L;
J=rot90(eye(mm,mm));
RCC=(RCCf+J*conj(RCCf)*J)/2;
%RCC=(RCCf+RCCB)/2;
%end

%eigen decomposition
[U,V,uu]=svd(RCC);
abs(V)
Un=U(:,[d+1:mm]);

%譜峰搜索
    dtheta=0.1;   %搜索步長
     n=[1:dtheta:180];
for i=1:length(n)
     aa=zeros(mm,1);
      for m=1:mm
      aa(m,1)=exp(j*(m-1)*pi*cos(n(i)*pi/180));
      end      
    P(i)=-10*log(abs(aa'*Un*Un'*aa));
 % P(i)=1/(abs(aa'*Un*Un'*aa));
end
i=1:length(n);
figure(1)
plot(n(i),P(i)),grid on;
zoom on
%end