% Gianni Schena  July 2005, schena@units.it
% Lattice Boltzmann LBE, geometry: D2Q9, model: BGK
% Application to permeability in porous media

Restart=false % to restart from an earlier convergence
logical(Restart);

if Restart==false;
close all, clear all % start from scratch and clean ...
Restart=false;
% type of channel geometry ;
% one of the flollowing flags == true
Pois_test=true, % no obstacles in the 2D channel
% porous systems
obs_regolare=false %
obs_irregolare=false %
tic
%   IN
% |vvvv|    + y
% |vvvv|     ^
% |vvvv|     | -> + x
%  OUT

% Pores in 2D : Wet and Dry locations (Wet ==1 , Dry ==0 )
wXh_Dry=[3,1];wXh_Wet=[3,4];

if obs_regolare, % with internal obstacles

A=repmat([zeros(wXh_Dry),ones(wXh_Wet)],[1,3]);A=[A,zeros(wXh_Dry)];
B=ones(size(A));
C=[A;B]  ; D=repmat(C,4,1);
D=[B;D]
end

if obs_irregolare, % with int obstacles
A1=repmat([zeros(wXh_Dry),ones(wXh_Wet)],[1,3]);
A1=[A1,zeros(wXh_Dry)]  ;
B=ones(size(A1));
C1=repmat([ones(wXh_Wet),zeros(wXh_Dry)],[1,3]); C1=[C1,ones(wXh_Dry)];
E=[A1;B;C1;B];
D=repmat(E,2,1);
D=[B;D]
end

if ~Pois_test
figure,imshow(D,[])
Channel2D=D;
Len_Channel_2D=size(Channel2D,1); % Length
Width=size(Channel2D,2); % should not be hod
Channel_2D_half_Width=Width/2,
end

% test without obstacles (i.e. 2D channel & no obstacles)

if Pois_test
%over-writes the definition of the pore space
clear Channel2D
Len_Channel_2D=36, % lunghezza canale 2d
Channel_2D_half_Width=8; Width=Channel_2D_half_Width*2;
Channel2D=ones(Len_Channel_2D,Width); % define wet area
%Channel2D(6:12,6:8)=0; % put fluid obstacle
imshow(Channel2D,[]);
end

[Nr Mc]=size(Channel2D); % Number rows and Munber columns

% porosity
porosity=nnz(Channel2D==1)/(Nr*Mc)

% FLUID PROPERTIES
% physical properties
cs2=1/3; %
cP_visco=0.5; % [cP] 1 CP Dinamic water viscosity 20 C
density=1.; % fluid density
Lky_visco=cP_visco/density; % lattice kinematic viscosity
omega=(Lky_visco/cs2+0.5).^-1; %  omega: relaxation frequency
%Lky_visco=cs2*(1/omega - 0.5) , % lattice kinematic viscosity
%dPdL= Pressure / dL;% External pressure gradient [atm/cm]

uy_fin_max=-0.2;
%dPdL = abs( 2*Lky_visco*uy_fin_max/(Channel_2D_half_Width.^2) );
dPdL=-0.0125;
uy_fin_max=dPdL*(Channel_2D_half_Width.^2)/(2*Lky_visco); % Poiseuille Gradient;
% max poiseuille final  velocity on the flow profile
uy0=-0.001; ux0=0.0001; %  linear vel .. inizialization

%
% uy_fin_max=-0.2; % max poiseuille final  velocity on the flow profile
% omega=0.5, cs2=1/3; % omega: relaxation frequency
% Lky_visco=cs2*(1/omega - 0.5) , % lattice kinematic viscosity
% dPdL = abs( 2*Lky_visco*uy_fin_max/(Channel_2D_half_Width.^2) ); % Poiseuille Gradient;
%

uyf_av=uy_fin_max*(2/3);; % average fluid velocity on the profile

x_profile=([-Channel_2D_half_Width:+Channel_2D_half_Width-1]+0.5);
uy_analy_profile=uy_fin_max.*(1-  ( x_profile /Channel_2D_half_Width).^2 ); % analytical velocity profile

av_vel_t=1.e+10; % inizialization (t=0)
%PixelSize= 5; % [Microns]
%dL=(Nr*PixelSize*1.0E-4); % sample hight [cm]

%
% EXPERIMENTAL SET-UP
% inlet and outlet buffers
inb=2, oub=2; % inlet and outlet buffers thickness
% add fluid at the inlet (top) and outlet (down)
inlet=ones(inb,Mc); outlet=ones(oub,Mc);
Channel2D=[ [inlet]; Channel2D ;[outlet] ] ; % add flux in and down (E to W)
[Nr Mc]=size(Channel2D); % update size
% boundaries related to the experimental set up
wb=2; % wall thickness
Channel2D=[zeros(Nr,wb), Channel2D , zeros(Nr,wb)]; % add walls (no fluid leak)
[Nr Mc]=size(Channel2D); % update size
uy_analy_profile=[zeros(1,wb), uy_analy_profile, zeros(1,wb) ] ; % take into account walls
x_pro_fig=[[x_profile(1)-[wb:-1:1]], [x_profile, [1:wb]+x_profile(end)] ];

% Figure plots analytical parabolic profile
figure(20), plot(x_pro_fig,uy_analy_profile,'-'), grid on,
title('Analytical parab. profile for Poiseuille planar flow in a channel')

% VISUALIZE PORE SPACE & FLUID OSTACLES & MEDIAL AXIS
figure, imshow(Channel2D); title('Vassel geometry');
Channel2D=logical(Channel2D);
% obstacles for Bounce Back ( in front of the grain)
Obstacles=bwperim(Channel2D,8); % perimeter of the grains for bounce back Bound.Cond.
border=logical(ones(Nr,Mc));
border([1:inb,Nr-oub:Nr],[wb+2:Mc-wb-1])=0;
Obstacles=Obstacles.*(border);
figure, imshow(Obstacles); title(' Fluid obstacles (in the fluid)' );
%
Medial_axis=bwmorph(Channel2D,'thin',Inf); %
figure, imshow(Medial_axis); title('Medial axis');
figure(10) % used to visualize evolution of rho
figure(11) % used to visualize ux
figure(12) % used to visualize uy (i.e. top -> down)

% INDICES
% Wet locations etc.
[iabw1 jabw1]=find(Channel2D==1); % indices i,j, of active lattice locations i.e. pore
lena=length(iabw1); % number of active location i.e. of pore space lattice cells
ija= (jabw1-1)*Nr+iabw1; % equivalent single index (i,j)->> ija for active locations
% absolute (single index) position of the obstacles in for bounce back in Channel2D
% Obstacles
[iobs jobs]=find(Obstacles);lenobs=length(iobs); ijobs= (jobs-1)*Nr+iobs; % as above
% Medial axis of the pore space
[ima jma]=find(Medial_axis); lenma=length(ima);  ijma= (jma-1)*Nr+ima; % as above
% Internal wet locations : wet & ~obstables
% (i.e. internal wet lattice location non in contact with dray locations)
[iawint jawint]=find(( Channel2D==1 & ~Obstacles)); % indices i,j, of active lattice locations
lenwint=length(iawint); % number of internal (i.e. not border) wet locations
ijaint= (jawint-1)*Nr+iawint; % equivalent singl
NxM=Nr*Mc;

% DIRECTIONS: E N W S NE NW SW SE ZERO (ZERO:Rest Particle)
%    y^
%  6 2 5           ^         NW  N  NE
%  3 9 1 ... +x-> +y         W   RP  E
%  7 4 8                     SW  S  SE
%   -y
% x & y components of velocities , +x is to est , +y is to nord
East=1; North=2; West=3; South=4; NE=5; NW=6; SW=7; SE=8; RP=9;
N_c=9 ; % number of directions
% versors D2Q9
C_x=[1 0 -1  0 1 -1 -1  1 0];
C_y=[0 1  0 -1 1  1 -1 -1 0]; C=[C_x;C_y]

% BOUNCE BACK SCHEME
% after collision the fluid elements densities f are sent back to the
% lattice node they come from with opposite direction
% indices opposite to 1:8 for fast inversion after bounce
ic_op = [3 4 1 2 7 8 5 6]; %   i.e. 4 is opposite to 2 etc.

% PERIODIC BOUNDARY CONDITIONS - reinjection rules
yi2=[Nr , 1:Nr , 1]; % this definition allows implemening Period Bound Cond
%yi2=[1, Nr , 2:Nr-1 , 1,Nr]; % re-inj the second last to as first
% directional weights (density weights)
w0=16/36. ; w1=4/36. ; w2=1/36.;
W=[ w1 w1 w1 w1 w2 w2 w2 w2 w0];
%c constants (sound speed related)
cs2=1/3; cs2x2=2*cs2; cs4x2=2*cs2.^2;
f1=1/cs2; f2=1/cs2x2; f3=1/cs4x2;
f1=3., f2=4.5; f3=1.5; % coef. of the f equil.

% declarative statemets
f=zeros(Nr,Mc,N_c); % array of fluid density distribution
feq=zeros(Nr,Mc,N_c); % f at equilibrium
rho=ones(Nr,Mc); % macro-scopic density
temp1=zeros(Nr,Mc);
ux=zeros(Nr,Mc);   uy=zeros(Nr,Mc); uyout=zeros(Nr,Mc);  % dimensionless velocities
uxsq=zeros(Nr,Mc); uysq=zeros(Nr,Mc);   usq=zeros(Nr,Mc);  % higher degree velocities

% initialization arrays : start values in the wet area
for ia=1:lena % stat values in the active cells only ; 0 outside
    i=iabw1(ia);  j=jabw1(ia);
    f(i,j,:)=1/9; % uniform density distribution for a start
end
uy(ija)=uy0; ux(ija)=ux0; % initialize fluid velocities
rho(ija)=density;

% EXTERNAL (Body) FORCES e.g. inlet pressure or inlet-outlet gradient
% directions: E N W S NE NW SW SE ZERO
force = -dPdL*(1/6)*1*[0 -1 0 1 -1 -1 1  1  0]'; %;
%...                   E  N E S NE NW SW SE RP ...
% the pressure pushes the fluid down i.e. N to S

% While .. MAIN TIME EVOLUTION LOOP
StopFlag=false; % i.e. logical(0)
Max_Iter=3000; % max allowed number of iteration
Check_Iter=1; Output_Every=20; % frequency of check & output
Cur_Iter=0; % current iteration counter inizialization
toler=1.0e-8; % tollerance to declare convegence
Cond_path=[]; % recording values of the convergence criterium
density_path=[]; % recording aver. density values for convergence
end % ends if restart

if(Restart==true)
 StopFlag=false;  Max_Iter=Max_Iter+3000; toler=1.0e-12;
end

