%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                                     In3_Out2.m                                     %%%
%%%                                                                                    %%%
%%%  This program is the implementation of the tensor decomposition based blind MIMO   %%%
%%%  system identification algorithm with 3-input 2-ouput.                             %%%
%%%                                                                                    %%%
%%%  This code is for the following CISS'2001 paper.                                   %%%
%%%  Binning Chen, Athina P. Petropulu and Lieven De Lathauwer, ``Blind Identification %%%
%%%  of Convolutive MIMO System with 3 Sources and 2 Sensors ," {\it 35th Annual       %%%
%%%  Conf. on Information Sciences and Systems, CISS'2001}, The Johns Hopkins          %%%
%%%  University, Baltimore, Maryland, USA. March 2001.                                 %%%
%%%                                                                                    %%%
%%%  Designed by Binning Chen on January 28, 2001.                                     %%%
%%%                                                                                    %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Program_Start=clock;

MODIFY_Hest_Magnitude_by_Smoothing=0

MODIFY_Hest_at_DC_Freq=1

UsingCrossCumulantStructure=1

Using_DeLathauwer_ALS_method=0

ALS_Method_index=1

USING_cum_true=1

Using_Pw_true=1

global VECTEURSNOYAU
global POLYNOMECIRCULAIRE

USING_LievenDeLathauwer_Equation=1

DEBUG=0

LIMIT_HESTP_MAGNITUDE=1

WSIZE=20
STDEV=1.07
Hest_MAX=5

Using_C_31=1   % Else using C^{40}

m=2    %%% Number of Outputs
n=3    %%% Number of Inputs

N=1024*4
seg_length = 1024*1;            %%% Segment length used in estimating the cross cumulants
seg_num=N/seg_length;           %%% Number of segments

CL=5
R_LENGTH=CL-1;
C_LENGTH=CL-1;

Roots_Amplitude=0.9

Pw_symmetry=1

ADD_WINDOW=0
ADD_POLYSPECTRA_WINDOW=0

NF=128;

RUN_TIMES=1

%%% For the constant permutation purpose
l_index=1,
Ref_Freq=1

%%% For the phase ambiguity recovery
k_arfa=1  %% correspond to C_{l_1 l_2}(-w, w+k_arfa, w3)
l_1=1;
l_2=1;
w3=1;
A1=hosmatrix(NF, k_arfa);
A1=inv(A1);

%%% For time domain impulse response estimation using ifft
Limit_Delay_time=1


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


if 1 %%% Very good example for 2x3 case, March 13, 2001, CISS'2001
h(:,1,1) = [ 1.0000   -1.0191   -1.5532    1.5117   -0.7217];
h(:,1,2) = [ 1.0000    2.2149    1.0828   -1.1731   -0.8069];
h(:,1,3) = [ 1.0000   -1.5537   -0.0363    0.5847    0.5093];
h(:,2,1) = [ 1.0000   -0.6879   -0.8976   -0.6126   -0.1318];
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];
end


HH = shiftdim(fft(h,NF,1),1); %%% The MIMO system transfer function, m x n x NF.
Phase_true=angle(HH);           %%% The true phase of the MIMO system

%%% For the constant permutation purpose
[H_dummy H_order_i]=sort(reshape(abs(HH(l_index,:,Ref_Freq)),n,1));
H_order(H_order_i)=1:n;

%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.
%GAMMA=3;                    %%% The Fourth order cumulant of the double-side exponential distributed white noise.


H_roots=zeros(NF, 3);
L_roots=zeros(NF, 3);
H2_roots=zeros(NF, 3);
L2_roots=zeros(NF, 3);

Q1_roots=zeros(NF, 3);
Q2_roots=zeros(NF, 3);
Q3_roots=zeros(NF, 3);
Q4_roots=zeros(NF, 3);

L_roots_his=zeros(NF, 3, RUN_TIMES);
L2_roots_his=zeros(NF, 3, RUN_TIMES);

Q1_roots_his=zeros(NF, 3, RUN_TIMES);
Q2_roots_his=zeros(NF, 3, RUN_TIMES);
Q3_roots_his=zeros(NF, 3, RUN_TIMES);
Q4_roots_his=zeros(NF, 3, RUN_TIMES);


