%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                          mimo_main.m
% This program is designed to estimate MIMO parameters
% in the Frequency Domain.
% 
% Joint Diagonalization was used to get a good estimation.
%
% Stationary Second order and third/fourth order spetrals are used.
%
% Reference: 
%  [1] Binning Chen and Athina P. Petropulu, "Frequency Domain Blind 
%      MIMO System Identification Based On Second- And Higher-Order 
%      Statistics," IEEE Transactions on Signal Processing, 
%      vol. 49(8), pp. 1677-1688, August 2001.
%
%  Communications and Signal Processing Laboratory
%  ECE Department, Drexel University
%  Philadelphia, PA 19104, USA
%  http://www.ece.drexel.edu/CSPL
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

MATLAB_VERSION_NUMBER=version;   % Because the implementation of function "xcorr" are different
                                 % in MATLAB 5 and MATLAB 6, this program is designed in MATLAB5,
                                 % a small modification is needed to run well in MATLAB6.
                                 
Program_START = clock;           % Set a timer for the program

Over_Estimate_R_and_C=1          % Suppose we don't know the true channel order, we need to over estimate
                                 % the correlations and cumulants

Modify_Hest_Mag_by_Order_Constrain=0

Process_hest_CUM_his=1           % For ststitical purpose, we need compare the estimated channel impulse
                                 % with the true one.

Equalizer_Length=15;

MAX_CORRELATION_LENGTH=100;

L = 5                            % Channel length, including h(0).

Le=L+4                           % Extended channel length.

if Le > L+1
   MAX_Minus_ORDER_ifft=floor((Le-L)/3);
else
   MAX_Minus_ORDER_ifft=0;
end

MAX_Minus_ORDER=1;
MAX_Plus_ORDER=Le-MAX_Minus_ORDER-1;

sum_w1_w2=0

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Settings for the MIMO system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
m = 2                           %%% Number of Output signals
n = 2                           %%% Number of Input signals


SYSTEM_REAL=1                   %%% The system is real if set to 1, then the H(w) are conjugate symmetric.

L_extend=L
%L_extend=Le-L
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Settings for the Monte-Carlo simulations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=1024*4                        %%% Length of Input(Output) signal 
seg_length = 128*4;            %%% Segment length used in estimating the cross cumulants
seg_num=N/seg_length;           %%% Number of segments

NF = 128                        %%% Length of FFT used in the estimation, it also determine the
                                %%% Frequency resolution.

MAX_CORRELATION_LENGTH_ifft=NF;      %%% For calculate hest_ifft directly from ifft.

RUN_TIMES=3                     %%% Number of Monte-Carlo runs

ADD_NOISE=1                     %%% Observation noise is added if set to 1


SNR=10                          %%% Signal to Noise Ratio of the observation signal

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Settings for the Second order Statistics
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if Over_Estimate_R_and_C
   R_LENGTH=2*(Le-1)               %%% how much correlation need estimate, maximum argument of correlation
else
   R_LENGTH=L-1                    %%% how much correlation need estimate, maximum argument of correlation
end

ADD_WINDOW=1                    %%% Add window when estimating cross power spectrum.
ADD_WINDOW=ADD_WINDOW & Over_Estimate_R_and_C

S_x_symmetry=1                  %%% Forbid the estimated Cross power spectrum to be symmetry, 
                                %%% this is always true.
V_diagonal_real=1               %%% Forbid the diagonal entries of V(w) to be real
Rx_diagonal_real=1              %%% Forbid the diagonal entries of Rx(w) to be real

Select_Freq_by_Rx_cond=1        %%% reconstruct system using only frequencies corresponding to
                                %%% small conditiona number of cross power spectrum P_x(w)
                                %%% usually, this is not needed if interpolate_V=1
                                
Rx_cond_selection_percentage=80 %%% How many frequencies to select based on the condition number
                                %%% of cross power spectrum P_x(w), in percentage.