while(~StopFlag)
    Cur_Iter=Cur_Iter+1 % iteration counter update

    % density and moments
    rho=sum(f,3); % density

    if Cur_Iter >1 % use inizialization ux uy to start
        % Moments ... Note:C_x(9)=C_y(9)=0
        ux=zeros(Nr,Mc); uy=zeros(Nr,Mc);
        for ic=1:N_c-1;
            ux = ux + C_x(ic).*f(:,:,ic) ; uy = uy + C_y(ic).*f(:,:,ic)  ;
        end
       % uy=f(:,:,2) +f(:,:,5)+f(:,:,6)-f(:,:,4)-f(:,:,7)-f(:,:,8); % in short !
       % ux=f(:,:,1) +f(:,:,5)+f(:,:,8)-f(:,:,3)-f(:,:,6)-f(:,:,7); % in short !
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    ux(ija)=ux(ija)./rho(ija); uy(ija)=uy(ija)./rho(ija);
    uxsq(ija)=ux(ija).^2; uysq(ija)=uy(ija).^2;
    usq(ija)=uxsq(ija)+uysq(ija); %

    % weighted densities : rest particle, principal axis, diagonals
    rt0 = w0.*rho; rt1 = w1.*rho; rt2 = w2.*rho;

    % Equilibrium distribution
    % main  directions ( + cross)
    feq(ija)= rt1(ija) .*(1 +f1*ux(ija) +f2*uxsq(ija) -f3*usq(ija));
    feq(ija+NxM*(2-1))= rt1(ija) .*(1 +f1*uy(ija) +f2*uysq(ija) -f3*usq(ija));
    feq(ija+NxM*(3-1))= rt1(ija) .*(1 -f1*ux(ija) +f2*uxsq(ija) -f3*usq(ija));
    %feq(ija+NxM*(3)=f(ija)-2*rt1(ija)*f1.*ux(ija); % much faster... !!
    feq(ija+NxM*(4-1))= rt1(ija) .*(1 -f1*uy(ija) +f2*uysq(ija) -f3*usq(ija));

    % diagonals (X diagonals) (ic-1)
    feq(ija+NxM*(5-1))= rt2(ija) .*(1 +f1*(+ux(ija)+uy(ija)) +f2*(+ux(ija)+uy(ija)).^2 -f3.*usq(ija));
    feq(ija+NxM*(6-1))= rt2(ija) .*(1 +f1*(-ux(ija)+uy(ija)) +f2*(-ux(ija)+uy(ija)).^2 -f3.*usq(ija));
    feq(ija+NxM*(7-1))= rt2(ija) .*(1 +f1*(-ux(ija)-uy(ija)) +f2*(-ux(ija)-uy(ija)).^2 -f3.*usq(ija));
    feq(ija+NxM*(8-1))= rt2(ija) .*(1 +f1*(+ux(ija)-uy(ija)) +f2*(+ux(ija)-uy(ija)).^2 -f3.*usq(ija));
    % rest particle (.) ic=9
    feq(ija+NxM*(9-1))= rt0(ija) .*(1 - f3*usq(ija));

    %Collision (between fluid elements)omega=relaxation frequency
    f=(1.-omega).*f + omega.*feq;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %add external body force due to the pressure gradient prop. to dPdL
    for ic=1:N_c;%-1
        for ia=1:lena
            i=iabw1(ia);  j=jabw1(ia);
            % if Obstacles(i,j)==0 % the i,j is not aderent to the boundaries
            % if ( f(i,j,ic) + force(ic) ) >0; %! avoid negative distributions
            %i=1 ;% force only on the first row !
            f(i,j,ic)= f(i,j,ic) + force(ic);
            % end
            % end
        end
    end

    % % STREAM
    % Forward Propagation step & % Bounce Back (collision fluid with obstacles)
    %f(:,:,9) = f(:,:,9); % Rest element do not move

    feq = f; % temp storage of f in feq
        for ic=1:1:N_c-1, % select velocity layer

        ic2=ic_op(ic); % selects the layer of the velocity opposite to ic for BB
        temp1=feq(:,:,ic); %

        % from wet location that are NOT on the border to other wet locations
        for ia=1:1:lenwint % number of internal (i.e. not border) wet locations
            i=iawint(ia);  j=jawint(ia);  % so that we care for the wet space only !
            i2 = i+C_y(ic); j2 = j+C_x(ic); % Expected final locations to move
            i2=yi2(i2+1); % i2 corrected for PBC when necessary (flow out re-fed to inlet)
            % i.e the new position (i2,j2) is sure another wet location
            % therefore normal propagation from (i,j) to (i2,j2) on layer ic
            f(i2,j2,ic)=temp1(i,j); % see circshift(..) fnct for circularly shifts
        end ; % i and j single loop

        % from wet locations that ARE on the border of obstacles
        for ia=1:1:lenobs % wet border locations
            i=iobs(ia);  j=jobs(ia);  % so that we care for the wet space only !
            i2 = i+C_y(ic); j2 = j+C_x(ic); % Expected final locations to move
            i2=yi2(i2+1); % i2 corrected for PBC

            if( Channel2D(i2,j2) ==0 ) % i.e the new position (i2,j2) is dry
                f(i,j,ic2) =temp1(i,j); % invert direction: bounce-back in the opposite direction ic2
            else % otherwise, normal propagation from (i,j) to (i2,j2) on layer ic
                f(i2,j2,ic)=temp1(i,j); % see circshift(..) fnct for circularly shifts
            end ; % b.b. and propagations

        end ; % i and j single loop
        % special treatment for Corners
        %   f(1,wb+1,ic)=temp1(Nr,Mc-wb);      f(1,Mc-wb,ic)=temp1(Nr,wb+1);
        %   f(Nr,wb+1,ic)=temp1(1,Mc-wb);      f(Nr,Mc-wb,ic)=temp1(1,wb+1);

    end ; %  for ic direction

    % ends of Forward Propagation step &  Bounce Back Sections

    % re-calculate  uy as uyout for convergence
    rho=sum(f,3); % density
    % check velocity
    uyout= zeros(Nr,Mc);
    for ic=1:N_c-1;
        uyout= uyout + C_y(ic).*f(:,:,ic) ; % flow dim.less velocity out
    end
   % uyout(ija)=uyout(ija)./rho(ija); % from momentum to velocity

    % Convergence check on velocity values
    if (mod(Cur_Iter,Check_Iter)==0) ; % check for convergence every 'Check_Iter' iterations

        % variables monitored
        % mean density and
        vect=rho(ija); vect=vect(:);
        cur_density=mean(vect);
        % mean 'interstitial' velocity
        % uy(ija)=uy(ija)/rho(ija); ?
        vect=uy(ija); av_vel_int= mean(vect)  ; % seepage velocity (in the wet area)
        % on the whole cross-sectional area of flow (wet + dry)
        av_vel_int=av_vel_int*porosity, % av. vel. on the wet + dry area
        %av_vel_int=mean2(uy),
        av_vel_tp1 = av_vel_int;
        Condition=abs( abs(av_vel_t/av_vel_tp1 )-1), % should --> 0

        Cond_path=[Cond_path, Condition]; % records the convergence path (value)
        density_path=[density_path, cur_density];
        %
        av_vel_t=av_vel_tp1; % time t & t+1

        if (Condition < toler) | (Cur_Iter > Max_Iter)
            StopFlag=true;
            display( 'Stop iteration: Convergence met or iteration exeeding the max allowed' )
            display( ['Current iteration: ',num2str(Cur_Iter),...
                ' Max Number of iter: ',num2str(Max_Iter)] )
            break % Terminate execution of WHILE .. exit the time evolution loop.

        end    % if(Condition < toler

    end

    if (mod(Cur_Iter,Output_Every)==0) ;  % Output from loop every ...
        %if (Cur_Iter>60) ;  % Output from loop every ...

        rho=sum(f,3); % density
        figure(10); imshow(rho,[0.1 0.9]); title(' rho'); % visualize density evolution
        figure(11); imshow(ux,[ ]); title(' ux' ); % visualize fluid velocity horizontal
        figure(12); imshow(-uy,[ ]); title(' uy' ); % visualize fluid velocity down
        figure(14), imshow(-uyout,[]), title('uyout'); % vis vel flow out
        up=2; % linear section to visualize up from the lower row
        figure(15), hold off, feather(ux(Nr-up,:),uy(Nr-up,:)),
        figure(15), hold on , plot(uy_analy_profile,'r-')
        title('Analytical vs LB calculated, fluid velocity parabolic profile')
        pause(3); % time given to visualize properly

    end % every

   % pause(1);

end %  End main time Evolution Loop

% Output & Draw after the end of the time evolution

figure, plot(Cond_path(2:end)); title('convergence path')
%figure, plot(density_path(2:end)); title('density convergence path')
figure, plot( [uy(Nr-up,:)-uy_analy_profile] ); title('difference : LB - Analytical solution')

toc

% Permeability K

K_Darcy_Porous_Sys= (av_vel_int*porosity)/dPdL*Lky_visco ,

K_Analy_2D_Channel=(Width^2)/12


  function ss = outer_sum(xx,yy)
%|function ss = outer_sum(xx,yy)
%|
%| compute an "outer sum" x + y'
%| that is analogous to the "outer product" x * y'
%|
%| in
%|	xx	[nx 1]
%|	yy	[1 ny]
%|		more generally: xx [(dim)] + yy [L,1] -> xx [(dim) LL]
%| out
%|	ss [nx ny]	ss(i,j) = xx(i) + yy(j)
%|
%| Copyright 2001, Jeff Fessler, University of Michigan

if ~nargin, ir_usage, end
if streq(xx, 'test'), outer_sum_test, return, end

% for 1D vectors, allow rows or cols for backward compatibility
if ndims(xx) == 2 && min(size(xx)) == 1 && ndims(yy) == 2 && min(size(yy)) == 1
	nx = length(xx);
	ny = length(yy);
	xx = repmat(xx(:), [1 ny]);
	yy = repmat(yy(:)', [nx 1]);
	ss = xx + yy;
return
end

%if size(xx,1) == 1
%	warn 'xx is a row vector? are you sure?'
%end

% otherwise, xx is not a vector, but yy must be

xdim = size(xx);
ydim = size(yy);
if ndims(yy) ~= 2 || min(size(yy)) ~= 1
	error 'yy must be a vector'
end
sdim = [xdim length(yy)];
xo = repmat(xx, [ones(1, length(xdim)) length(yy)]); % [xdim] -> [xdim ny]
yo = repmat(yy(:), [1 xdim]); yo = permute(yo, [2 3 1]);
% yo = repmat(yy(:)', [xdim 1 1]); % not sure how to make this work.
ss = xo + yo;

function outer_sum_test
%xx = [1:4];
%yy = [0:10:50];
%pr outer_sum(xx,yy)
pr 'outer_sum([1:4], [0:10:50])'
xx = outer_sum([1:4], [0:10:50]);
pr 'outer_sum(xx, [100 200])'


function superima = QLsuperimage(C, S, iscale)
%------------------------------------------------------------------------------
% Creates an image that goes with a multiresolutiondecomposition that has been
% made in the way of LISQ. The superimage, that is an image that comprises
% all levels, is constructed as a rectangular gridfunction.
%
% C        is a one-dimensional array that contains the coefficients of the
%          Quincunx Lifting Scheme (LISQ) decompositions.
% S        is the bookkeeping vector
% iscale   optional inputparameter
%          1 show superimage as is (default)
%          2 show superimage after removal of outliers
%          3 show superimage after enhancement of contrast using
%            histogram equalisation (if available)
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: November 24, 2003.
%  2003 Stichting CWI, Amsterdam
%------------------------------------------------------------------------------
 if nargin == 3
   iopt = iscale;
 elseif nargin == 2
   iopt = 1;
 else
   error(' QLsuperimage - number of arguments should be either 2 or 3 ');
 end
%
 [nS, mS] = size(S);
 if mS ~= 6
   error(' QLsuperimage - unexpected dimensions of bookkeeping vector ');
 end
levels = S(nS,1);
if mod(levels, 2) == 1
  error('  QLsuperimage - only an even number of levels is accepted ');
elseif levels < 1
  superima = [];
else
%
% Firstly, we determine the dimensions of the superimage to be and create the
% necessary space (see LISQ/storeR.m and LISQ/QLiftRec2Nevill.m)
%
  [Detail10, Detail01] = retrieveQ1001(1, 'd', C, S);
  sizeR = size(Detail10) + size(Detail01);
  n  = sizeR(1); m = sizeR(2);
  nH = round((n+m+1.999)/2)+round((m+0.999)/2);
  nm = round((n+m+1.999)/2)+1;
  nV = 1;
  levels = S(nS,1);
  for k = 1:2:levels
    nV = nV + nm;
    nm = round((nm+0.999)/2);
  end
  Approx = retrieveR(levels, 'a', C, S);
  [nl, ml] = size(Approx); nV = nV + nl;
  superima = ones(nV, nH);
%
% Secondly, we fill the superimage with all subsequent detail coefficients
% Put the details at the rotated quincunx grid (level 1)
  background=max(max(max(Detail10)), max(max(Detail01)));
  RotaDet=rota1001fill(Detail10, Detail01, background);
  clear Detail10 Detail01;
%-----intermezzo---begin--
  hgram=ones(1,64);
  hgram(64)=round(31.5*(n/m+m/n))+1;
% hgram is a histogram that is desired to be matched by the image after
% its values have been transformed by histeq, see the documentation of
% histeq() in the Matlab Image Processing Toolbox.
% Due to the rotation of the quincunx grid to make it visible, there
% is a substantial area for padding with one colour (white or nearly
% white). Here follows an overview of the quantities of pixels involved
% at the rotated quincunx grid (asymptotically):
%  Let n and m be the dimensions of the original image.
%
%                               2
%                      ( n + m )
%  Grand total:         -------
%                          4
%
%                         n m
%  Used pixels:           ---
%                          2
%
%                        2    2
%                       n  + m
%  Pixels for padding:  --------
%                          4
%
%                            2    2
%                  n m      n  + m                 n   m
%  Used:padding =  ---   :  -------  = 63 : 31.5*( - + - )
%                   2          4                   m   n
%
%-----intermezzo---end----
  [nl, ml] = size(RotaDet);
  OrigV=1; OrigH=1;
  superima(OrigV:(OrigV+nl-1), OrigH:(OrigH+ml-1)) = imequi(RotaDet,iopt,hgram);
  Detail11 = retrieveR(2, 'd', C, S);
  OrigV=1; OrigH=ml+1;
  [nr, mr] = size(Detail11);
  superima(OrigV:(OrigV+nr-1), OrigH:(OrigH+mr-1)) = imequi(Detail11,iopt);
% Now the other levels, if any
  OrigV=OrigV+nl; OrigH=1;
  for k = 3:2:levels
%   Put the details at the rotated quincunx grid
    [Detail10, Detail01] = retrieveQ1001(k, 'd', C, S);
    background=max([max(max(Detail10))  max(max(Detail01))]);
    RotaDet = rota1001fill(Detail10, Detail01, background);
    clear Detail10 Detail01;
    [nl, ml] = size(RotaDet);
    superima(OrigV:(OrigV+nl-1), OrigH:(OrigH+ml-1)) = imequi(RotaDet,iopt,hgram);
    Detail11 = retrieveR(k+1, 'd', C, S);
    OrigH=ml+1;
    [nr, mr] = size(Detail11);
    superima(OrigV:(OrigV+nr-1), OrigH:(OrigH+mr-1)) = imequi(Detail11,iopt);
    clear Detail11;
    OrigV=OrigV+nl; OrigH=1;
  end
% Thirdly, we fill the superimage with approximation coefficients.
  [nl, ml] = size(Approx);
  superima(OrigV:(OrigV+nl-1), OrigH:(OrigH+ml-1)) = imequi(Approx,iopt);
end
%------------------------------------------------------------------------------


function [C, S] = QLiftDec2MaxMin(X, N)
%-----------------------------------------------------------------------------
% QLiftDec2MaxMin
% Multilevel 2-D decomposition by the lifting scheme and using quincunx grids
%
% The MaxMin scheme has been proposed by Heijmans and Goutsias, see e.g.
%    H.J.A.M. Heijmans, J. Goutsias,
%    Multiresolution signal decomposition schemes.
%    Part 2: morphological wavelets.
%    CWI Report PNA-R9905, Amsterdam, 1999.
%    http://repository.cwi.nl:8888/cwi_repository/docs/IV/04/04625D.pdf
%
% Calls for: QLmaxlev,
%            storeQ1001, storeR,
%            getcolor01, getcolor10, getcolor00, getcolor11,
%            putcolor01, putcolor10, putcolor00, putcolor11.
% See also: QLiftRec2MaxMin
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: May 16, 2002.
% (c) 1999-2002 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
%Firstly, check input data
%
if  isempty(X)
  error(' QLiftDec2MaxMin - empty matrix ');
else
  if mod(N, 2) == 1
    error(' QLiftDec2MaxMin - only an even number of levels is accepted ');
  end
  if QLmaxlev(size(X), 'maxmin') < N
    error(' QLiftDec2MaxMin - too many levels requested ');
  end
  if N < 2
    disp([' QLiftDec2MaxMin - WARNING too few levels requested ' ...
          '-> empty decomposition ']);
  end
end
%
%Secondly, start decomposition
%
O = X; % For the sake of efficient use of memory this could be improved upon.
% We descend to coarser grids, integer lev indicates number of scale.
C = []; S = [];

for lev=1:2:N
%
   [nO, mO] = size(O);
   if ( nO < 3 ) || ( mO < 3)
     error(' QLiftDec2MaxMin - too many levels ');
   end
   minO = min(min(O));
   maxO = max(max(O));
   cmin = minO-(maxO-minO);
   cmax = maxO+(maxO-minO);
%
%  The Lifting Scheme proceeds from a rectangular grid
%  towards a quincunx grid.
%
%  Stage: predict
   A00 =getcolor00(O);
   A11 =getcolor11(O);
%  Quincunx grid Q0011 is the union of the values at .00 and .11: "even slots"
%  Quincunx grid Q1001 is the union of the values at .10 and .01: "odd slots"
   Q1001D01 = getcolor01(O) - synA01max(A11, A00, cmin);                % Y1
   Q1001D10 = getcolor10(O) - synA10max(A11, A00, cmin);                % Y1
%  At this point the union (quincunx) of Q1001D01 & Q1001D10
%  contains the DETAILS of O.
%
%  For the inverse transform Q1001D01 and Q1001D10 have to be stored:
   [C, S] = storeQ1001( Q1001D10, Q1001D01, lev, 'd', C, S);
%
%  Stage: update
   Q0011A00 = A00 + ...
        max(zeros(size(A00)), synA00max(Q1001D10, Q1001D01, cmin));     % X1
   clear A00;
   Q0011A11 = A11 + ...
        max(zeros(size(A11)), synA11max(Q1001D10, Q1001D01, cmin));     % X1
   clear A11 Q1001D10 Q1001D01;
%  At this point the union (quincunx) of Q0011A00 & Q0011A11
%  contains the updated APPROXIMATION of O, the DETAILS of O
%  were in the union (quincunx) of Q1001D01 & Q1001D10 (see above).
%
%  The Lifting Scheme proceeds by a subsequent step from quincunx
%  to rectangular grid.
%
%  Q0011 is split into the 11 colour with the "odd slots" and
%  the 00 colour with the "even slots".
%
%  Stage: predict
   DETAIL11 = Q0011A11 - synA11Qmin(Q0011A00, size(Q0011A11), cmax);        % Y2
   clear Q0011A11;
%  Stage: update
   APPROX00 = Q0011A00 + ...
     min(zeros(size(Q0011A00)), synA00Qmin(DETAIL11, size(Q0011A00), cmax));% X2
%
%  DETAIL11 presents the detail gridfunction w.r.t. Q0011
%  APPROX00 now represents the updated version of the approximation of Q0011
%
%  For the inverse transform DETAIL11 has to be stored:
   [C, S] = storeR( DETAIL11, lev+1, 'd', C, S);
   clear Q0011A00 DETAIL11;
%
%  At this point gridfunction DETAIL11 containing the DETAILS has been stored,
%  gridfunction APPROX00 contains the updated APPROXIMATION, on the (down-
%  sampled) rectangular grid and has to be stored as well if at the highest
%  scale.
%  Note that APPROX00 is downsampled onto a rectangular grid with dimensions of
%  half size of the original O.
   if lev+1 >= N
     [C, S] = storeR(APPROX00, lev+1, 'a', C, S);
%    It is obligatory that at least at one scale the Approximation has to be
%    stored or else the scheme cannot be inverted.
     clear APPROX00;
%    In the Lifting Scheme all scales have now been processed!
   else
%    We proceed to the next scale.
     O = APPROX00; clear APPROX00;
   end
end
%-----------------------------------------------------------------------------


function [fYr, nMn, nDay, nHr, nMin, nSec]=decyear2mat(fDy)
    % Calculate decimal year format to matrix with columns year, month, day, hour, minute, and second.
    % [fYr, nMn, nDay, nHr, nMin, nSec] = decyear2mat(fDy)
    % ------------------------------------------------------------------------------------------------
    % Calculate decimal year format (from zmap-function decyear, i.e. 1998.734)
    % to matrix with columns year, month, day, hour, minute, and second.
    % This function was programmed because datevec.m does not work with decimal
    % year input format
    %
    % Input parameters:
    %   fDy     Decimal year (like 1998.2515) as vector or float
    %
    % Output parameters:
    %   fYr     decimal year
    %   nMn     month
    %   nDay    day
    %   nHr     hour
    %   nMin    minute
    %   nSec    second
    %
    % Example [fYr, nMn, nDay, nHr, nMin, nSec]=decyear2mat(decyear([1989 12 31 10 35 44.3]))
    %
    % Thomas van Stiphout
    % Mai 9, 2007

    disp('~/zmap/src/decyear2mat.m')

    % save year in decimal format
    fYr=fDy;
    % define leap years
    bLeapYr = rem(fix(fDy),4) == 0 & rem(fix(fDy),100) ~= 0 | rem(fix(fDy),400) == 0 ;

    % loop over each date
    for i=1:size(bLeapYr,1)
        if bLeapYr(i) % for leap years
            mDay=[0,31,60,91,121,152,182,213,244,274,305,335]'; %leapyear
            nMn(i)=sum(mDay<rem(fDy(i),1).*366); % calculate year
            nDay(i)=ceil(rem(fDy(i),1).*366)-mDay(nMn(i)); % calculate days
            nHr(i)=rem(fDy(i),1).*366.*24-(mDay(nMn(i))+nDay(i)-1).*24;
            nMin(i)=rem(nHr(i),1).*60;
            nHr(i)=fix(nHr(i)); % hours
            nSec(i)=rem(nMin(i),1).*60; % seconds
            nMin(i)=fix(nMin(i)); % minutes

        else
            mDay= [0,31,59,90,120,151,181,212,243,273,304,334]';%cumulative days in one year
            nMn(i)=sum(mDay<rem(fDy(i),1)*365);
            nDay(i)=ceil(rem(fDy(i),1)*365)-mDay(nMn(i));
            nHr(i)=rem(fDy(i),1)*365*24-(mDay(nMn(i))+nDay(i)-1)*24;
            nMin(i)=rem(nHr(i),1)*60;
            nHr(i)=fix(nHr(i));
            nSec(i)=rem(nMin(i),1)*60;
            nMin(i)=fix(nMin(i));
        end
    end
end


function result = convolutionalPropagation(edges, edgeWeights, fixedVertices, ...
                    fixedDists,areaWeights,kernel,kernelTranspose,entropyLimit)

ne = size(edges,1);
nv = max(edges(:));
n = size(fixedDists,1);

% Initialize marginals
vv = ones(n,ne);
ww = ones(n,ne);
d = zeros(n,ne);

result = ones(n,nv);
result(:,fixedVertices) = fixedDists;

% For convenience, a binary array
isBoundary = zeros(nv,1);
isBoundary(fixedVertices) = 1;

% For each vertex compute his incoming and outgoing edges
incomingEdges = cell(nv,1);
outgoingEdges = cell(nv,1);

for i=1:ne
    e1 = edges(i,1);
    e2 = edges(i,2);
    outgoingEdges{e1} = [outgoingEdges{e1} i];
    incomingEdges{e2} = [incomingEdges{e2} i];
end

% No reason to project a distribution whose neighbors haven't been projected
queue = zeros(nv,1);
queue(fixedVertices) = 1;

projectEntropy = 0;

for j=1:1000
    toVisit = find(queue);

%     if mod(j,2)==0
%         toVisit = toVisit(end:-1:1); % reverse order each time for fun
%     end
%     toVisit = toVisit(randperm(length(toVisit))); % to avoid bias

    % Reset queue
    queue = zeros(nv,1);
    queue(fixedVertices) = 1;

    oldResult = result;

    for k=1:length(toVisit)
        v = toVisit(k);

        if isBoundary(v)
            p = result(:,v);

            for i=1:length(outgoingEdges{v}) %(v,w)
                e = outgoingEdges{v}(i);
                ww(:,e) = p ./ kernelTranspose(vv(:,e).*areaWeights);
            end

            for i=1:length(incomingEdges{v}) %(w,v)
                e = incomingEdges{v}(i);
                vv(:,e) = p ./ kernel(ww(:,e).*areaWeights);
            end
        else % not boundary
            omega = sum(edgeWeights(outgoingEdges{v}))+sum(edgeWeights(incomingEdges{v}));
            p = ones(n,1);

            for i=1:length(outgoingEdges{v}) %(v,w)
                e = outgoingEdges{v}(i);
                d(:,e) = ww(:,e).*kernelTranspose(vv(:,e).*areaWeights);
                d(:,e) = max(d(:,e),1e-10);
                p = p .* d(:,e).^(edgeWeights(e)/omega);
            end

            for i=1:length(incomingEdges{v}) %(w,v)
                e = incomingEdges{v}(i);
                d(:,e) = vv(:,e).*kernel(ww(:,e).*areaWeights);
                d(:,e) = max(d(:,e),0);
                p = p .* d(:,e).^(edgeWeights(e)/omega);
            end

            entropy = -sum(p.*log(p).*areaWeights);
            if nargin == 8 && entropy > entropyLimit && projectEntropy
                try % just ignore projection if it fails
                    fn = @(x) full(-sum(x*areaWeights.*((p.^x).*log(p))) - entropyLimit);
                    options = optimset('Display','none','tolfun',1e-6,'tolx',1e-6);
                    a = fzero(fn,[0 10],options);
                    p = p.^a;
                end
            end

            result(:,v) = p;

            for i=1:length(outgoingEdges{v}) %(v,w)
                e = outgoingEdges{v}(i);
                ww(:,e) = ww(:,e) .* p ./ d(:,e);
            end

            for i=1:length(incomingEdges{v}) %(w,v)
                e = incomingEdges{v}(i);
                vv(:,e) = vv(:,e) .* p ./ d(:,e);
            end
        end

        queue(edges(incomingEdges{v},:)) = 1;
        queue(edges(outgoingEdges{v},:)) = 1;
        queue(v) = 1;
    end

%     subplot(1,2,1);
%     imagesc(log(max(result,1e-20)));
%     colorbar;
%     title(sprintf('Iteration %d, log scale',j));
%     axis off;
%
%     subplot(1,2,2)
%     imagesc(result);
%     colorbar;
%     title(sprintf('Iteration %d, result',j));
%     axis off;
%
%     drawnow;

    change = sum(sum(bsxfun(@times,areaWeights,abs(result-oldResult))))/nv;
    change = full(change);

    fprintf('Iteration %d:  %g\n', j, change);

    if ~projectEntropy && change < 1e-3 && length(toVisit) == nv % was -3
        fprintf('Starting to project entropy.\n');
        projectEntropy = 1;
    elseif change < 1e-5 && j > 5 && projectEntropy
        return
    end
end


function [W,CovW,X,CovX,R] = rotate_to_pca(W,CovW,X,CovX,weightsWW)
% [W,CovW,X,CovX,R] = rotate_to_pca(W,CovW,X,CovX,weightsW)

% Dimensionalities
[M,D] = size(W);
[D,N] = size(X);

% Convert cell covariance matrices to arrays
cellW = false;
if iscell(CovW)
  CovW = covcell_to_covarray(CovW);
  cellW = true;
end

cellX = false;
if iscell(CovX)
  CovX = covcell_to_covarray(CovX);
  cellX = true;
end

if nargin < 5 || isempty(weightsWW)
  weightsWW = ones(M,1);
else
  weightsWW = weightsWW(:) .* ones(M,1);
end

%% Find mixing rotation Xpca = R * Xgp

R = 1;

% 1) Whiten <XX'>

XX = X * X' + sum(CovX,3);
[V,A] = svd(XX/N);
R = diag(sqrt(1./diag(A))) * V';
% Rotate W
W = W / R;
for i=1:rows(W)
  CovW(:,:,i) = R' \ CovW(:,:,i) / R;
end

% 2) Orthogonalise weighted <W'W>

% Evaluate weighted second moment
WW = W' * diag(weightsWW) * W;
for m=1:M
  WW = WW + weightsWW(m) * CovW(:,:,m);
end

% Diagonalise it
[V,D] = svd(WW);
R = V' * R;

% Rotate W
W = W * V;
for m=1:M
  CovW(:,:,m) = V' * CovW(:,:,m) * V;
end

% $$$ % Debug the rotation!
% $$$ weights2 = cosd(data.coordinates(2,:));
% $$$ WW = W' * diag(weights2) * W;
% $$$ for m=1:M
% $$$   WW = WW + weights2(m) * CovW(:,:,m);
% $$$ end
% $$$ XX = R * (X*X'+sum(CovX,3)) * R';
% $$$ WW(1:10,1:10)
% $$$ XX(1:10,1:10) / size(X,2)

% Rotate X to PCA
X = R * X;
for n=1:N
  CovX(:,:,n) = R * CovX(:,:,n) * R';
end

% Convert back to cells
if cellW
  CovW = covarray_to_covcell(CovW);
end
if cellX
  CovX = covarray_to_covcell(CovX);
end


function [obj_out, varargout] = image_math(obj1, varargin)
% Perform simple mathematical and boolean operations on image objects
%
% :Usage:
% ::
%
%    obj_out = image_math(obj1, [optional inputs, e.g., a 2nd object, keywords])
%
% For objects: Type methods(object_name) for a list of special commands
%              Type help object_name.method_name for help on specific
%              methods.
%
% :Inputs:
%
%   **obj1:**
%        An image_vector object
%
% :Optional Inputs:
%
%   **obj2:**
%        An additional image_vector object
%   **{'add', 'plus'}:**
%        Keyword to perform image-wise addition of images in obj1
%                              and obj2.  Assumes these are paired/matched objects.
%   **{'subtract', 'minus'}:**
%        Keyword to perform image-wise subtraction of images
%                              in obj1 and obj2
%   **{'cat', 'concatenate'}:**
%        Concatenate obj1 and obj2 image-wise.  Requires same
%                              number of voxels in both image sets.  Returns effects
%                              codes of 1, -1 in obj_out.Y.
%   **{'power'}:**
%        Keyword to raise data to power element-wise; obj.dat = obj.dat.^b;
%                              Followed by exponent to apply (b)
%
%   **{'rmssd'}:**
%        Keyword to perform root-mean-square successive differences.
%        Meaningful if image series is a timeseries.
%
%        [obj, rmssd, wh_outliers] = image_math(obj, 'rmssd');
%        Does not affect object.
%
% :Outputs:
%
%   **obj_out:**
%        The result - an image_vector object
%
% ..
%     Author and copyright information:
%
%     Copyright (C) 2015  Tor Wager
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
% ..

% ..
% DEFAULTS AND INPUTS
% ..

keyword = '';  % initalize optional variables to default values here.
% keyword. should enter one...
obj2 = [];
my_exponent = [];

% optional inputs with default values
% -----------------------------------

for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}

            case {'add', 'plus'}, keyword = 'plus'; varargin{i} = [];
            case {'subtract', 'minus'}, keyword = 'minus'; varargin{i} = [];
            case {'cat', 'concatenate'}, keyword = 'cat'; varargin{i} = [];

            case 'power', keyword = 'power'; varargin{i+1} = my_exponent;
                %case 'basistype', basistype = varargin{i+1}; varargin{i+1} = [];

            case 'rmssd', keyword = 'rmssd'; varargin{i} = [];

            otherwise, warning(['Unknown input string option:' varargin{i}]);
        end

    elseif isa(varargin{i}, 'image_vector')

        obj2 = varargin{i}; varargin{i} = [];

    end
end

% -------------------------------------------------------------------------
% DATA CHECKS
% -------------------------------------------------------------------------

switch keyword

    case ''
        disp('Nothing to do.')
        return

    case 'power'
        %Check if image_vector object
        if ~isa(obj1,'image_vector')
            error('Input Data is not an image_vector object')
        end

    case {'plus', 'minus', 'cat'}
        n1 = size(obj1.dat, 2);
        n2 = size(obj2.dat, 2);

        y1 = ones(n1, 1);
        y2 = ones(n2, 1);

        %Check if image_vector object
        if ~isa(obj1,'image_vector') || ~isa(obj2,'image_vector')
            error('Input Data is not an image_vector object')
        end

        %Check number of rows
        if size(obj1.dat,1) ~= size(obj2.dat,1)
            % Voxel list mismatch
            % Try to fix

            obj1 = replace_empty(obj1);
            obj2 = replace_empty(obj2);

            if size(obj1.dat,1) ~= size(obj2.dat,1)
                % If still different, give a warning and resample.

                warning('Image_math: Objects being operated on do not appear to be in the same space. Resampling 2nd object, but proceed with caution!!');

                obj2 = resample_space(obj2, obj1);
            end

            %error('number of voxels is different between objects.')
        end

        if strcmp(keyword, 'cat')
            % we are done

        else

            % The rest is for plus/minus, which assume paired images

            %Check number of columns
            if n1 ~= n2
                error('Sizes of objects do not match.');
            end

        end

    case 'rmssd'

    otherwise
        warning(['Unknown keyword:' keyword]);
        return

end % keyword ; data checks

% -------------------------------------------------------------------------
% RUN OPERATION
% -------------------------------------------------------------------------

switch keyword

    case ''
        disp('Nothing to do.')
        return

    case 'plus'

        obj_out = obj1;
        obj_out.dat = obj_out.dat + obj2.dat;
        obj_out.history{end+1} = 'Image-wise addition operation by image_math';
        obj_out.dat_descrip = cell(1, 3);
        obj_out.dat_descrip{1} = 'Names of images added in next cells, 1st set plus 2nd';
        obj_out.dat_descrip{2} = obj1.fullpath;
        obj_out.dat_descrip{3} = obj2.fullpath;
        obj_out.image_names = [];
        obj_out.fullpath = [];
        obj_out.files_exist = false;

    case 'minus'

        obj_out = obj1;
        obj_out.dat = obj_out.dat - obj2.dat;
        obj_out.history{end+1} = 'Image-wise subtraction operation by image_math';
        obj_out.dat_descrip = cell(1, 3);
        obj_out.dat_descrip{1} = 'Names of images subtracted in next cells, 1st set minus 2nd';
        obj_out.dat_descrip{2} = obj1.fullpath;
        obj_out.dat_descrip{3} = obj2.fullpath;
        obj_out.image_names = [];
        obj_out.fullpath = [];
        obj_out.files_exist = false;

    case 'cat'

        obj_out = obj1;
        obj_out.dat = [obj_out.dat obj2.dat];
        obj_out.history{end+1} = 'Image-wise concatenation operation by image_math';
        obj_out.image_names = char(obj1.image_names, obj2.image_names);
        obj_out.fullpath = char(obj1.fullpath, obj2.fullpath);
        if isfield(obj_out, 'Y')
            obj_out.Y = [y1; -y2];
            obj_out.Y_descrip = 'Effects codes for image set A (1) and B (-1) added by image_math';
        end
    case 'power'
        obj_out = obj1;
        obj_out.dat = obj_out.dat .^ my_exponent;
        obj_out.history{end+1} = sprintf('Raised to %3.0f element-wise by image_math', my_exponent);

    case 'rmssd'

        obj_out = obj1;

        sdiffs = diff(obj1.dat')';
        sdiffs = [mean(sdiffs, 2) sdiffs]; % keep in image order
        rmssd = ( mean(sdiffs .^ 2) ) .^ .5; % rmssd - root mean square successive diffs

        % avoid first time point being very different and influencing distribution and plots.
        rmssd(1) = median(rmssd);

        % z-scores of rmssd
        wh_outliers = scale(rmssd) > 3;

        varargout{1} = rmssd;
        varargout{2} = wh_outliers;

    otherwise
        warning(['Unknown keyword:' keyword]);
        return

end % switch keyword; operation

end % main function


%Charge Neutrality Equation
%Assuming that the material contains only one type of dopant
%NA (Acceptor)
function F = neutralp(Nv,Nc,NA,Ev,Ec,Ea,Efp,k,T,gA)
F = Nv*((exp(-((Ev-Efp)/(k*T)))+3*sqrt(pi/2)*((((Ev-Efp)/(k*T))+2.13)+((abs(((Ev-Efp)/(k*T))-2.13)).^2.4+9.6).^(5/12)).^(-3/2)).^-1)-Nc*((exp(-((Efp-Ec)/(k*T)))+3*sqrt(pi/2)*((((Efp-Ec)/(k*T))+2.13)+((abs(((Efp-Ec)/(k*T))-2.13)).^2.4+9.6).^(5/12)).^(-3/2)).^-1)-(NA/(1+gA*exp((Ea-Efp)/(k*T))));


%% This file is the PDE simulation file of SINDy-PI discovered Belousov-Zhabotinsky reaction.
% Coded By: K
% Last Updated: 2019/09/19
%%
function [rhs,x_t,z_t,s_t,u_t,x,z,s,u,x_x,z_x,s_x,u_x,x_y,z_y,s_y,u_y,x_xx,z_xx,s_xx,u_xx,x_yy,z_yy,s_yy,u_yy,x_lap,z_lap,s_lap,u_lap]=...
    BZ_Reaction_SINDy_PI_PDE(t,xzsut,Kx,Kxx,Ky,Kyy,K22,n,N,NeedDev)

% Calculate u and v terms
xt=reshape((xzsut(1:N)),n,n);
zt=reshape((xzsut((N+1):(2*N))),n,n);
st=reshape((xzsut(2*N+1:3*N)),n,n);
ut=reshape((xzsut((3*N+1):(4*N))),n,n);

x=real(ifft2(xt));
z=real(ifft2(zt));
s=real(ifft2(st));
u=real(ifft2(ut));

% Reaction Terms (You need to manully code up the equation discovered by DL-SINDy)
xtrhs=reshape((fft2(  (0.24667*x + 0.33333*s + 0.5*z + 3.3333*x.*s - 5.0*x.*z + 2.1333*x.^2 - 3.3333*x.^3)./(x + 0.1)  )),N,1);
ztrhs=reshape((fft2(  x + 0.4*u - 1.3*z  )),N,1);
strhs=reshape((fft2(  0.17333*x-0.66667*s  )),N,1);
utrhs=reshape((fft2(  100*z-133.33*u  )),N,1);

rhs=[-K22.*xzsut(1:N)+xtrhs
     -0.1*K22.*xzsut(N+1:2*N)+ztrhs
     -K22.*xzsut(2*N+1:3*N)+strhs
     -K22.*xzsut(3*N+1:4*N)+utrhs
     ];

 % If you don't need to extract the value, don't run the following code to
 % speed up
if NeedDev==1
    % Get the derivative you want
    x_x=real(ifft2(reshape((1j*Kx).*xzsut(1:N),n,n)));
    z_x=real(ifft2(reshape((1j*Kx).*xzsut(N+1:2*N),n,n)));
    s_x=real(ifft2(reshape((1j*Kx).*xzsut(2*N+1:3*N),n,n)));
    u_x=real(ifft2(reshape((1j*Kx).*xzsut(3*N+1:4*N),n,n)));
    %
    x_y=real(ifft2(reshape((1j*Ky).*xzsut(1:N),n,n)));
    z_y=real(ifft2(reshape((1j*Ky).*xzsut(N+1:2*N),n,n)));
    s_y=real(ifft2(reshape((1j*Ky).*xzsut(2*N+1:3*N),n,n)));
    u_y=real(ifft2(reshape((1j*Ky).*xzsut(3*N+1:4*N),n,n)));
    %
    x_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(1:N),n,n)));
    z_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(N+1:2*N),n,n)));
    s_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(2*N+1:3*N),n,n)));
    u_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(3*N+1:4*N),n,n)));
    %
    x_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(1:N),n,n)));
    z_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(N+1:2*N),n,n)));
    s_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(2*N+1:3*N),n,n)));
    u_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(3*N+1:4*N),n,n)));
    %
    x_t=real(ifft2(reshape(rhs(1:N),n,n)));
    z_t=real(ifft2(reshape(rhs(N+1:2*N),n,n)));
    s_t=real(ifft2(reshape(rhs(2*N+1:3*N),n,n)));
    u_t=real(ifft2(reshape(rhs(3*N+1:4*N),n,n)));
    %
    x_lap=real(ifft2(reshape(-K22.*xzsut(1:N),n,n)));
    z_lap=real(ifft2(reshape(-K22.*xzsut(N+1:2*N),n,n)));
    s_lap=real(ifft2(reshape(-K22.*xzsut(2*N+1:3*N),n,n)));
    u_lap=real(ifft2(reshape(-K22.*xzsut(3*N+1:4*N),n,n)));

