%-------------------------------------------------------------------
%                OFDM TOA DOA ESTIMATION
%
%                   KUIRS CHEN 2009.02%
%                           CUMT
%                     KUIRS_CH@GMAIL.COM
%--------------------------------------------------------------------
clear all; clc;
fftsize=128; cp=fftsize/8;         % FFT 長度 循環前綴長度
lamda=3e8/2.4e9;d=lamda*0.5;         % 載波波長 天線單元間距
%============================ ======================================
M =4; data = randint(fftsize,1,M); % 調制階數,隨機整數Message signal
mod_data = qammod(data,M,0);       % QAM調試
% scatterplot(mod_data);           % 散點圖    ;
train=creat_cazac(fftsize,127);    %  訓練數列 CAZAC序列
txd=[train mod_data];              % 插入訓練序列
ofdm=ifft(txd,fftsize);            % OFDM 調制
txx=[ofdm((fftsize-cp+1):fftsize,:);ofdm]; % 插入循環前綴
tx=[txx(:,1); txx(:,2)];                   % 并－－－－》串
%=================================================================
an=8;                                     % 天線數量，決定峰值的尖銳程度
ch2=[0.8441 zeros(1,2) 0.5 0 0 0.20 0 0 0.16661]; % 信道脈沖響應
%=================================================================
theta=d2r([60 20 22 59]);                          % 各徑到達的角度
a_t=exp(j*2*pi/lamda*(1:an)'*d*cos(theta));      % 導向矩陣 an×theta
a_m=[a_t(:,1) zeros(an,2) a_t(:,2)...
    zeros(an,2) a_t(:,3) zeros(an,2) a_t(:,4)];  % 導向矩陣多徑處理

snr=30;
%  SNR=[4:2:16];
%  for s =1:length(SNR)
%      an=SNR(s);
%===============================================================================   
ch_m=[];
for i=1:an
    ch_m=[ch_m;ch2]; % 天線接收脈沖響應矩陣
end
chh=a_m.*ch_m;       % 信道脈沖響應矩陣乘上導向矩陣
%================================================================================
% hegecishu=0;doaerr=0;simnum=100; % 實驗次數
% for s=1:simnum




for sample=1:100  % 快拍數
    for i=1:an
        rdata(i,:) = filter(chh(i,:),1,tx);          % 信號通過各天線
        rdata(i,:) = awgn(rdata(i,:),snr,'measured');  % 加入AWGN噪聲
        TE = rdata(i,17:144);     % 去除循環前綴，取出訓練序列的接收量 
        p(i,:)=fft(TE,128);       % 結合信號做FFT變換，到頻域  
    end
    sa_m(:,:,sample)=p;
end
sam=mean(sa_m,3);       % 對OFDM個載波數據進行平均
%==================== MUSIC  ALGORITHM ==============================================


RCC=sam*sam'/length(sam);                   % 接收信道矩陣相關 修改后為（AN×AN）矩陣
flag=2;                         %  1：正常的方法 2：改進的方法
JJ=fliplr(eye(length(RCC)));    % 修正矩陣
MR=(RCC+JJ*RCC.'*JJ)/2;         % 修正后的相關矩陣

if flag==1
    [U,V,uu]=svd(RCC);
else
    [U,V,uu]=svd(MR);
end

Un=U(:,[5:end]);
dtheta=0.005; % doa 搜索步長
ww=[0 100];     % MUSIC搜索范圍，減少仿真計算量
n=[ww(1):dtheta:ww(2)-dtheta];
for i=1:length(n)
    aa=zeros(an,1);
    for m=1:an
        aa(m,1)=exp(j*(m-1)*pi*cos(n(i)*pi/180));
    end
    P(i)=-10*log(abs(aa'*Un*Un'*aa));
end
P=P./sqrt(sum(P.^2));


%============ 搜索DOA峰值  ===============================================
fanwei=[1: length(P)];
[peak_h,index_h]=findpeak(P(fanwei)); % 搜索范圍 縮小范圍減少計算量
t_h=(index_h-1)*dtheta;
Lp=4;   %多徑的數量，即有多少個不同的入射角度
[DOAa,Atten]=selectpath(peak_h,t_h,Lp);
% ==========================================================================
disp(['天線數量：' num2str(an) ' 信噪比：' num2str(snr)]);
disp(['估計的DOA：' num2str(DOAa)]);

% s
% temp=min(DOAa)-10;
% if abs(temp)<10
%     doaerr=(temp-20)^2+doaerr;
%     hegecishu=hegecishu+1
% end
% end
% 
% rmse=sqrt(doaerr/hegecishu);
% disp(['DOA的RMSE誤差：' num2str(rmse)]);


% 
i=1:length(n);
plot(n(i),P(i));hold on;
grid on; title(['MUSIC DOA (SNR= ' num2str(snr) ' dB )']);
xlabel('來波方向角');ylabel('P(dB)')



% DOA_M(s,:)=DOA;
%  end
% DOA_M=sort(DOA_M,2)
% % plot(SNR,sqrt((x.^2)))