interpolate_V=1                 %%% V(w) is the whitening matrix, which is the inverse square
                                %%% root of cross correlation matrix P_x(w). Since the estimation
                                %%% of V(w) isaffected by the condition number of P_x(w), so we
                                %%% can interpolate the V(w) in the frequencies corresponding to
                                %%% high condition number of P_x(w).
                                %%% When the number of output is bigger than the number of inputs,
                                %%% that is m>n, then the interpolation is not needed.
                                %%% Also note, the estimation of V^{-1}(w) need not interpolation.

CHOOSE_EIG=1;                   %%% Estimate the system using only part freqencies

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Settings for the Higher order Statistics
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if Over_Estimate_R_and_C
   C_LENGTH=2*(Le-1)               %%% how much cumulants need estimate, maximum argument of cumulants
else
   C_LENGTH=L-1                    %%% how much cumulants need estimate, maximum argument of cumulants
end

ADD_CUM_WINDOW=1                %%% Add window when estimating cross polyspectra.

win_width=C_LENGTH;             %%% Window length for estimating cross cumulants.

Using_Bispectrum=1;             %%% Using third order cumulants if set to 1. 
                                %%% If set to 0, then use fouth order cumulants in the estimation.

CUMULANTS_ARE_REAL = 0;         %%% If set to 1, then the estimated cumulants are modified to real.

Calculate_B_x_Using_FFT2=1;     %%% This method is faster than direct method

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Parameters for constructing the polyspectra matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Select_Rx1_index=1;             %%% Select Rx1_index, the k in the fomula (8) of Binning Chen's.
                                %%% ICASSP 2000 paper, this is for the third order cumulant case.

w2=1                            %%% Frequency w2 in the formula (8) of Binning Chen's.
                                %%% ICASSP 2000 paper, this is for bith third and fourth 
                                %%% order cumulant case.  1<= w2 <=NF.
                                
w3_equal_0=1                    %%% For the fourth order cumulant case only
                                %%% If set to 1, then w3=0
w2_equal_minus_w3_plus_arfa=0   %%% For the fourth order cumulant case only
                                %%% If set to 1, then w2+w3=arfa

Select_Freq_by_SVD=0;           %%% Select good frequencies based on the SVD of cross polyspectra matrix
Smat_std_plus_coeff=2           %%% Keep only the singular value not so strange, <mean+std * this coeff.
Smat_std_minus_coeff=2          %%% Keep only the singular value not so strange, >mean-std * this coeff.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Settings for Joint Diagonalization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Using_Joint_Diag=1;             %%% Using Joint Decomposition in estimating the orthogonal
                                %%% matrix W(w), there are two kinds of joint decompositions
                                %%% One is Cardoso's joint diagonalization, the other is
                                %%% Pesquet's Joint SVD

Freq_Select_Ratio=0.16;
Ref_Frequencies=0:1:(NF/2-1);       %%% A series of possible w2, each w2 can give an separate estimation,
                                %%% We can use joint diagonalization (SVD) to get an better estimation.
Freq_number=length(Ref_Frequencies)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Settings for Phase estimation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k_arfa=1                        %%% The phase recursion parameter, which is the k_arfa in 
                                %%% formula (21) of Binning Chen's CISS 2000 paper.

i_phase_index_1=1               %%% Reference singal index for phase retrieval
i_phase_index_2=2               %%% Reference singal index for phase retrieval

Using_Pesquet_phase=0           %%% Pesquet suggested using FFT to solve the phase recursion equation
                                %%% If set to 1, then the Phi is estimated using FFT, not recursion.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Settings for Reconstruct inputs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
RECONSTRUCT_INPUT=1;            %%% Reconstruct input using Wiener filter if set to 1.

Minimum_Phase_System=0;         %%% The underlying MIMO system is minimum phase,
                                %%% this will affect the reconstruction of the inputs.


Modify_Hest_by_Phase_est=1      %%% The Hest has an phase error, here we use the estimated phase
                                %%% Phase_est to replace the phase of Hest. 