end


function a = dirichlet_fit_s_simple(data,a)
% DIRICHLET_FIT_S_SIMPLE   Initial guess for Dirichlet precision.
%
% DIRICHLET_FIT_S_SIMPLE(data,a) returns an initial guess for the Dirichlet
% parameter vector A, by scaling the input A.

bar_p = mean(log(data));
m = a/sum(a);
bar_p = sum(m.*bar_p);
s = 1/(sum(m.*log(m)) - bar_p);
s = s*(length(m)-1)/2;
a = s*m;


function sparse_count_test08 ( dim_min, dim_max, level_max_min, ...
  level_max_max )

%*****************************************************************************80
%
%% SPARSE_COUNT_TEST08 tests OWN_E_SIZE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_MIN, the minimum spatial dimension to consider.
%
%    Input, integer DIM_MAX, the maximum spatial dimension to consider.
%
%    Input, integer LEVEL_MAX_MIN, the minimum value of LEVEL_MAX to consider.
%
%    Input, integer LEVEL_MAX_MAX, the maximum value of LEVEL_MAX to consider.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_COUNT_TEST08\n' );
  fprintf ( 1, '  OWN_E_SIZE returns the number of\n' );
  fprintf ( 1, '  distinct points in an OWN_E sparse grid made from \n' );
  fprintf ( 1, '  product grids formed from open weakly nested \n' );
  fprintf ( 1, '  quadrature rules with exponential growth, including:\n' );
  fprintf ( 1, '  * GGH_E, the Generalized Gauss-Hermite Exponential Growth Family;\n' );
  fprintf ( 1, '  * GH_E, the Gauss-Hermite Exponential Growth Family;\n' );
  fprintf ( 1, '  * LG_E, the Gauss-Legendre Exponential Growth Family;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   DIM: ' );

  for dim_num = dim_min : dim_max
    fprintf ( 1, '  %10d', dim_num );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   LEVEL_MAX\n' );
  fprintf ( 1, '\n' );

  for level_max = level_max_min : level_max_max
    fprintf ( 1, '    %4d', level_max );
    for dim_num = dim_min : dim_max
      point_num = own_e_size ( dim_num, level_max );
      fprintf ( 1, '  %10d', point_num );
    end
    fprintf ( 1, '\n' );
  end

  return