for run=1:RUN_TIMES
   
   rand('state',sum(137*clock));
   
   %s = rand(n,N);             %%% uniformly distributed real signal
   s = -log(rand(n,N));      %%% Single side Exponential distributed real signal
   
   %s = rand(n,N);
   %s = s-(mean(s'))'*ones(1,N);
   %s = -log(rand(n,N)).*sign(s); %%% Double-side Exponential distributed real signal
   
   s = s-(mean(s'))'*ones(1,N);
   s=diag(1./std(s,0,2))*s;  %%% Input Signal
   x = zeros(m,N);       %%% Observed mixture , received signal
   for ii=1:m
      for jj=1:n
         x(ii,:) = x(ii,:) + filter(h(:,ii,jj),1,s(jj,:));
      end               
   end
   x = x-(mean(x'))'*ones(1,N);
   
   
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%%    Cumulant and spectra estimation begin here
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   for ii=1:m
      for jj=1:m
         for kk=1:m
            for ll=1:m
               ii,jj,kk,ll;
               
               cum_true=zeros(2*C_LENGTH+1, 2*C_LENGTH+1, 2*C_LENGTH+1);
               for mm=1:n
                  cum_true = cum_true + true_cum4_3D(h(:,ii,mm),h(:,jj,mm),h(:,kk,mm),h(:,ll,mm),C_LENGTH);
               end
               
               if ~USING_cum_true
                  cum = g_cc4_3D(x(ii,:),x(jj,:),x(kk,:),x(ll,:),C_LENGTH);
                  
                  if UsingCrossCumulantStructure
                     NonZeroCumIndex=zeros(size(cum_true));
                     NonZeroCumIndex(find(cum_true))=1;
                     cum = cum .* NonZeroCumIndex;
                  end
                  
                  cum_all(:,:,:,ii,jj,kk,ll)=cum;
               end
               
               cum_true_all(:,:,:,ii,jj,kk,ll)=cum_true;
               
               if USING_cum_true
                  cw = g_TriSpec_www_slice(cum_true,C_LENGTH,NF,ADD_POLYSPECTRA_WINDOW);
               else
                  cw = g_TriSpec_www_slice(cum,C_LENGTH,NF,ADD_POLYSPECTRA_WINDOW);
               end
               
               %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
               %%%
               %%%                        ATTENTION HERE !!!
               %%%
               %%%    Since the MIMO identification algorithm requires that the tensor
               %%%    Tw_31=H o H o H o H3
               %%%    By the cumulant estimation, we can get a tensor
               %%%          H3 o H o H o H
               %%%    To make these two tensors the same for the purpose of MIMO
               %%%    Identification, we here using Tw_31(ll,jj,kk,ii,:) instead of
               %%%                            using Tw_31(ii,jj,kk,ll,:).
               %%%
               %%%    April 4, 2001. By Binning Chen.
               %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
               Tw_31(ll,jj,kk,ii,:)=cw;
               
               if USING_cum_true
                  cw = g_TriSpec_ww1w_slice(cum_true,C_LENGTH,NF,ADD_POLYSPECTRA_WINDOW);
               else
                  cw = g_TriSpec_ww1w_slice(cum,C_LENGTH,NF,ADD_POLYSPECTRA_WINDOW);
               end
               
               %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
               %%%
               %%%                        ATTENTION HERE !!!
               %%%
               %%%    Since the MIMO identification algorithm requires that the tensor
               %%%    Tw_22=H o H o conj(H) o conj(H)
               %%%    By the cumulant estimation, we can get a tensor
               %%%          conj(H) o H o H o conj(H)
               %%%    To make these two tensors the same for the purpose of MIMO
               %%%    Identification, we here using Tw_31(kk,jj,ii,ll,:) instead of
               %%%                            using Tw_31(ii,jj,kk,ll,:).
               %%%
               %%%    April 4, 2001. By Binning Chen.
               %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
               Tw_22(kk,jj,ii,ll,:)=cw;
               
            end
         end
      end
   end
   
   SUM_ABS_cum_true_all=sum(sum(sum(sum(sum(sum(sum(abs(cum_true_all))))))))
   SUM_ABS_cum_all_error=sum(sum(sum(sum(sum(sum(sum(abs(cum_true_all-cum_true_all/GAMMA))))))))
   
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%%    Cumulant and spectra estimation ENDS here
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   
   
   for w=1:NF
      
      w
      
      H=HH(:,:,w);
      
      if USING_LievenDeLathauwer_Equation
         %%% Lieven De Lathauwer's Equation
         C_31(1,:)=[Tw_31(1,1,1,1,w), Tw_31(2,1,1,1,w), Tw_31(2,2,1,1,w), Tw_31(2,2,2,1,w)]; 
         C_31(2,:)=[Tw_31(1,1,1,2,w), Tw_31(1,1,2,2,w), Tw_31(1,2,2,2,w), Tw_31(2,2,2,2,w)]; 
      else
         %%% Pierre Comon's Equation, It is wrong here.
         C_31(1,:)=[T_31_true(1,1,1,1), T_31_true(1,1,1,2), T_31_true(1,1,2,2), T_31_true(1,2,2,2)]; 
         C_31(2,:)=[T_31_true(1,1,1,2), T_31_true(1,1,2,2), T_31_true(1,2,2,2), T_31_true(2,2,2,2)]; 
      end
      
      T_22_true=Tw_22(:,:,:,:,w);
      
      
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %%% Comon's code begin here 1
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      
      M=C_31;
      
      V=null(M);v1=V(:,1);v2=V(:,2);
      VECTEURSNOYAU=[v1 v2];
      
      %%% CUMULANTS CIRCULAIRES
      % Cumulant des observations (Complexe Circulaire)
      
      TC = T_22_true;   %%%% TC is actually the C_22 here, we keep using TC, because comon used it.
      TC1111=TC(1,1,1,1);TC1112=TC(1,1,1,2);TC1122=TC(1,1,2,2);
      TC1221=TC(1,2,2,1);TC1222=TC(1,2,2,2);TC2222=TC(2,2,2,2);
      
      % polynome reel associe a TC (en dim double: 4 variables)
      PC=pzz2pX(TC1111,TC1112,TC1122,TC1221,TC1222,TC2222);  
      POLYNOMECIRCULAIRE=PC;
      % PC est de taille 35=nombre de monomes de degre 4 en 4 variables
      
      %%% RECHERCHE DE LA COMBINAISON OPTIMALE
      smin=[];theta0=0;phi0=0;
      
      %X=fmins('objectif',[theta0,phi0]);
      Optim_OPTIONS=optimset('MaxFunEvals', 2000);
      X=fminunc('objectif',[theta0,phi0],Optim_OPTIONS);
      
      
      %%%% PRESENTATION DU RESULTAT
      u=v1*cos(X(1))+v2*sin(X(1))*exp(i*X(2));
      if abs(u(1))<2*eps, 
         q=roots(u(2:n));
         L=[1 q(:).';0 ones(1,2)];
      else 
         q=roots(u); 
         L=[q(:).';ones(1,3)]; 
      end;
      
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %%% Comon's code END here 1
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      
      %%% The system trasnfer function H(w) is
      %%% /                                  \
      %%% | L_roots(1) L_roots(2) L_roots(3) |
      %%% |     1         1           1      |
      %%% \                                  /
      
      %%% Note: The following Hroots and Lroots are actually the minus roots.
      [Hroots Hroots_index(w,:)]=sort(H(1,:)./H(2,:));
      [Lroots Hroots_index(w,:)]=sort(L(1,:));
      %Hroots,Lroots;
      H_roots(w,:)=Hroots;
      L_roots(w,:)=Lroots;
      
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %%% Comon's code begin here 2
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      
      M2=C_31(:,4:-1:1);
      
      V=null(M2);v1=V(:,1);v2=V(:,2);
      VECTEURSNOYAU=[v1 v2];
      
      %%% CUMULANTS CIRCULAIRES