Limit_Delay_time=1



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Some parameters for testing this algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Roots_Amplitude=1.50;           %%% The amplitude of the roots of the impulse response
                                %%% if greater than 1, then it is non-minimum phase

Diagonal_amplitude=1;           %%% This parameter will change the amplitude of the diagonal
                                %%% elements of the MIMO system impuls response matrix.


PLOT_PHASE=0;                   %%% Plot the phase figure or not


if Using_Bispectrum
   GAMMA=2;                     %%% The third order cumulant of the exponentially distributed real white noise
else  % Using Trispectrum
   %GAMMA=-1.2;                 %%% The Fourth order cumulant of the uniform distributed real white noise
   %GAMMA=-1.2;                 %%% The Fourth order cumulant of the uniform distributed complex white noise
   %GAMMA=-2;                   %%% The Fourth order cumulant of the uniform distributed BPSK (2 PAM)signal
   GAMMA=-1;                    %%% The Fourth order cumulant of the uniform distributed 4-QAM signal
   %GAMMA=6;                    %%% The Fourth order cumulant of the exponential distributed white noise.
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Parameters setting ENDS here
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


if n < m
   Less_Input_than_Output=1
else
   Less_Input_than_Output=0
end


hest_CUM_his=zeros(MAX_Minus_ORDER+MAX_Plus_ORDER+1,m,n,RUN_TIMES);


format compact;
i=sqrt(-1);


CIRCLE=exp(i*(0:1000)*2*pi/1000);

r_window = kaiser(2*R_LENGTH+1,7);      %%% Kaiser window function for estimating cross-correlation.
%r_window=hamming(2*R_LENGTH+1); %%% Hamming window function for estimating cross-correlation.


%d_win=zeros(1,1+win_width);     %%% Single side Window function prototype for estimating cross-cumulants.
%% Optimal Window, also called Sasaki Window
%d_win=abs(sin((0:win_width)*pi/win_width))/pi + (1.0-(0:win_width)/win_width).*cos((0:win_width)*pi/win_width);
d_win = kaiser(2*win_width+1,7);      %%% Kaiser window function for estimating cross-correlation.
d_win = d_win(win_width+1:2*win_width+1);      %%% Kaiser window function for estimating cross-correlation.
%% Parzen Window
%d_win(1:1+floor(win_width/2))=1-6*((0:floor(win_width/2))/win_width).^2+6*((0:floor(win_width/2))/win_width).^3;
%d_win(2+floor(win_width/2):win_width+1)=2*(1-(1+floor(win_width/2):win_width)/win_width).^3;

dd_win=zeros(1,2*win_width+1);  %%% Double side Window function prototype for estimating cross-cumulants.
dd_win(win_width+1:2*win_width+1)=d_win;
dd_win(1:win_width)=d_win(win_width+1:-1:2);

cum_win=zeros(win_width*2+1);   %%% Two dimensioanl Window function for estimating cross-cumulants.
for ii=-win_width:win_width
   for jj=-win_width:win_width
      if abs(ii-jj) <= win_width 
         cum_win(ii+win_width+1,jj+win_width+1)=d_win(abs(ii)+1)*d_win(abs(jj)+1)*d_win(abs(ii-jj)+1);
      end
   end
end


eig_index_his=zeros(NF,RUN_TIMES);

h_amplitude=ones(m)+eye(m)*(Diagonal_amplitude-1);

h=zeros(L,m,m);

if 0               %% Design the system impulse response matrix h(t,i,j)
   for ii=1:m
      for jj=1:m
         Zeros = roots(2*rand(1,L)-1);
         Zeros = Roots_Amplitude*tanh(abs(Zeros)) .* exp(j*angle(Zeros));
         h(:,ii,jj)=real((poly(Zeros)));
      end
   end
end

if 1   %%% Non-Minimum Phase  m=2; L=5  %%% Example for IEEE Transaction paper
h(:,1,1) = [ 1.0000   -1.5537   -0.0363    0.5847    0.5093];
h(:,1,2) = [ 1.0000    2.2149    1.0828   -1.1731   -0.8069];
h(:,2,1) = [ 1.0000    0.9295    0.2453   -0.7510    0.3717];
h(:,2,2) = [ 1.0000   -0.7137   -1.5079    1.6471   -1.2443];
end