end


%% This file is the main file of using the SINDy-PI method to
% infer the Single Pendulum on Cart Model.
%
% Date: 2019/07/26
% Coded By: K

%% Close all, clear all, clc
close all;clear all; clc;
dir
set(0,'defaulttextInterpreter','latex')
[status,message,messageid] = mkdir('Results');
addpath('Function');
%% Simulate the budworm population growth and gather the simulation data

%Define the model parameters
M=1;m=1;L=1;g=9.81;

% Define noise level and add gaussian noise to the data
noise=0.02;

% Define whehter you have control, if you have it, please define it
Control=1;

%Define whether you want to shuffel the final data
Shuffle=0;

% Peform simulation
state0=[0.3;0;1;0];state0_test=[0.1;0;0.3;0];
dt=0.001;T=16;T_test=2;
tspan=0:dt:T;tspan_test=0:dt:T_test;
u=-0.2+0.5*sin(6*(tspan'));
u_test=-1+1*sin((tspan_test'))+3*sin(2*(tspan_test'));
[dData,Data]=Get_Sim_Data(@(t,y,u)SinglePendulum_ODE(t,y,u,M,m,L,g),state0,u,tspan,noise,Control,Shuffle);
dData(:,1)=Data(:,3);dData(:,2)=Data(:,4);
[dData_test,Data_test]=Get_Sim_Data(@(t,y,u)SinglePendulum_ODE(t,y,u,M,m,L,g),state0_test,u_test,tspan_test,noise,Control,Shuffle);
dData_test(:,1)=Data_test(:,3);dData_test(:,2)=Data_test(:,4);
%% Plot the simulation data
% close all
% HorizontalRange=5*L;
% VerticalRange=2.5*L;
% displayrate=1;
% cartoonOffLineAcce(Data(:,2),Data(:,1),L,HorizontalRange,VerticalRange,displayrate)
%%
% figure(1)
% plot(tspan,Data(:,1))
% title('Theta')
% grid on
% %
% figure(2)
% plot(tspan,Data(:,2))
% title('Position')
% grid on
% %
% figure(3)
% plot(tspan,Data(:,3))
% title('dTheta')
% grid on
% %
% figure(4)
% plot(tspan,Data(:,4))
% title('Velocity')
% grid on
% %
% figure(5)
% plot(tspan,u,'linewidth',3,'color','black')
% box('off')
% axis('on')

%% Now perform sparse regression of non-linear dynamics

% Get the number of states we have
[dtat_length,n_state]=size(Data);

% Define the control input(Should be zero in our example)
n_control=1;

% Choose whether you want to display actual ODE or not
disp_actual_ode=1;

% If the ODEs you want to display is the actual underlyting dynamics of the
% system, please set actual as 1
actual=1;

% Print the actual ODE we try to discover
digits(4)
z_vars=sym('z',[1,n_state]);
u_vars=sym('u',[1,n_control]);
d_vars=sym('dz',[1,n_state]);
ODEsP=SinglePendulum_ODE(0,z_vars,u_vars,M,m,L,g);
fprintf('The actual ODE of the system is/are :\n')
for i=1:n_state
    fprintf(strcat(char(d_vars(1,i)),'=',char(ODEsP(i,1)),'\n'));
end

% The implicit ODE has the following form:
% dz1=z3
% dz2=z4
% dz3=((981*sin(z1))/50 + u1*cos(z1) + z3^2*cos(z1)*sin(z1))/(cos(z1)^2 - 2)
% dz4=-(u1 + z3^2*sin(z1) + (981*cos(z1)*sin(z1))/100)/(cos(z1)^2 - 2)

% Create symbolic states
dz=sym('dz',[n_state,1]);
z=sym('z',[n_state,1]);
uc=sym('u',[n_control,1]);

% Now we first create the parameters of the function right hand side
Highest_Poly_Order_Guess=1;
Highest_Trig_Order_Guess=2;
Highest_U_Order_Guess=0;

% Then create the right hand side library parameters
Highest_Poly_Order=2;
Highest_Trig_Order=2;
Highest_U_Order=0;
Highest_dPoly_Order=1;

%% Define parameters for the sparese regression
lam=[1e-4;5e-4;1e-3;2e-3;3e-3;4e-3;5e-3;6e-3;7e-3;8e-3;9e-3;1e-2;2e-2;3e-2;4e-2;5e-2;...
    6e-2;7e-2;8e-2;9e-2;1e-1;2e-1;3e-1;4e-1;5e-1;6e-1;7e-1;8e-1;9e-1;1;1.5;2;2.5;3;3.5;4;4.5;5;...
    6;7;8;9;10;20;30;40;50;100;200];

N_iter=20;
disp=0;
LibPoly=[1;1;0;0];
LibTrig=[0;0;3;4];
%
GuessPoly=[1;1;0;0];
GuessTrig=[0;0;1;1];

% Normalize the library?
NormalizeLib=0;

% Define a cell matrix to store the variable
Xi_Final=cell(n_state,1);
Score_Final=cell(n_state,1);

for iter=1:n_state
    % Change the library base on what equation you want to work on
    Highest_Poly_Order=LibPoly(iter);
    Highest_Poly_Order_Guess=GuessPoly(iter);
    Highest_Trig_Order=LibTrig(iter);
    Highest_Trig_Order_Guess=GuessTrig(iter);

    fprintf('Calculating the %i expression \n',iter)

    % According to the previous parameter generate the left hand side guess
    [LHS_Data,LHS_Sym]=GuessLib(Data,dData(:,iter),iter,u,Highest_Poly_Order_Guess,Highest_Trig_Order_Guess,Highest_U_Order_Guess);

    %Generate the corresponding data
    [SINDy_Data,SINDy_Struct]=SINDyLib(Data,dData(:,iter),iter,u,Highest_Poly_Order,Highest_Trig_Order,Highest_U_Order,Highest_dPoly_Order);

    % Run the for loop and try all the left hand guess
    for i=1:length(LHS_Sym)
        fprintf('\n\t\t Testing the left hand side as %s... \n',cell2sym(LHS_Sym(1,i)))

        % Exclude the guess from SINDy-PI library
        [RHS_Data,RHS_Struct]=ExcludeGuess(SINDy_Data,SINDy_Struct,LHS_Sym{i});

        if iter==1
            Xi=cell(length(LHS_Sym),length(lam));
            Score=zeros(length(LHS_Sym),length(lam));
            ODE=cell(length(LHS_Sym),length(lam));
        end

        % Define dummy variables for parfor
        LHS_Sym_Dum=LHS_Sym{i};
        LHS_Data_Dum=LHS_Data(:,i);
        dz_Dum=dz(iter);
        dData_test_dum=dData_test(:,iter);
        for j=1:length(lam)
            fprintf('\n\t\t Testing the lambda as %d... \n',lam(j,1))
            % Perform the sparse regression problem
            [Xi{i,j},ODE{i,j}]=sparsifyDynamics(RHS_Data,LHS_Data_Dum,LHS_Sym_Dum,lam(j,1),N_iter,RHS_Struct,disp,NormalizeLib);

            % Perform sybolic calculation and solve for dX
            Eqn=LHS_Sym_Dum==ODE{i,j};
            digits(4)
            ODE_Guess=simplify(vpa(solve(Eqn,dz_Dum)));
            % Store the result of each valuation
            try
                ODEs(i,j)=ODE_Guess;
                func=matlabFunction(ODEs(i,j),'Vars',{z,uc});
                funcVal=func(Data_test',u_test')';
                Score(i,j)=norm(dData_test_dum-funcVal)/norm(dData_test(:,iter));
            catch
                ODEs(i,j)=0;
                Score(i,j)=NaN;
            end
        end
    end

    % Calculate the minimum score
    [minVal,minIndex]=min(Score,[],2);
    [minVal2,minIndex2]=min(minVal);

    % Store the best ODE approximation
    ODE_Best(iter,1)=ODEs(minIndex2,minIndex(minIndex2));

    % Print the result
    fprintf('\n \tThe SINDy-PI discovered Best ODE for %i equation is:\n',iter)
    digits(4)
    fprintf(strcat(char(dz(iter)),'=',char(simplify(ODE_Best(iter,1))),'\n'));

    % Save this result
    Xi_Final{iter,1}=Xi;
    Score_Final{iter,1}=Score;

    % Plot the score
    figure(iter)
    hold on
    for k=1:length(LHS_Sym)
        plot(lam,Score(k,:),'color',[1 iter*0.2 iter*0.25],'linewidth',2.5)
        scatter(lam,Score(k,:),100,'MarkerFaceColor',[1 iter*0.2 iter*0.25],'MarkerEdgeColor',[0 0 0],'linewidth',2.5)
        set(gca,'XScale','log')
    end

end

%% Now generate the ODE function file and test the accuracy of the
% identified system
Noise=0;

% Now generate this best guess ODE
Generate_ODE_RHS(ODE_Best(:,1),n_state,n_control);

% Simulate the system with the SINDy-PI identified ODE
%state0_test=[0.5;0.1;0.1;-0.1];
state0_test=[pi;0;0;0];
u_test=-0.5+0.2*sin((tspan_test'))+0.3*sin(2*(tspan_test'));
LHS_Sym=0;
[dData_test,Data_test]=Get_Sim_Data(@(t,y,u)SinglePendulum_ODE(t,y,u,M,m,L,g),state0_test,u_test,tspan_test,Noise,Control,Shuffle);
[d_Data_Es,Data_Es]=Get_Sim_Data(@(t,y,u)Sindy_ODE_RHS(t,y,u),state0_test,u_test,tspan_test,Noise,Control,Shuffle);

%% Print the Result
disp_best=1;
if disp_best==1
    fprintf('The SINDy-PI discovered Best ODE is:\n')
    digits(4)
    for i=1:n_state
        fprintf(strcat(char(dz(i)),'=',char(simplify(ODE_Best(i,1))),'\n'));
    end
end
%% Save the result
File_Name=strcat('Results/Noise_Level_',num2str(noise),'.mat');
save(File_Name,'Xi_Final','Score_Final','ODE_Best','state0_test',...
    'u_test','dData_test','Data_test','d_Data_Es','Data_Es','tspan_test')

%% Plot the simulation data
close all
% Create the new directory to save the plot
[fld_status, fld_msg, fld_msgID]=mkdir('Figures');
%
figure(1)
plot(tspan_test,Data_test(:,1),'linewidth',4.5,'Color','black')
hold on
plot(tspan_test,Data_Es(:,1),'linewidth',4.5,'linestyle','--','color','blue')
% legend('Actual Dynamics','Best Model')
% title('Validation','FontSize',24)
% xlabel('Time $(t)$','FontSize', 24)
% ylabel('$\theta(t)$','FontSize', 24)
set(gca,'FontSize',34);
grid on
h = gca;
set(gca,'xticklabel',[])
set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
box('on')
%print('-depsc2', '-loose', 'Figures/SinglePendulum_Thetat1.eps');

%
figure(2)
plot(tspan_test,Data_test(:,2),'linewidth',4.5,'Color','black')
hold on
plot(tspan_test,Data_Es(:,2),'linewidth',4.5,'linestyle','--','color','blue')
% legend('Actual Dynamics','Best Model')
% title('Validation','FontSize',18)
% xlabel('Time $(t)$','FontSize', 18)
% ylabel('$x(t)$','FontSize', 18)
set(gca,'FontSize',34);
grid on
%h = gca; h.XAxis.Visible = 'off';
set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
box('on')
print('-depsc2', '-loose', 'Figures/SinglePendulum_x.eps');

%%
figure(3)
plot(tspan_test,Data_test(:,3),'linewidth',2,'Color','green')
hold on
plot(tspan_test,Data_Es(:,3),'linewidth',2,'linestyle','--','color','blue')
legend('Actual Dynamics','Best Model')
title('Validation','FontSize',18)
xlabel('Time $(t)$','FontSize', 18)
ylabel('$\dot{\theta}(t)$','FontSize', 18)
set(gca,'FontSize',18);
grid on

set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
print('-depsc2', '-loose', 'Figures/SinglePendulum_dThetat.eps');

%%
figure(4)
plot(tspan_test,Data_test(:,4),'linewidth',2,'Color','green')
hold on
plot(tspan_test,Data_Es(:,4),'linewidth',2,'linestyle','--','color','blue')
legend('Actual Dynamics','Best Model')
title('Validation','FontSize',18)
xlabel('Time $(t)$','FontSize', 18)
ylabel('$\dot{x}(t)$','FontSize', 18)
set(gca,'FontSize',18);
grid on

set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
print('-depsc2', '-loose', 'Figures/SinglePendulum_dx.eps');

%
figure(5)
plot(Data_test(:,1),Data_test(:,3),'linewidth',2,'Color','green')
hold on
plot(Data_Es(:,1),Data_Es(:,3),'linewidth',2,'linestyle','--','color','blue')
legend('Actual Dynamics','Best Model')
title('Validation','FontSize',18)
xlabel('$\theta(t)$','FontSize', 18)
ylabel('$\dot{\theta}(t)$','FontSize', 18)
set(gca,'FontSize',18);
grid on

set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
print('-depsc2', '-loose', 'Figures/SinglePendulum_theta_vs_dtheta.eps');

%
figure(6)
plot(Data_test(:,2),Data_test(:,4),'linewidth',2,'Color','green')
hold on
plot(Data_Es(:,2),Data_Es(:,4),'linewidth',2,'linestyle','--','color','blue')
legend('Actual Dynamics','Best Model')
title('Validation','FontSize',18)
xlabel('$x(t)$','FontSize', 18)
ylabel('$\dot{x}(t)$','FontSize', 18)
set(gca,'FontSize',18);
grid on

set(gcf,'Position',[100 100 600 400]);
set(gcf,'PaperPositionMode','auto');
print('-depsc2', '-loose', 'Figures/SinglePendulum_x_vs_dx.eps');


function res = vg_method_train(X, Y, args)

% Variational Garrote: performs linear regression with L0-norm penalty (Spike and Slab model)
%   See file test_vg.m for an example of use
%
% required parameters (n input dimension, p samples)
%   X     : n x p (training set, input)
%   Y     : 1 x s (training set, output)
%
% args: optional parameters (default)
%   method      : method for optimization 'dual' or 'regression' for fixed gamma ('dual')
%   maxiter     : maximum number of iterations for optimization for fixed gamma (1e4)
%   max_sum_m   : increases gamma values until sum(m)=max_sum_m  (n/2)
%   beta_max    : increases gamma values until beta=beta_max (1e3)
%   n_gamma     : number of gamma values to scan (50)
%   dmmin       : convergence threshold for mean field error (1e-12)
%   valset      : part of the training set used for validation (0.1*p)

%----------------
% REQUIRED PARAMS

n = size(X,1);

%----------------
% OPTIONAL PARAMS

% method for optimization {dual or regression} for fixed gamma
if ~isfield(args, 'method') method='dual'; else method=args.method; end

% maximum number of iterations for optimization for fixed gamma
if ~isfield(args, 'maxiter') maxiter=1e4; else maxiter=args.maxiter; end

% increases gamma values until sum(m)=max_sum_m
if ~isfield(args, 'max_sum_m') max_sum_m=n/2; else max_sum_m=args.max_sum_m; end

% increases gamma values until beta=beta_max
if ~isfield(args, 'beta_max') beta_max=1e3; else beta_max=args.beta_max; end

% number of gamma values to scan
if ~isfield(args, 'n_gamma') n_gamma=50; else n_gamma=args.n_gamma; end

% convergence threshold for mean field error
if ~isfield(args, 'dmmin') dmmin=1e-12; else dmmin=args.dmmin; end

% part of the training set used for validation (default 0.1*p)
if ~isfield(args, 'valset') valset=ceil(.25*n); else valset=args.valset; end

% randomly split training and validation datasets
dataok = false;
nits = 1;
p = size(X,2)-valset;
pv = valset;
while ~dataok && nits < 10
    it = randperm(size(X,2));
    xv = X(:,it(1:valset));
    yv = Y(:,it(1:valset));
    x = X(:,it(valset+1:end));
    y = Y(:,it(valset+1:end));

    % normalize training
    x=x-mean(x,2)*ones(1,p);
    dx=sqrt(1/p*sum(x.^2,2));
    x=x./(dx*ones(1,p));
    y=y-mean(y);

    % normalize validation
    xv=xv-mean(xv,2)*ones(1,pv);
    dxv=sqrt(1/pv*sum(xv.^2,2));
    xv=xv./(dxv*ones(1,pv));
    yv=yv-mean(yv);

    dataok = ~any(isnan(x(:))) && ~any(isnan(xv(:)));
    nits=nits+1;
end
if nits==10
    error('VG Error: Increase training set size');
end

%----------------
% compute garrote solution for range of gammas
% first from gamma_min to gamma_max and then in
% a second pass from gamma_max to gamma_min.

% C is input data covariance matrix.
if strcmp(method, 'regression')
    if n<=1500,
        C=x*x'/p;
    end;
end

% b is input output covariance
b=x*y'/p;

% sigma is output variance
sigmay=y*y'/p;

% set gamma range (min, max and step size)
delta=1e-8;
[b2sort,isort]=sort(b.^2,'descend');
bsort=b(isort);
gamma_min=log(delta*sigmay/p/max(abs(b)));
eps_gamma=0.001;
gamma_max=eps_gamma*gamma_min;
gamma_all =linspace(gamma_min,gamma_max,n_gamma);

% initial step size of mean field update
eta0=1; %e-2;
% initial step size for change in w in dual.m
eta_w0=0.02;

% input data variance
chi_ii=1/p*sum(x.^2,2);
if sum(abs(chi_ii-1)>1e-10),
    fprintf('input design matrix is not normalized\n');
    pause
end;

lg=n_gamma;
kl_all=inf(lg,2);
v_all=inf(lg,2,n);
m_all=inf(lg,2,n);
beta_all=inf(lg,2);
v_mf_all=inf(lg,n);
m_mf_all=inf(lg,n);
iter_all=inf(lg,2);
beta_mf_all=inf(1,lg);
error_mf_all=inf(1,lg);
errorv_mf_all=inf(1,lg);
m=zeros(1,n);

% the estimated inverse noise variance beta is initialized as the
% output variance
beta=1/sigmay;
i=0;

% for gamma is gamma_min to gamma_max, or when some criteria are
% satisfied
while (beta<beta_max)&&(i<n_gamma)&&(sum(m)<max_sum_m),
	i=i+1;
	gamma=gamma_all(i);
    eval(method);
	v_all(i,1,:)=v;
	m_all(i,1,:)=m;
	beta_all(i,1)=beta;
	iter_all(i,1)=iter;
	kl_all(i,1)=kl1;
	fprintf('gamma = %f beta = %f sum(m) = %f iter = %d kl = %f\n',gamma,beta,sum(m),iter,kl1);
end;

if beta>=beta_max
 fprintf('-----------------------------------------------------------\n');
 fprintf('beta > beta_max (%.3f > %.3f)\n', beta, beta_max);
 if i>1
    m = squeeze(m_all(i-1,1,:))';
 end
end
if sum(m)>=max_sum_m
 fprintf('-----------------------------------------------------------\n');
 fprintf('sum(m) > max_sum_m (%.3f > %.3f)\n', sum(m), max_sum_m);
end

% for gamma is current gamma decreasing to gamma_min
imax=i-1;
for i=imax:-1:1,
	gamma=gamma_all(i);
    eval(method);
	v_all(i,2,:)=v;
	m_all(i,2,:)=m;
	beta_all(i,2)=beta;
	iter_all(i,2)=iter;
	kl_all(i,2)=kl1;
	fprintf('gamma = %f beta = %f sum(m) = %f iter = %d kl = %f\n',gamma,beta,sum(m),iter,kl1);
end;

% select for each gamma from these two solutions the one with lowest KL
[klmin, imin]=min(kl_all,[],2);
for i=1:imax,
	v_mf_all(i,:)=squeeze(v_all(i,imin(i),:))';
	m_mf_all(i,:)=squeeze(m_all(i,imin(i),:))';
	beta_mf_all(i)=beta_all(i,imin(i));
	error_mf_all(i)=1/p*sum((y-v_mf_all(i,:)*x).^2,2);
	errorv_mf_all(i)=1/pv*sum((yv-v_mf_all(i,:)*xv).^2,2);
end;

% select the gamma that optimizes the validation error (errorv_mf_all)
[minerrorv i]=min(errorv_mf_all(1:imax));

res.gamma_mf=gamma_all(i);
res.v_mf=v_mf_all(i,:);
res.m_mf=m_mf_all(i,:);
res.n_mf1=sum(res.m_mf>0.5);

res.error_mf=error_mf_all(i);
res.errorv_mf=errorv_mf_all(i);
res.beta_mf=beta_mf_all(i);

if (res.beta_mf==beta_max)
	fprintf('beta_max too small: beta_mf %6.4f, beta_max %6.4f\n', beta_mf(iruns),beta_max);
	pause
end;
if (res.gamma_mf==gamma_min)
	fprintf('gamma at minimum range boundary\n');
end;
if (res.gamma_mf==gamma_max)
	fprintf('gamma at maxium range boundary\n');
end;


function [model] = USRtrain(fea, options)
% USRtrain: Training Unsupervised Spectral Regression Model
%
%       [model] = USRtrain(fea, options)
%
%             Input:
%
%               fea     - data matrix. Each row is a data point.
%           options     - Struct value in Matlab. The fields in options
%                         that can be set:
%
%                      W       -  Affinity matrix. You can either call
%                                 "constructW" to construct the W, or
%                                 construct it by yourself.
%                                 If W is not provided, USRtrain will
%                                 build a k-NN graph with Heat kernel
%                                 weight, where k is a prameter.
%
%                      k       -  The parameter for k-NN graph (Default is 5)
%                                 If W is provided, this k will be ignored.
%
%                  ReducedDim  -  The number of reduced dimensions.
%                                 Default ReducedDim = 30.
%
%                  ReguType    -  'Ridge': L2-norm regularizer (default)
%                                 'Lasso': L1-norm regularizer
%                  ReguAlpha   -  regularization paramter for L2-norm regularizer
%                                 Default 0.1
%                  ReguGamma   -  regularization paramter for L1-norm regularizer
%                                 Default 0.05
%                  LASSOway    -  'LARs': use LARs to solve the LASSO
%                                 problem. You need to specify the
%                                 cardinality requirement in LassoCardi.
%                                 'SLEP': use SLEP to solve the LASSO
%                                 problem. Please see http://www.public.asu.edu/~jye02/Software/SLEP/
%                                  for details on SLEP. (The Default)
%
%                 bCenter = 0 | 1  whether to center the data. (In some
%                                  cases, e.g., text categorization, The
%                                  data is very spase, centering the data
%                                  will destroy the sparsity and consume
%                                  too much memory. In this case, bCenter
%                                  should be set to 0)
%                                   Default: 1
%
%
%             Output:
%               model   -  used for USRtest.m
%
%
%    Examples:
%
%
%
% See also SR, SR_caller
%
%Reference:
%
%   [1] Deng Cai, Xiaofei He, Wei Vivian Zhang, Jiawei Han, "Regularized
%   Locality Preserving Indexing via Spectral Regression", Proc. 2007 ACM
%   Int. Conf. on Information and Knowledge Management (CIKM'07), Lisboa,
%   Portugal, Nov. 2007.
%
%   [2] Deng Cai, "Spectral Regression: A Regression Framework for
%   Efficient Regularized Subspace Learning", PhD Thesis, Department of
%   Computer Science, UIUC, 2009.
%
%   version 3.0 --Jan/2012
%   version 2.0 --December/2011
%   version 1.0 --May/2006
%
%   Written by Deng Cai (dengcai AT gmail.com)
%

if ~exist('options','var')
    options = [];
end

bCenter = 1;
if isfield(options,'bCenter')
    bCenter = options.bCenter;
end

k = 5;
if isfield(options,'k')
    k = options.k;
end

ReducedDim = 30;
if isfield(options,'ReducedDim')
    ReducedDim = options.ReducedDim;
end

if ~isfield(options,'ReguType')
    options.ReguType = 'Ridge';
end

LARs = false;
switch lower(options.ReguType)
    case {lower('Ridge')}
        if ~isfield(options,'ReguAlpha')
            options.ReguAlpha = 0.1;
        end
    case {lower('Lasso')}
        if isfield(options,'ReguAlpha') && options.ReguAlpha > 0
            options.RidgeAlpha = options.ReguAlpha;
            options.ReguType = 'RidgeLasso';
        end
        if isfield(options,'ReguGamma')
            options.ReguAlpha = options.ReguGamma;
        else
            options.ReguAlpha = 0.05;
        end
        if ~isfield(options,'LASSOway')
            options.LASSOway = 'SLEP';
        end

        if strcmpi(options.LASSOway,'LARs')
            LARs = true;
            if ~isfield(options,'LassoCardi')
                options.LassoCardi = 10:10:50;
            end
        end
    otherwise
        error('ReguType does not exist!');
end

nSmp=size(fea,1);
% Graph construction
if isfield(options,'W')
    W = options.W;
else
    Woptions.k = k;
    if nSmp > 3000
        tmpD = EuDist2(fea(randsample(nSmp,3000),:));
    else
        tmpD = EuDist2(fea);
    end
    Woptions.t = mean(mean(tmpD));
    W = constructW(fea,Woptions);
end

% Respnse generation
Y = Eigenmap(W,ReducedDim);

% Projection learning
if bCenter
    sampleMean = mean(fea);
    fea = (fea - repmat(sampleMean,nSmp,1));
end

[model.projection, LassoCardi] = SR(options, Y, fea);

model.LARs = LARs;
model.LassoCardi = LassoCardi;

model.TYPE = 'USR';
model.options = options;


function y = aprod( mode, m, n, x, iw, rw )
% This is the simplest example for testing  LSQR.
% :math:`A = rw`.
% If `mode = 1`, `aprod` computes :math:`y = A x`.
% Ff `mode = 2`, `aprod` computes :math:`y = A^T x`.
% for some matrix  `A`.

if mode == 1,
   y = rw*x;
else
   y = rw'*x;
end


function X = mat(x,n)
% Y = MAT(x,n)   or   Y = MAT(x)     (the 2nd argument is optional)
%   Given a vector of length n^2, this produces the n x n matrix
%   Y such that x = vec(Y).  In other words, x contains the columns of the
%   matrix Y, stacked below each other.
%
% See also vec.

% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA
%

if nargin < 2
    n = floor(sqrt(length(x)));
    if (n*n) ~= length(x)
        error('Argument X has to be a square matrix')
    end
end
X = reshape(x,n,n);


function c = tapas_ehgf_binary_pu_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the Hierarchical Gaussian Filter (HGF)
% for binary inputs in the *presence* of perceptual uncertainty.
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
% The binary HGF model has since been augmented with a positive factor kappa1 which
% scales the second level with respect to the first, i.e., the relation between the
% first and second level is
%
% p(x1=1|x2) = s(kappa1*x2), where s(.) is the logistic sigmoid.
%
% By default, kappa1 is fixed to 1, leading exactly to the model introduced in
% Mathys et al. (2011).
%
% This file refers to BINARY inputs (Eqs 1-3 in Mathys et al., (2011));
% for continuous inputs, refer to tapas_hgf_config.
%
% This file refers to UNCERTAIN inputs (Eqs 45-47 in Mathys et al., (2011));
% for inputs without uncertainty, refer to tapas_hgf_binary_config.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_hgf_binary_plotTraj(est)
%
% where est is the stucture returned by tapas_fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mu_0       row vector of initial values of mu (in ascending order of levels)
%         est.p_prc.sa_0       row vector of initial values of sigma (in ascending order of levels)
%         est.p_prc.rho        row vector of rhos (representing drift; in ascending order of levels)
%         est.p_prc.ka         row vector of kappas (in ascending order of levels)
%         est.p_prc.om         row vector of omegas (in ascending order of levels)
%         est.p_prc.al         scalar alpha (perceptual uncertainty)
%         est.p_prc.eta0       scalar eta0 (mean of first input category)
%         est.p_prc.eta1       scalar eta1 (mean of second input category)
%
% Note that the first entry in all of the row vectors will be NaN because, at the first level,
% these parameters are either determined by the second level (mu_0 and sa_0) or undefined (rho,
% kappa, and omega).
%
%         est.traj.mu          mu (rows: trials, columns: levels)
%         est.traj.sa          sigma (rows: trials, columns: levels)
%         est.traj.muhat       prediction of mu (rows: trials, columns: levels)
%         est.traj.sahat       precisions of predictions (rows: trials, columns: levels)
%         est.traj.v           inferred variance of random walk (rows: trials, columns: levels)
%         est.traj.w           weighting factors (rows: trials, columns: levels)
%         est.traj.da          volatility prediction errors  (rows: trials, columns: levels)
%         est.traj.ud          updates with respect to prediction  (rows: trials, columns: levels)
%         est.traj.psi         precision weights on prediction errors  (rows: trials, columns: levels)
%         est.traj.epsi        precision-weighted prediction errors  (rows: trials, columns: levels)
%         est.traj.wt          full weights on prediction errors (at the first level,
%                                  this is the learning rate) (rows: trials, columns: levels)
%
% Note that in the absence of sensory uncertainty (which is the assumption here), the first
% column of mu, corresponding to the first level, will be equal to the inputs. Likewise, the
% first column of sa will be 0 always.
%
% Tips:
% - When analyzing a new dataset, take your inputs u and use
%
%   >> est = tapas_fitModel([], u, 'tapas_hgf_binary_pu_config', 'tapas_bayes_optimal_binary_config');
%
%   to determine the Bayes optimal perceptual parameters (given your current priors as defined in
%   this file here, so choose them wide and loose to let the inputs influence the result). You can
%   then use the optimal parameters as your new prior means for the perceptual parameters.
%
% - When analyzing a new dataset, take your inputs u and use
%
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
% - If the log-model evidence cannot be calculated because the Hessian poses problems, look at
%   est.optim.H and fix the parameters that lead to NaNs.
%
% - Your guide to all these adjustments is the log-model evidence (LME). Whenever the LME increases
%   by at least 3 across datasets, the adjustment was a good idea and can be justified by just this:
%   the LME increased, so you had a better model.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2017 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'ehgf_binary_pu';

% Number of levels (minimum: 3)
c.n_levels = 3;

% Input intervals
% If input intervals are irregular, the last column of the input
% matrix u has to contain the interval between inputs k-1 and k
% in the k-th row, and this flag has to be set to true
c.irregular_intervals = false;

% Sufficient statistics of Gaussian parameter priors

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first two levels, this is usually best
% kept fixed to 1 (determines origin on x_i-scale). The
% first level is NaN because it is determined by the second,
% and the second implies neutrality between outcomes when it
% is centered at 0.
c.mu_0mu = [NaN, 0, 1];
c.mu_0sa = [NaN, 0, 0];

c.logsa_0mu = [NaN,   log(0.1), log(1)];
c.logsa_0sa = [NaN,          0,      0];

% Rhos
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
% Fix this to zero to turn off drift.
c.rhomu = [NaN, 0, 0];
c.rhosa = [NaN, 0, 0];

% Kappas
% Format: row vector of length n_levels-1.
% Fixing log(kappa1) to log(1) leads to the original HGF model.
% Higher log(kappas) should be fixed (preferably to log(1)) if the
% observation model does not use mu_i+1 (kappa then determines the
% scaling of x_i+1).
c.logkamu = [log(1), log(1)];
c.logkasa = [     0,      0];

% Omegas
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
c.ommu = [NaN,  -3,  -6];
c.omsa = [NaN, 4^2, 4^2];

% Alpha
% Format: scalar.
c.logalmu = log(0.5);
c.logalsa = 1;

% Eta0
% Format: scalar.
c.eta0mu = 0;
c.eta0sa = 0;

% Eta1
% Format: scalar.
c.eta1mu = 1;
c.eta1sa = 0;

% Gather prior settings in vectors
c.priormus = [
    c.mu_0mu,...
    c.logsa_0mu,...
    c.rhomu,...
    c.logkamu,...
    c.ommu,...
    c.logalmu,...
    c.eta0mu,...
    c.eta1mu,...
         ];

c.priorsas = [
    c.mu_0sa,...
    c.logsa_0sa,...
    c.rhosa,...
    c.logkasa,...
    c.omsa,...
    c.logalsa,...
    c.eta0sa,...
    c.eta1sa,...
         ];

% Check whether we have the right number of priors
expectedLength = 3*c.n_levels+2*(c.n_levels-1)+4;
if length([c.priormus, c.priorsas]) ~= 2*expectedLength;
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_ehgf_binary_pu;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_ehgf_binary_pu_transp;

return;


function labels = grLabel(nodes, edges)
%GRLABEL Associate a label to each connected component of the graph
%
%   LABELS = grLabel(NODES, EDGES)
%   Returns an array with as many rows as the array NODES, containing index
%   number of each connected component of the graph. If the graph is
%   totally connected, returns an array of 1.
%
%   Example
%       nodes = rand(6, 2);
%       edges = [1 2;1 3;4 6];
%       labels = grLabel(nodes, edges);
%   labels =
%       1
%       1
%       1
%       2
%       3
%       2
%
%   See also
%   getNeighborNodes
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2007-08-14,    using Matlab 7.4.0.287 (R2007a)
% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

% init
Nn = size(nodes, 1);
labels = (1:Nn)';

% iteration until stability
modif = true;
while modif
    modif = false;

    % compute the minimum label in the neighborhood of each node
    for i = 1:Nn
        neigh = grAdjacentNodes(edges, i);
        neighLabels = labels([i;neigh]);

        % check for a modification
        if length(unique(neighLabels)) > 1
            modif = true;
        end

        % put new labels
        labels(ismember(labels, neighLabels)) = min(neighLabels);
    end
end

% renumbering to have fewer labels
labels2 = unique(labels);
for i = 1:length(labels2)
    labels(labels == labels2(i)) = i;
end


function [ cx, cy ] = cswap ( n, cx, incx, cy, incy )

%*****************************************************************************80
%
%% CSWAP interchanges two complex vectors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vectors.
%
%    Input, complex CX(*), one of the vectors to swap.
%
%    Input, integer INCX, the increment between successive entries of CX.
%
%    Input, complex CY(*), one of the vectors to swap.
%
%    Input, integer INCY, the increment between successive elements of CY.
%
%    Output, complex CX(*), the input vector CX, with some elements swapped.
%
%    Input, integer INCX, the increment between successive entries of CX.
%
%    Output, complex CY(*), the input vector CY, with some elements swapped.
%
  temp                    = cx(1:incx:1+(n-1)*incx);
  cx(1:incx:1+(n-1)*incx) = cy(1:incy:1+(n-1)*incy);
  cy(1:incy:1+(n-1)*incy) = temp;

  return
end


% KM_DEMO_AKCCA Demonstration of Alternating Kernel Canonical Correlation
% Analysis algorithm for blind equalization of single-input multiple-output
% Wiener systems.
%
% Author: Steven Van Vaerenbergh (steven *at* gtas.dicom.unican.es), 2011.
%
% The algorithm in this file is based on the following publication:
% S. Van Vaerenbergh, J. Via and I. Santamaria, "Blind Identification of
% SIMO Wiener Systems based on Kernel Canonical Correlation Analysis",
% accepted for publication in IEEE Transactions on Signal Processing, 2013.
%
% This file is part of the Kernel Methods Toolbox for MATLAB.
% https://github.com/steven2358/kmbox

close all
clear
rs = 1; % seed for random generator
rng('default')
rng(rs)

fprintf('\nAlternating kernel CCA for blind equalization of ')
fprintf('SIMO Wiener systems.\n');

%% SETUP PARAMETERS
pars_setup.model_num = 30244; % source model, see generate_data.m
pars_setup.model_L = 5; % channel length
pars_setup.data_type = 'gaussian'; % bits, gaussian
pars_setup.data_N = 256; % number of data points
pars_setup.data_SNR = 20; % signal-to-noise ratio
pars_setup.zf_k = 15; % number of observations to estimate zero-forcing equalizers
pars_setup.p = 4; % number of branches in SIMO Wiener system
pars_setup.verbose = false;

%% ALT-KCCA PARAMETERS
pars = pars_setup; % copy some setup parameters
pars.it_max = 100; % maximum number of iterations
pars.it_stop = 1E-10; % stop iteration if change in cost is smaller than this
pars.kernel.type = 'gauss'; % kernel type
pars.kernel.par = @(x) km_silverman(x); % kernel parameter, either a scalar or a function to determine it
pars.m = 1E-8; % number of KPCA autovectors / precision of ICD, or fraction of discarded signal energy
pars.reg = 1E-5; % regularization
pars.decomp = 'ICD'; % ICD or KPCA
pars.identical_nonlin = 0;	% boolean indicating common nonlinearity for each channel

%% PROGRAM
tic

N = pars_setup.data_N; p = pars_setup.p; SNR = pars_setup.data_SNR;
% GENERATE DATA
switch pars_setup.data_type,
    case 'gaussian'
        s = randn(N,1); s = s-mean(s); % source signal
    case 'bits'
        s = 2*round(rand(N,1))-1; % source signal
end
y = cell(p,1); x = cell(p,1); sigpow = 0;
f = @(x) tanh(0.8*x)+0.1*x; % nonlinearity
B1 = [0.6172   -0.8601    2.1383    0.4269   -1.3153
    0.6247    0.1532    0.9686   -0.5820   -0.4584
    0.3373   -0.1888   -1.4263    0.8060   -0.1740
    -0.0349   -0.6264   -0.2486    1.1975    1.2195
    -3.2957    0.9985   -0.3768    0.6139   -1.2011]/1.5; % linear filter
B = cell(p,1);
for i=1:p, % simulate Wiener system
    B{i} = B1(:,i);
    y{i} = filter(B{i},1,s);
    x{i} = f(y{i});
    sigpow = sigpow + x{i}'*x{i}/p/N;
end
noisepow = 10^(-SNR/10)*sigpow;
for i=1:p, x{i} = x{i} + sqrt(noisepow)*randn(N,1); end % add noise
data.s = s; data.B = B; data.y = y; data.x = x;

%% AKCCA algorithm
[vars,eval] = km_akcca(pars,data);

toc
%% OUTPUT

% calculate MSE or BER
switch pars_setup.data_type
    case 'gaussian'
        % scale signals and compare
        s1 = s(vars.ind_s); s2 = eval.s_est;
        s1_norm = s1/norm(s1); sc = s1_norm'*s2/(s2'*s2);
        err_vec = s1_norm - sc*s2;

        MSE = err_vec'*err_vec;
        sc = sc*norm(s1);

        fprintf('MSE = %.2f dB\n',10*log10(MSE));
    case 'bits'
        sc = 1;
        fprintf('BER = %.4f\n',eval.finalresult);
end

N = pars_setup.data_N;

% draw estimates of nonlinearities
figure
for i=1:p
    subplot(1,p,i); hold all
    xi = vars.x{i};
    y_est = vars.y_est{i};
    sci = y{i}'*y_est/(y_est'*y_est);

    plot(xi,data.y{i},'.b')
    plot(xi,sci*y_est,'.r');
    legend(sprintf('x%d vs true, unknown y%d',i),sprintf('x%d vs estimated y%d',i))
    xlabel('x')
end

% draw estimates of linear filters
figure
for i=1:p
    subplot(1,p,i); hold all
    stem(B{i},'ob','LineWidth',2,'MarkerSize',8);
    stem(vars.h{i}*norm(B{i})/norm(vars.h{i}),'xr','LineWidth',2,...
        'LineStyle','none','MarkerSize',10);

    xlabel('l');ylabel(sprintf('h_%d',i));
    legend({'real channel','estimated channel'})
end

% draw equalization result
figure; hold all
plot(s(vars.ind_s))
plot(sc*eval.s_est);
legend('source signal','recovered signal')


function [y,L] = midwt(x,h,L);
%    [x,L] = midwt(y,h,L);
%
%    Function computes the inverse discrete wavelet transform x for a 1D or
%    2D input signal y using the scaling filter h.
%
%    Input:
%	y : finite length 1D or 2D input signal (implicitly periodized)
%           (see function mdwt to find the structure of y)
%       h : scaling filter
%       L : number of levels. In the case of a 1D signal, length(x) must be
%           divisible by 2^L; in the case of a 2D signal, the row and the
%           column dimension must be divisible by 2^L.  If no argument is
%           specified, a full inverse DWT is returned for maximal possible
%           L.
%
%    Output:
%       x : periodic reconstructed signal
%       L : number of decomposition levels
%
%    1D Example:
%       xin = makesig('LinChirp',8);
%       h = daubcqf(4,'min');
%       L = 1;
%       [y,L] = mdwt(xin,h,L);
%       [x,L] = midwt(y,h,L)
%
%    1D Example's  output:
%
%       x = 0.0491 0.1951 0.4276 0.7071 0.9415 0.9808 0.6716 0.0000
%       L = 1
%
%    See also: mdwt, mrdwt, mirdwt
%

%
%
%File Name: midwt.m
%Last Modification Date: 08/07/95	15:13:52
%Current Version: midwt.m	2.4
%File Creation Date: Wed Oct 19 10:51:58 1994
%Author: Markus Lang  <lang@jazz.rice.edu>
%
%Copyright (c) 2000 RICE UNIVERSITY. All rights reserved.
%Created by Markus Lang, Department of ECE, Rice University.
%
%This software is distributed and licensed to you on a non-exclusive
%basis, free-of-charge. Redistribution and use in source and binary forms,
%with or without modification, are permitted provided that the following
%conditions are met:
%
%1. Redistribution of source code must retain the above copyright notice,
%   this list of conditions and the following disclaimer.
%2. Redistribution in binary form must reproduce the above copyright notice,
%   this list of conditions and the following disclaimer in the
%   documentation and/or other materials provided with the distribution.
%3. All advertising materials mentioning features or use of this software
%   must display the following acknowledgment: This product includes
%   software developed by Rice University, Houston, Texas and its contributors.
%4. Neither the name of the University nor the names of its contributors
%   may be used to endorse or promote products derived from this software
%   without specific prior written permission.
%
%THIS SOFTWARE IS PROVIDED BY WILLIAM MARSH RICE UNIVERSITY, HOUSTON, TEXAS,
%AND CONTRIBUTORS AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
%BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
%FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL RICE UNIVERSITY
%OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
%EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
%PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
%OR BUSINESS INTERRUPTIONS) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
%WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
%OTHERWISE), PRODUCT LIABILITY, OR OTHERWISE ARISING IN ANY WAY OUT OF THE
%USE OF THIS SOFTWARE,  EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
%For information on commercial licenses, contact Rice University's Office of
%Technology Transfer at techtran@rice.edu or (713) 348-6173
%
%Change History:
%
%Modification #1
%Mon Aug  7 11:52:33 CDT 1995
%Rebecca Hindman <hindman@ece.rice.edu>
%Added L to function line so that it can be displayed as an output
%
%Thu Mar  2 13:07:11 CDT 2000
%Ramesh Neelamani<neelsh@ece.rice.edu>
%Revamped the help file
%