if 0  %%% Non-Minimum Phase  m=3; L=5  %%% mino
h(:,1,1) = [ 1.0000   -1.5537   -0.0363    0.5847    0.5093];
h(:,1,2) = [ 1.0000    2.2149    1.0828   -1.1731   -0.8069];
h(:,1,3) = [ 1.0000   -1.7325    0.4123    0.3471   -0.2796];
h(:,2,1) = [ 1.0000    0.9295    0.2453   -0.7510    0.3717];
h(:,2,2) = [ 1.0000   -0.7137   -1.5079    1.6471   -1.2443];
h(:,2,3) = [ 1.0000    2.1911    1.7313   -0.1818   -0.2214];
h(:,3,1) = [ 1.0000   -1.0191   -1.5532    1.5117   -0.7217];
h(:,3,2) = [ 1.0000    2.0637    0.8907   -0.3785   -0.3789];
h(:,3,3) = [ 1.0000   -0.6879   -0.8976   -0.6126   -0.1318];
end

%%% Modify the imuplse response matrix of the MIMO system.
for ii=1:m
   for jj=1:m
      h(:,ii,jj)=h(:,ii,jj)*h_amplitude(ii,jj);
   end
end

%%% Frequency domain MIMO system response, System transfer function matrix
H = fft(h,NF,1);    


if Select_Rx1_index
   if Less_Input_than_Output
      H_0=reshape(H(sum_w1_w2+1,:,1:n),m,n);
      for row=1:m
         H_0_dummy=sort(abs(H_0(row,:)));
         min_ratio(row)=min(H_0_dummy(n:-1:2)./H_0_dummy(n-1:-1:1));
      end
      [max_min_ratio Rx1_index]=max(min_ratio);
   else
      H_0=reshape(H(sum_w1_w2+1,:,:),m,m);
      for row=1:m
         H_0_dummy=sort(abs(H_0(row,:)));
         min_ratio(row)=min(H_0_dummy(m:-1:2)./H_0_dummy(m-1:-1:1));
      end
      [max_min_ratio Rx1_index]=max(min_ratio);
   end
else
   Rx1_index=1;
end


H_order=zeros(1,n);             %%% Since there is a column permutation in the estimated H(w),
                                %%% H_order tells the column permutation, it is determined 
                                %%% by the true channel response before the simulation.
                                
HH=shiftdim(H,1);               %%% The MIMO system transfer function, m x n x NF.
Phase_true=angle(HH);           %%% The true phase of the MIMO system



Ryn = zeros(m,m,Freq_number,NF);%%% The whitened cross polyspectra matrices.
Cyn = zeros(m,m,Freq_number,NF);%%% Ryn'*Ryn, used for Cardoso's joint diagonalization.
Wn  = zeros(m,m,NF);            %%% The estimated orthogonal matrix W(w) based on Joint Diag.


if w2_equal_minus_w3_plus_arfa & ~Using_Bispectrum
   arfa_matrix=ones(2*C_LENGTH+1,2*C_LENGTH+1,2*C_LENGTH+1);
   for ii=-C_LENGTH:C_LENGTH
      arfa_matrix(:,:,ii+C_LENGTH+1)=arfa_matrix(:,:,ii+C_LENGTH+1)*exp(-j*arfa_w2_plus_w3*ii/NF);
   end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  The Monte-Carlo test begin
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for iloop=1:RUN_TIMES
   
   N
   iloop
   rand('state',sum(100*clock));
   
   %%% Generate the input signal.
   if Using_Bispectrum
      %s = -log(rand(m,N))-log(rand(m,N))*i;
      s = -log(rand(m,N));      %%% Single side Exponential distributed real signal
   else  % Using Trispectrum
      s = -log(rand(m,N));     %%% Single side Exponential distributed
      %s = rand(m,N)+i*rand(m,N);