function [ x, y ] = p06_dat ( data_num )

%*****************************************************************************80
%
%% P06_DAT returns the data vector for problem 6.
%
%  Discussion:
%
%    The X data is equally spaced.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DATA_NUM, the number of data points.
%
%    Output, real X(DATA_NUM,1), the abscissa data.
%
%    Output, real Y(DATA_NUM,1), the ordinate data.
%
  x = zeros ( data_num, 1 );
  y = zeros ( data_num, 1 );

  num_int = 5;

  n = 1;
  x(n,1) = 0.0;
  y(n,1) = 0.0;

  for i = 1 : num_int

    for j = 1 : i
      n = n + 1;
      x(n,1) = ( i - 1 ) + 0.5 * j /i;
      y(n,1) = j / i;
    end

    for j = 1 : i
      n = n + 1;
      x(n,1) = i - 1 + 0.5 + 0.5 * j / i;
      y(n,1) = 1.0 - j / i;
    end

  end

  return
end


function [gps_week, gps_sow, gps_dow] = date2gps(date)

% SYNTAX:
%   [gps_week, gps_sow, gps_dow] = date2gps(date);
%
% INPUT:
%   date = date [year, month, day, hour, min, sec]
%
% OUTPUT:
%   gps_week = GPS week
%   gps_sow  = GPS seconds of week
%   gps_dow  = GPS day of week
%
% DESCRIPTION:
%   Conversion from calendar date to GPS time.

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

gps_start_datenum = 723186; %This is datenum([1980,1,6,0,0,0])

%number of days since the beginning of GPS time
%deltat   = (datenum([date(:,1), date(:,2), date(:,3)]) - gps_start_datenum);
% hack: datenummmx is faster cause it does not check argins
deltat   = (datenummx(date(:,1:3)) - gps_start_datenum);

gps_week = floor(deltat/7);            %GPS week
gps_dow  = floor(deltat - gps_week*7); %GPS day of week
gps_sow  = (deltat - gps_week*7)*86400;
gps_sow = gps_sow + date(:,4)*3600 + date(:,5)*60 + date(:,6); %GPS seconds of week

% %alternative way, using the Julian day
% jd = date2jd(date);
% [gps_week, gps_dow, gps_sow] = jd2gps(jd);


function [P,X] = vgg_PX_from_6pts_3img(x1,x2,x3)
% vgg_PX_from_6pts_3img  Computes camera matrices and world points
% from 6 points across 3 images.
%
%   [P,X] = vgg_PX_from_6pts_3img(x), where
%      x ... double(3,6,3) or cell{3} of double(3,6), 6 homogeneous points in 3 images
%      P ... double(3,4,3), P(:,:,k) is k-th camera matrix
%      X ... double(4,6), homogeneous world points
%   There are 0 to 3 solutions for (P,X). Solutions are pruned by requirement that
%   scalars s in all projective equations s*x==P*X are positive.
%   In case of multiple solutions, P and X have one dimension
%   more such that P(:,:,:,n) and X(:,:,n) is the n-th solution.
%
%   Also the form [P,X] = vgg_PX_from_6pts_3img(x1,x2,x3) is accepted.

% Algorithm in Hartley-Zisserman, Alg 19.1 page 493 in 1st edition,
%                                 Alg 20.1 page 511 in 2nd edition
% Coded by werner@robots.ox.ac.uk, Nov 2002.

if nargin==3
  x = cat(3,x1,x2,x3);
else
  if iscell(x1)
    x = cat(3,x1{:});
  else
    x = x1;
  end
end
if any(size(x)~=[3 6 3])
  error('Wrong size of input points.');
end

for k = 1:3
  % Find homographies H_k mapping first 4 pts in each image to standard projective basis.
  % Now, x(:,1:4,k) = H(:,:,k)*[eye(3) [1;1;1]].
  H(:,:,k) = H_from_4x(x(:,3:6,k));

  % Form transformed points xs(:,1:2,k)
  xs(:,:,k) = inv(H(:,:,k))*x(:,:,k);
end

% Compute dual fundamental matrix
Fd = Fdual_from_x(xs(:,1:2,:));

% Retrieve (non-dual) cameras P and world points X from (each solution for) dual fund. matrix.
P = [];
X = [];
for i = 1:size(Fd,3)

  % Compute canonical Xi and Pi
  Xi = X_from_Fdual(Fd(:,:,i));
  Pi = P_from_Xx_canonical(Xi,xs);

  % return from canonical to original image bases
  for k = 1:3
    Pi(:,:,k) = H(:,:,k)*Pi(:,:,k);
  end

  % Compute signs of P and X, if possible. If impossible, Pi==Xi==[].
  [Pi,Xi] = vgg_signsPX_from_x(Pi,Xi,x);
  if isempty(Pi), continue, end
  %for k = 1:3, Pi(:,:,k)*Xi ./ x(:,:,k), end  % test code

  P = cat(4,P,Pi);
  X = cat(3,X,Xi);

end

return

%%%%%%%%%%%%%%%% auxiliary functions

% Solve for dual fundamental matrix.
function F = Fdual_from_x(x)

% Linear step. After that,
% for k=1:3 and i=1:2, it is x(:,1,k)'*F_i*x(:,2,k)==0.
A = [];
for k = 1:3
  x1 = x(1,1,k);  y1 = x(2,1,k);  z1 = x(3,1,k);
  x2 = x(1,2,k);  y2 = x(2,2,k);  z2 = x(3,2,k);
  A = [A; [x1*y2-z1*y2,...
           x1*z2-z1*y2,...
           y1*x2-z1*y2,...
           y1*z2-z1*y2,...
           z1*x2-z1*y2] ];
end
[u,s,v] = svd(A,0);
v1 = v(:,end-1);
v2 = v(:,end);
FF{1} = [0 v1(1:2)'; v1(3) 0 v1(4); v1(5) -sum(v1) 0];
FF{2} = [0 v2(1:2)'; v2(3) 0 v2(4); v2(5) -sum(v2) 0];

% Non-linear step. Find linear combination of F_i having zero determinant.
% Dual fund. matrix is now  F = a*FF{1} + (1-a)*FF{2}, for each element of a.
a = vgg_singF_from_FF(FF);
for i = 1:length(a)
  F(:,:,i) = a(i)*FF{1} + (1-a(i))*FF{2};
end
return

% Given dual fundamental matrix Fd, computes cameras P and world points X such that
% for each k, P(:,:,k)*X ~ x(:,:,k).
function X = X_from_Fdual(Fd)

% Retrieve second dual camera matrix Pd2.
% It is done by considering Fdual in form
%[0 b*(d-c) -c*(d-b)
% -a*(d-c) 0 c*(d-a)
% a*(d-b) -b*(d-a) 0].

Fd_aux = reshape( Fd([4 7 1 2 1 8 1 3 6]), [3 3] );
[u,s,v] = svd(Fd_aux,0);
ABC = v(:,3); % It is a:b:c = A:B:C
[u,s,v] = svd(Fd',0);
KLM = v(:,3); % It is K:L:M = ((d-a):(d-b):(d-c)

G = [0 -ABC(3) ABC(2) 0
     ABC(3) 0 -ABC(1) 0
     -ABC(2) ABC(1) 0 0
     KLM(2) -KLM(1) 0 KLM(1)-KLM(2)
     0 KLM(3) -KLM(2) KLM(2)-KLM(3)
     -KLM(3) 0 KLM(1) KLM(3)-KLM(1) ];
[u,s,v] = svd(G,0);
abcd = v(:,end);

% The test code:
%Pd1 = [eye(3) [1;1;1]];
%Pd2 = [diag(abcd(1:3)) [1;1;1]*abcd(4)];
%vgg_F_from_P(Pd1,Pd2) ./ Fd

% Retrieve 6 world points X(:,1:6)
X = [ abcd [1;1;1;1] eye(4) ];

return

% Computes camera P from world points X and image points x, everything in canonical form.
function P = P_from_Xx_canonical(X,x)
X = X(:,1);
for k = 1:3
  A = [contreps(x(:,1,k))*[X(1) 0 0 X(4)
                           0 X(2) 0 X(4)
                           0 0 X(3) X(4)]
       contreps(x(:,2,k))*[eye(3) [1;1;1]]];
  [u,s,v] = svd(A,0);
  a = v(:,end);
  P(:,:,k) = [a(1) 0 0 a(4)
              0 a(2) 0 a(4)
              0 0 a(3) a(4)];
end
return

% A bit faster vgg_contreps.
function X = contreps(x)
X = [0 x(3) -x(2)
     -x(3) 0 x(1)
     x(2) -x(1) 0];
return

% H_from_4x  Having four point matches, the homography relating them is given by
% H = H_from_4x(x2)*inv(H_from_4x(x1)).
function H = H_from_4x(x)
H = x(:,1:3) * diag(inv(x(:,1:3))*x(:,4));
return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Test code:
P = randn(3,4,3); X = randn(4,6);
for k = 1:3, x(:,:,k) = P(:,:,k)*X; end
[Pn,Xn] = vgg_PX_from_6pts_3img(x);

% check image points x predicted up to scale
for k=1:3, Pn(:,:,k)*Xn(:,:) ./ x(:,:,k), end


function [nrm] = lognrm(tt)
%Frobenius norm of the TT-tensor
%   [NRM]=LOGNRM(TT) Computes log10 of the Frobenius norm of a tensor
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

d=tt.d;
n=tt.n;
r=tt.r;
pos=tt.ps;
cr=tt.core;
pos1=1;
nrm=zeros(d,1);
core0=cr(1:r(1)*n(1)*r(2));
%Orthogonalization from left-to-tight
for i=1:d-1
   core0=reshape(core0,[r(i)*n(i),r(i+1)]);
   [core0,ru]=qr(core0,0); nrm(i)=norm(ru,'fro');
   nrm(i)=max(nrm(i),1e-308);
   ru=ru./nrm(i);
   core1=cr(pos(i+1):pos(i+2)-1);
   core1=reshape(core1,[r(i+1),n(i+1)*r(i+2)]);
   core1=ru*core1;
   r(i+1)=size(core0,2);
   cr(pos1:pos1-1+r(i)*n(i)*r(i+1))=core0(:);
   cr(pos1+r(i)*n(i)*r(i+1):pos1+r(i)*n(i)*r(i+1)+r(i+1)*n(i+1)*r(i+2)-1)=core1(:);
   core0=core1;
   pos1=pos1+r(i)*n(i)*r(i+1);
end
pos1=pos1+r(d)*n(d)*r(d+1)-1;
nrm(d)=norm(core0(:));
nrm=sum(log10(nrm));
return
end