%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting %%	lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us %to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the HEART-TUBE-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function HeartTube()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx = 128;        % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Ny = 128;        % # of Eulerian Grid Pts. in y-Direction (MUST BE EVEN!!!)
Lx = 5.0;        % Length of Eulerian Grid in x-Direction
Ly = 5.0;        % Length of Eulerian Grid in y-Direction
dx = Lx/Nx;      % Eulerian Grid-step in x-direction
dy = Ly/Ny;      % Eulerian Grid-step in y-direction

% Immersed Structure Geometric / Dynamic Parameters %
ds = Lx/(2*Nx);  % Lagrangian Spacing
d = 1.0;         % Diameter of the tube
L = 3.0;         % Length of the heart-tube
struct_name = 'HeartTube'; % Name for .vertex, .spring, etc files.

% Call function to construct geometry
[xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(ds,L,d,Lx,Ly);
yLag = yLag - 1.25;

fprintf('\nUSER-INPUT FOR MODEL IN FITZHUGH_NAGUMO_1d:\n');
fprintf('numPtsAlongTube = %d\n',length(xLag)/2);
fprintf('ind_Start = %d\n',length(xLag)-2+1);
fprintf('ind_End = %d\n\n',length(xLag)-2+length(xLag)/2)

% Plot Geometry to test
plot(xLag(1:end/2),yLag(1:end/2),'r-'); hold on;
plot(xLag(end/2+1:end),yLag(end/2+1:end),'r-'); hold on;
plot(xLag,yLag,'*'); hold on;
xlabel('x'); ylabel('y');
axis([0 Lx 0 Ly]);

% Prints .vertex file!
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% Prints .spring file!
k_Spring = 1e7;
print_Lagrangian_Springs(xLag,k_Spring,ds,d,struct_name);

% Prints .muscle file!
%LFO = d; SK = 0.3; a = 0.25; b = 4.0; Fmax = 1e5;
%print_Lagrangian_Muscles(xLag,LFO,SK,a,b,Fmax,struct_name)

% Prints .beam file!
k_Beam = 2.5e6; C = 0.0;
print_Lagrangian_Beams(xLag,k_Beam,C,struct_name);

% Prints .target file!
k_Target = 1e7;
print_Lagrangian_Target_Pts(xLag,k_Target,struct_name);

% Prints .concentration file!
kDiffusion = 5e-5;
Concentration = give_Me_Initial_Concentration(Lx,Ly/2,Nx,Ny/2,dx,dy);
print_Concentration_Info(Nx,Ny/2,Concentration,kDiffusion,struct_name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints Vertex points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

    N = length(xLag); % Total Number of Lagrangian Pts
    num = 1;          % Number of target points on each end

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', 4*num );

    %Loops over all Lagrangian Pts.
    %for s = 1:N

    %Left Bottom Target Points
    for s=1:num
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %Right Bottom Target Points
    for s=(N/2)-(num-1):N/2
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %Left Top Target Points
    for s=(N/2+1):(N/2+1)+(num-1)
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %Right Top Target Points
    for s=N-(num-1):N
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    %end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called rubberband.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', N - 4 );

    %spring_force = kappa_spring*ds/(ds^2);

    %BEAMS BETWEEN VERTICES
    for s = 2:N-1
            if  s <= N/2-1
                % Bottom of tube
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C);
            elseif ( ( s >= N/2+2 ) && ( s <= N-1 ) )
                % Top of Tube
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1,   k_Beam, C);
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints MUSCLE points to a file called "struct_name".muscle
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Muscles(xLag,LFO,SK,a,b,Fmax,struct_name)

    N = length(xLag); %Number of Lagrangian Pts. Total

    muscle_fid = fopen([struct_name '.muscle'], 'w');

    fprintf(muscle_fid, '%d\n', N/2-2 );

    %spring_force = kappa_spring*ds/(ds^2);

    %MUSCLES BETWEEN VERTICES
    for s = 2:N/2-1
            fprintf(muscle_fid, '%d %d %1.16e %1.16e %1.16e %1.16e %1.16e\n', s, s+N/2, LFO, SK, a,b,Fmax);
    end
    fclose(muscle_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called rubberband.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,k_Spring,ds_Rest,d,struct_name)

    N = length(xLag); %Number of Lagrangian Pts. Total

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N-2 + N/2 ); %(N-2) btwn adajcent Lag. Pts, (N/2) btwn opposite sides of HT

    %spring_force = kappa_spring*ds/(ds^2);

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N/2
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            elseif ( ( s >= N/2+1 ) && ( s < N ) )
                %Case s=N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            end
    end

    for s = 1:N/2
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+N/2,   k_Spring/1e10, 0.0);
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(ds,L,d,Lx,Ly)

% The immsersed structure is a straight heart-tube %
x1 = [-L/2:ds:L/2 L/2];       % Constructs x-Values for bottom of tube
y1 = -d/2*ones(1,length(x1)); % Constructs y-Values for bottom of tube

x2 = x1;                      % Constructs x-Values for top of tube
y2 = -y1;                     % Constructs y-Values for top of tube

x1 = x1 + Lx/2;               % Shift into correct box
x2 = x2 + Lx/2;                % Shift into correct box

y1 = y1 + Ly/2;               % Shift into correct box
y2 = y2 + Ly/2;               % Shift into correct box

xLag = [x1 x2];
yLag = [y1 y2];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints CONCENTRATION INFO to file called
%           'struct_name'.concentration
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Concentration_Info(Nx,Ny,C,kDiffusion,struct_name)

    con_fid = fopen([struct_name '.concentration'], 'w');

    fprintf(con_fid, '%d\n', kDiffusion );

    for i=1:Ny
        for j=1:Nx
            fprintf(con_fid, '%1.16e ', C(i,j) );
        end
        fprintf(con_fid,'\n');
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: gives initial concentration gradient inside channel
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C = give_Me_Initial_Concentration(Lx,Ly,Nx,Ny,dx,dy)

%WHERE OUTER TUBE LIES
%xMin = 0.15; xMax = 0.45;
%yMin = 0.85; yMax = 1.15;

xMin = 1.6; xMax = 2.2;
yMin = 0.8; yMax = 1.7;

xMid = (xMin+xMax)/2;
yMid = (yMin+yMax)/2;

xDiff = (xMax-xMin)/2;
yDiff = (yMax-yMin)/2;

x = 0:dx:Lx;
y = 0:dy:Ly;
inds = give_Me_Indices_To_Apply_Force(x,y,xMin,xMax,yMin,yMax);

C = zeros(Ny,Nx);
for i=1:length( inds(:,1) )
    xInd = inds(i,1);
    yInd = inds(i,2);
    xPt = x(xInd);
    yPt = y(yInd);
    %C(xInd,yInd ) = (-0.5/yDiff^2)*( (yPt-yMid) - yDiff )*( (yPt-yMid) + yDiff ) +  (-0.5/xDiff^2)*( (xPt-xMid) - xDiff )*( (xPt-xMid) + xDiff ); %1.0;
    C(yInd,xInd ) = (-1.0/xDiff^2)*( (xPt-xMid) - xDiff )*( (xPt-xMid) + xDiff ); %1.0;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes indices for placing initial concentration
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function inds = give_Me_Indices_To_Apply_Force(x,y,xMin,xMax,yMin,yMax)

j=1; noMinYet = 1;
while noMinYet

    if ( x(j) >= xMin )
        iX_min = j;
        noMinYet = 0;
    end
    j=j+1;
end

j=length(x); noMaxYet = 1;
while noMaxYet

    if ( x(j) <= xMax )
        iX_max = j;
        noMaxYet = 0;
    end
    j=j-1;
end

j=1; noMinYet = 1;
while noMinYet

    if ( y(j) >= yMin )
        iY_min = j;
        noMinYet = 0;
    end
    j=j+1;
end

j=length(y); noMaxYet = 1;
while noMaxYet

    if ( y(j) <= yMax )
        iY_max = j;
        noMaxYet = 0;
    end
    j=j-1;
end

iX_Vec = iX_min:1:iX_max;
iY_Vec = iY_min:1:iY_max;

n = 1;
for i=1:length(iX_Vec)
    for j=1:length(iY_Vec)
        inds(n,1) = iX_Vec(i);
        inds(n,2) = iY_Vec(j);
        n = n+1;
    end
end


function lik = lik_inputdependentnoise(varargin)
%lik_inputdependentnoise    Create input-dependent noise likelihood structure
%
%  Description
%    LIK = LIK_INPUTDEPENDENTNOISE('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates a Gaussian likelihood with input dependent noise structure
%    in which the named parameters have the specified values. Any unspecified
%    parameters are set to default values.
%
%    LIK = LIK_INPUTDEPENDENTNOISE(LIK,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a likelihood function structure with the named
%    parameters altered with the specified values.
%
%    Parameters for Gaussian likelihood function [default]
%      sigma2       - variance [0.1]
%      sigma2_prior - prior for sigma2 [prior_logunif]
%      n            - number of observations per input (See using average
%                     observations below)
%
%    Note! If the prior is 'prior_fixed' then the parameter in
%    question is considered fixed and it is not handled in
%    optimization, grid integration, MCMC etc.
%
%    The likelihood is defined as follows:
%                    __ n
%      p(y|f1, f2) = || i=1 N(y_i | f1_i, sigma2*exp(f2_i))
%
%      where f1 is the first latent variable defining the mean of the
%      gaussian distribution, f2 is the second latent variable defining
%      the noise structure and sigma2 is coefficient for noise.
%
%  See also
%    GP_SET, LIK_*, PRIOR_*
%
% Copyright (c) 2007-2010 Jarno Vanhatalo & Jouni Hartikainen
% Copyright (c) 2010 Aki Vehtari
% Copyright (c) 2011 Ville Tolvanen

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'LIK_INPUTDEPENDENTNOISE';
  ip.addOptional('lik', [], @isstruct);
  ip.addParamValue('sigma2',0.1, @(x) isscalar(x) && x>0);
  ip.addParamValue('sigma2_prior',prior_logunif(), @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.nondiagW=true;
    lik.type = 'Inputdependentnoise';
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'Inputdependentnoise')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('sigma2',ip.UsingDefaults)
    lik.sigma2 = ip.Results.sigma2;
  end
  % Initialize prior structure
  if init
    lik.p=[];
  end
  if init || ~ismember('sigma2_prior',ip.UsingDefaults)
    lik.p.sigma2=ip.Results.sigma2_prior;
  end

  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_inputdependentnoise_pak;
    lik.fh.unpak = @lik_inputdependentnoise_unpak;
    lik.fh.lp = @lik_inputdependentnoise_lp;
    lik.fh.lpg = @lik_inputdependentnoise_lpg;
    lik.fh.ll = @lik_inputdependentnoise_ll;
    lik.fh.llg = @lik_inputdependentnoise_llg;
    lik.fh.llg2 = @lik_inputdependentnoise_llg2;
    lik.fh.llg3 = @lik_inputdependentnoise_llg3;
    lik.fh.predy = @lik_inputdependentnoise_predy;
    lik.fh.invlink = @lik_inputdependentnoise_invlink;
    lik.fh.predprcty = @lik_inputdependentnoise_predprcty;
    lik.fh.recappend = @lik_inputdependentnoise_recappend;
  end

  function [w,s,h] = lik_inputdependentnoise_pak(lik)
  %LIK_INPUTDEPENDENTNOISE_PAK  Combine likelihood parameters into one vector.
  %
  %  Description
  %    W = LIK_INPUTDEPENDENTNOISE_PAK(LIK) takes a likelihood structure LIK and
  %    combines the parameters into a single row vector W. This is a mandatory
  %    subfunction used for example in energy and gradient computations.
  %
  %       w = log(lik.sigma2)
  %
  %   See also
  %   LIK_INPUTDEPENDENTNOISE_UNPAK, GP_PAK

  w = []; s = {}; h=[];
  if isfield(lik.p, 'sigma2') && ~isempty(lik.p.sigma2)
    w = [w log(lik.sigma2)];
    s = [s 'log(gaussian.sigma2)'];
    h = [h 0];
    % Hyperparameters of sigma2
    [wh, sh, hh] = lik.p.sigma2.fh.pak(lik.p.sigma2);
    w = [w wh];
    s = [s sh];
    h = [h hh];
  end
  end

  function [lik, w] = lik_inputdependentnoise_unpak(lik, w)
  %LIK_INPUTDEPENDENTNOISE_UNPAK  Extract likelihood parameters from the vector.
  %
  %  Description
  %    [LIK, W] = LIK_INPUTDEPENDENTNOISE_UNPAK(W, LIK) takes a likelihood
  %    structure LIK and extracts the parameters from the vector W
  %    to the LIK structure. This is a mandatory subfunction used for
  %    example in energy and gradient computations.
  %
  %   Assignment is inverse of
  %       w = log(lik.sigma2)
  %
  %   See also
  %   LIK_INPUTDEPENDENTNOISE_PAK, GP_UNPAK

  if ~isempty(lik.p.sigma2)
    lik.sigma2 = exp(w(1));
    w = w(2:end);

    % Hyperparameters of sigma2
    [p, w] = lik.p.sigma2.fh.unpak(lik.p.sigma2, w);
    lik.p.sigma2 = p;
  end
  end

  function lp = lik_inputdependentnoise_lp(lik, varargin)
  %LIK_INPUTDEPENDENTNOISE_LP  log(prior) of the likelihood parameters
  %
  %  Description
  %    LP = LIK_INPUTDEPENDENTNOISE_LP(LIK) takes a likelihood structure
  %    LIK and returns log(p(th)), where th collects the parameters. This
  %    subfunction is needed when there are likelihood parameters.
  %
  %  See also
  %    LIK_INPUTDEPENDENTNOISE_LLG, LIK_INPUTDEPENDENTNOISE_LLG3, LIK_INPUTDEPENDENTNOISE_LLG2, GPLA_E

  % If prior for sigma2 parameter, add its contribution
  lp = 0;

  if ~isempty(lik.p.sigma2)
    likp=lik.p;
    lp = likp.sigma2.fh.lp(lik.sigma2, likp.sigma2) + log(lik.sigma2);
  end

  end

  function lpg = lik_inputdependentnoise_lpg(lik)
  %LIK_INPUTDEPENDENTNOISE_LPG  d log(prior)/dth of the likelihood
  %                parameters th
  %
  %  Description
  %    E = LIK_INPUTDEPENDENTNOISE_LPG(LIK) takes a likelihood structure
  %    LIK and returns d log(p(th))/dth, where th collects the parameters.
  %    This subfunction is needed when there are likelihood parameters.
  %
  %  See also
  %    LIK_INPUTDEPENDENTNOISE_LLG, LIK_INPUTDEPENDENTNOISE_LLG3, LIK_INPUTDEPENDENTNOISE_LLG2, GPLA_G

  lpg = [];

  if ~isempty(lik.p.sigma2)
    likp=lik.p;

    lpgs = likp.sigma2.fh.lpg(lik.sigma2, likp.sigma2);
    lpg = lpgs(1).*lik.sigma2 + 1;
    if length(lpgs) > 1
      lpg = [lpg lpgs(2:end)];
    end
  end
  end

  function ll = lik_inputdependentnoise_ll(lik, y, ff, z)
  %LIK_INPUTDEPENDENTNOISE_LL  Log likelihood
  %
  %  Description
  %    LL = LIK_INPUTDEPENDENTNOISE_LL(LIK, Y, F, Z) takes a likelihood
  %    structure LIK, incedence counts Y, expected counts Z, and
  %    latent values F. Returns the log likelihood, log p(y|f,z).
  %    This subfunction is needed when using Laplace approximation
  %    or MCMC for inference with non-Gaussian likelihoods. This
  %    subfunction is also used in information criteria (DIC, WAIC)
  %    computations.
  %
  %  See also
  %    LIK_INPUTDEPENDENTNOISE_LLG, LIK_INPUTDEPENDENTNOISE_LLG3, LIK_INPUTDEPENDENTNOISE_LLG2, GPLA_E

    f=ff(:);
    n=size(y,1);
    f1=f(1:n);
    f2=f((n+1):2*n);
    expf2=exp(f2);
    expf2(expf2<eps)=eps;
    expf2(expf2>1e6)=1e6;
    sigma2 = lik.sigma2;

    ll = sum(-0.5*log(2*pi.*sigma2.*expf2) - 1./(2.*sigma2.*expf2).*(y-f1).^2);

  end

  function llg = lik_inputdependentnoise_llg(lik, y, ff, param, z)
  %LIK_INPUTDEPENDENTNOISE_LLG  Gradient of the log likelihood
  %
  %  Description
  %    LLG = LIK_INPUTDEPENDENTNOISE_LLG(LIK, Y, F, PARAM) takes a likelihood
  %    structure LIK, incedence counts Y, expected counts Z and
  %    latent values F. Returns the gradient of the log likelihood
  %    with respect to PARAM. At the moment PARAM can be 'param' or
  %    'latent'. This subfunction is needed when using Laplace
  %    approximation or MCMC for inference with non-Gaussian likelihoods.
  %
  %  See also
  %    LIK_INPUTDEPENDENTNOISE_LL, LIK_INPUTDEPENDENTNOISE_LLG2, LIK_INPUTDEPENDENTNOISE_LLG3, GPLA_E

    f=ff(:);
    n=size(y,1);
    f1=f(1:n);
    f2=f((n+1):2*n);
    expf2 = exp(f2);
    expf2(expf2<eps)=eps;
    expf2(expf2>1e6)=1e6;
    sigma2 = lik.sigma2;

    switch param
      case 'param'

        llg=sum(-0.5./sigma2+(y-f1).^2./(2*expf2.*sigma2^2));
        % correction for the log transformation
        llg = llg.*lik.sigma2;

      case 'latent'

        llg1= (y-f1)./(sigma2*expf2);
        llg2= -0.5+(y-f1).^2./(2.*(expf2.*sigma2));

%         llg1=(-y+f1)/(sqrt(2*pi).*(sigma2.*expf2).^(3/2)).*exp(-1/(2.*sigma2*expf2).*(y-f1).^2);
%         llg2=-exp(f2-(y-f1).^2/(2.*expf2.*sigma2)).*sigma2/(2.*sqrt(2.*pi).*(expf2.*sigma2).^(3/2))+exp(-f2-(y-f1).^2/(2*expf2*sigma2)).*(y-f1).^2/(2.*sqrt(2.*pi.*expf2).*sigma2.^(3/2));

        llg=[llg1; llg2];
    end
  end

  function [llg2] = lik_inputdependentnoise_llg2(lik, y, ff, param, z)
  %function [pi_vec, pi_mat] = lik_inputdependentnoise_llg2(lik, y, ff, param, z)
  %LIK_INPUTDEPENDENTNOISE_LLG2  Second gradients of the log likelihood
  %
  %  Description
  %    LLG2 = LIK_INPUTDEPENDENTNOISE_LLG2(LIK, Y, F, PARAM) takes a likelihood
  %    structure LIK, incedence counts Y, expected counts Z, and
  %    latent values F. Returns the Hessian of the log likelihood
  %    with respect to PARAM. At the moment PARAM can be only
  %    'latent'. LLG2 is a vector with diagonal elements of the
  %    Hessian matrix (off diagonals are zero). This subfunction
  %    is needed when using Laplace approximation or EP for
  %    inference with non-Gaussian likelihoods.
  %
  %  See also
  %    LIK_INPUTDEPENDENTNOISE_LL, LIK_INPUTDEPENDENTNOISE_LLG, LIK_INPUTDEPENDENTNOISE_LLG3, GPLA_E

    f=ff(:);

    n=length(y);
    f1=f(1:n);
    f2=f((n+1):2*n);
    sigma2 = lik.sigma2;
    expf2=exp(f2);
    expf2(expf2<eps)=eps;
    expf2(expf2>1e6)=1e6;

    switch param
      case 'param'

      case 'latent'

%         llg2 = [2./(sigma2.*expf2); 3/2.*(y-f1).^2./(sigma2.*expf2)];
%         llg2mat = [diag(1./sqrt(sigma2.*expf2)); diag(-(y-f1)./sqrt(sigma2.*expf2))];

        llg2_11=-1./(sigma2.*expf2);
        llg2_12=-(y-f1)./(sigma2*expf2);
        llg2_22=-(y-f1).^2./(2.*sigma2.*expf2);

        llg2 = [llg2_11 llg2_12; llg2_12 llg2_22];

      case 'latent+param'

        llg2_1=-(y-f1)./(expf2.*sigma2^2);
        llg2_2=-(y-f1).^2./(2*sigma2.^2.*expf2);

        llg2=[llg2_1; llg2_2];
        % correction for the log transformation
        llg2 = llg2.*lik.sigma2;

    end
  end

  function llg3 = lik_inputdependentnoise_llg3(lik, y, ff, param, z)
  %LIK_INPUTDEPENDENTNOISE_LLG3  Third gradients of the log likelihood
  %
  %  Description
  %    LLG3 = LIK_INPUTDEPENDENTNOISE_LLG3(LIK, Y, F, PARAM) takes a likelihood
  %    structure LIK, incedence counts Y, expected counts Z and
  %    latent values F and returns the third gradients of the log
  %    likelihood with respect to PARAM. At the moment PARAM can be
  %    only 'latent'. LLG3 is a vector with third gradients. This
  %    subfunction is needed when using Laplace approximation for
  %    inference with non-Gaussian likelihoods.
  %
  %  See also
  %    LIK_INPUTDEPENDENTNOISE_LL, LIK_INPUTDEPENDENTNOISE_LLG, LIK_INPUTDEPENDENTNOISE_LLG2, GPLA_E, GPLA_G

    f=ff(:);

    n=size(y,1);
    f1=f(1:n);
    f2=f((n+1):2*n);
    expf2=exp(f2);
    expf2(expf2<eps)=eps;
    expf2(expf2>1e6)=1e6;
    sigma2 = lik.sigma2;

    switch param
      case 'param'

      case 'latent'
        nl=2;
        llg3=zeros(nl,nl,nl,n);

        % y=0:
        % thrid derivative derivative wrt f1 (11)
        %       llg3(1,1,1,:) = 0
        % thrid derivative derivative wrt f2 (11)
        llg3(1,1,2,:) = 1./(expf2.*sigma2);

        % thrid derivative derivative wrt f1 (12/21)
        llg3(1,2,1,:) = 1./(expf2.*sigma2);
        llg3(2,1,1,:) = llg3(1,2,1,:);
        % thrid derivative derivative wrt f2 (12/21)
        llg3(1,2,2,:) = (y-f1)./(expf2.*sigma2);
        llg3(2,1,2,:) = llg3(1,2,2,:);

        % thrid derivative derivative wrt f1 (22)
        llg3(2,2,1,:) = llg3(1,2,2,:);
        % thrid derivative derivative wrt f1 (22)
        llg3(2,2,2,:) = (y-f1).^2./(2.*expf2.*sigma2);

      case 'latent2+param'

        llg3_11=1./(expf2.*sigma2.^2);
        llg3_12=(y-f1)./(expf2.*sigma2.^2);
        llg3_22=(y-f1).^2./(2.*expf2.*sigma2.^2);

        llg3 = [diag(llg3_11) diag(llg3_12); diag(llg3_12) diag(llg3_22)];
        % correction for the log transformation
        llg3 = llg3.*lik.sigma2;

    end
  end

  function [lpy, Ey, Vary] = lik_inputdependentnoise_predy(lik, Ef, Varf, yt, z)
  %LIK_INPUTDEPENDENTNOISE_PREDY  Returns the predictive mean, variance and density of y
  %
  %  Description
  %    [LPY] = LIK_INPUTDEPENDENTNOISE_PREDY(LIK, EF, VARF, YT) takes a
  %    likelihood structure LIK, posterior mean EF, posterior
  %    variance VARF of the latent variable and observations YT and
  %    returns the logarithm of the predictive density PY of YT, that is
  %        p(yt | th) = \int p(yt | f, th) p(f|y) df.
  %    This subfunction is needed when computing posterior predictive
  %    distributions for future observations.
  %
  %    [LPY, EY, VARY] = LIK_INPUTDEPENDENTNOISE_PREDY(LIK, EF, VARF YT)
  %    Returns also the posterior predictive mean EY and variance VARY of
  %    the observations related to the latent variables. This subfunction
  %    is needed when computing posterior predictive distributions for
  %    future observations.
  %
  %  See also
  %    GPLA_PRED, GPEP_PRED, GPMC_PRED

%     ntest=size(yt,1);
    Ef = Ef(:);
    ntest = 0.5*size(Ef,1);
    Ef1=Ef(1:ntest); Ef2=Ef(ntest+1:end);
    if size(Varf,2) == size(Varf,1)
      Varf1=diag(Varf(1:ntest,1:ntest));Varf2=diag(Varf(ntest+1:end,ntest+1:end));
    else
      if size(Varf,2)==1
        Varf=reshape(Varf,ntest,2);
      end
      Varf1=Varf(:,1); Varf2=Varf(:,2);
    end
    sigma2=lik.sigma2;

    lpy = zeros(size(yt));

    Ey=Ef1;
    Vary=Varf1 + sigma2.*exp(Ef2+Varf2/2);

    if ~isempty(yt)
      for i2=1:ntest
        m1=Ef1(i2); m2=Ef2(i2);
        s1=sqrt(Varf1(i2)); s2=sqrt(Varf2(i2));
        pd=@(f1,f2) norm_pdf(yt(i2), f1, sqrt(sigma2.*exp(f2))).*norm_pdf(f1,Ef1(i2),sqrt(Varf1(i2))).*norm_pdf(f2,Ef2(i2),sqrt(Varf2(i2)));
        lpy(i2) = log(dblquad(pd, m1-6.*s1, m1+6.*s1, m2-6.*s2, m2+6.*s2));
      end
    else
      lpy=[];
    end
%     sigma2 = lik.sigma2;
%     Ef1=Ef(1:ntest);
%     Ef2=Ef((ntest+1):2*ntest);
%     Ey = Ef1;
%     Vary = Varf + sigma2.*exp(Ef2);

  end

  function prctys = lik_inputdependentnoise_predprcty(lik, Ef, Varf, zt, prcty)
  %LIK_BINOMIAL_PREDPRCTY  Returns the percentiles of predictive density of y
  %
  %  Description
  %    PRCTY = LIK_BINOMIAL_PREDPRCTY(LIK, EF, VARF YT, ZT)
  %    Returns percentiles of the predictive density PY of YT, that is
  %    This requires also the succes counts YT, numbers of trials ZT. This
  %    subfunction is needed when using function gp_predprcty.
  %
  %  See also
  %    GP_PREDPCTY

  n=size(Ef,1)./2;
  prcty = prcty./100;
  prcty = norminv(prcty, 0, 1);
  prctys = bsxfun(@plus, Ef(1:n), bsxfun(@times, sqrt(Varf(1:n) + lik.sigma2.*exp(Ef(n+1:end))), prcty));

  end

  function p = lik_inputdependentnoise_invlink(lik, f, z)
  %LIK_INPUTDEPENDENTNOISE_INVLINK  Returns values of inverse link function
  %
  %  Description
  %    P = LIK_INPUTDEPENDENTNOISE_INVLINK(LIK, F) takes a likelihood structure LIK and
  %    latent values F and returns the values of inverse link function P.
  %    This subfunction is needed when using function gp_predprcty.
  %
  %     See also
  %     LIK_INPUTDEPENDENTNOISE_LL, LIK_INPUTDEPENDENTNOISE_PREDY

    p = exp(f);
  end

  function reclik = lik_inputdependentnoise_recappend(reclik, ri, lik)
  %RECAPPEND  Append the parameters to the record
  %
  %  Description
  %    RECLIK = GPCF_INPUTDEPENDENTNOISE_RECAPPEND(RECLIK, RI, LIK) takes a
  %    likelihood record structure RECLIK, record index RI and
  %    likelihood structure LIK with the current MCMC samples of
  %    the parameters. Returns RECLIK which contains all the old
  %    samples and the current samples from LIK. This subfunction
  %    is needed when using MCMC sampling (gp_mc).
  %
  %  See also
  %    GP_MC

  % Initialize record
    if nargin == 2
      reclik.type = 'Inputdependentnoise';
      reclik.nondiagW=true;

      % Initialize parameter

      % Set the function handles
      reclik.fh.pak = @lik_inputdependentnoise_pak;
      reclik.fh.unpak = @lik_inputdependentnoise_unpak;
      reclik.fh.lp = @lik_inputdependentnoise_lp;
      reclik.fh.lpg = @lik_inputdependentnoise_lpg;
      reclik.fh.ll = @lik_inputdependentnoise_ll;
      reclik.fh.llg = @lik_inputdependentnoise_llg;
      reclik.fh.llg2 = @lik_inputdependentnoise_llg2;
      reclik.fh.llg3 = @lik_inputdependentnoise_llg3;
      reclik.fh.predy = @lik_inputdependentnoise_predy;
      reclik.fh.predprcty = @lik_inputdependentnoise_predprcty;
      reclik.fh.invlink = @lik_inputdependentnoise_invlink;
      reclik.fh.recappend = @lik_inputdependentnoise_recappend;
      reclik.p=[];
      if ~isempty(ri.p.sigma2)
        reclik.p.sigma2 = ri.p.sigma2;
      end
      return
    end

    reclik.sigma2(ri,:)=lik.sigma2;
    if ~isempty(lik.p.sigma2)
      reclik.p.sigma2 = feval(lik.p.sigma2.fh.recappend, reclik.p.sigma2, ri, lik.p.sigma2);
    end
  end
end


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  battistn@tcnj.edu
% Current Institution: TCNJ
% Date Modified: April 2021
%
% Date Created: May 27th, 2015
% Institution when Created: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs or non-invariant beams*)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (battistn@tcnj.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the LEAF_LEAF-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Leaf_Leaf()

%-----------------------------------------------------
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%-----------------------------------------------------
Nx = 384         % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Ny = 64          % # of Eulerian Grid Pts. in y-Direction (MUST BE EVEN!!!)
Lx = 30.0        % Length of Eulerian Grid in x-Direction (cm)
Ly = 5.0         % Length of Eulerian Grid in y-Direction (cm)
ds= 0.5*Lx/Nx;   % Lagrangian spacing

%----------------------------------------------------------------
% AIR PROPERTIES FOR INPUT
%
%     Density:     [rho] = kg/m^3
%     Newton:      [N] = kg * m/s^2
%     Dyn. Visc.:  [N*s/m^2] = (kg*m/s^2)*(s/m^2) = kg / (m*s)
%
%----------------------------------------------------------------
% PROPERTIES IN MKS UNITS
%mu_Air = 18.03e-6; % N*s/m^2 dynamic viscosity of air at 18C (64.4F)
%rho_Air = 1.21;    % kg/m^3  density of air at 18C
mu_Air = 1.895e-5; % N*s/m^2 =  dynamic viscosity of air at 18C (64.4F)
rho_Air = 1.145;    % kg/m^3  density of air at 35C
%(from https://www.engineersedge.com/physics/viscosity_of_air_dynamic_and_kinematic_14483.htm)
%
% CONVERT TO CGS UNITS
mu_Air = mu_Air * (1000/1) * (1/100)     %(1000g/1kg) & (1m/100cm)
rho_Air = rho_Air * (1000/1) * (1/100)^3 %(1000g/1kg) & (1m/100cm)
%
% FINAL UNITS
% [mu_Air] = g/(cm*s)
% [rho_Air] = g/cm^3

%----------------------------------------------------
% LEAF GEOMETRY
%----------------------------------------------------
xLag = [];                     % Initialize Storage
yLag = [];                     % Initialize Storage
L_leaf = 1.0;                  % Length of tail off cylinder
struct_name = 'leaf'; % Name for .vertex, .spring, etc files
%
x0 = 3.0;                      % Starting x-Pt for LEAF
y0 = Ly/2;                     % Starting y_Pt for LEAF
%
xLag_L = x0:ds:x0+L_leaf;
yLag_L = y0*ones(1,length(xLag_L));

%----------------------------------------------------
% IF CHANNEL WALLS / CYLINDRICAL LEADING EDGE %
%----------------------------------------------------
r = 0.01;                      % Radii of Cylinder

%--------------------------------------------
% Call function to construct geometry
%       * No Channel Walls
%       * No Cylinder on Leading Edge
%--------------------------------------------
[xLag_Cy,yLag_Cy] = give_Me_Cylinder_Immersed_Boundary_Geometry(ds,r,x0,y0);
%
% % Plot Geometry to test
% plot(xLag_Ch(1:end/2),yLag_Ch(1:end/2),'r-'); hold on;
% plot(xLag_Ch(end/2+1:end),yLag_Ch(end/2+1:end),'r-'); hold on;
% plot(xLag_Cy,yLag_Cy,'r-'); hold on;
% plot(xLag_L,yLag_L,'m-'); hold on;
% axis([0 Lx 0 Ly]);
% %
% plot(xLag_Ch,yLag_Ch,'*'); hold on;
% plot(xLag_Cy,yLag_Cy,'g*'); hold on;
% xlabel('x'); ylabel('y');
% axis square;
%
% xLag = [xLag xLag_Ch xLag_Cy]; % Add xLagPts from Circle to xLag Pt. Vector (*no springs or beams*)
% yLag = [yLag yLag_Ch yLag_Cy]; % Add xLagPts from Circle to xLag Pt. Vector (*no springs or beams*)
%
% Nbefore = length(xLag);   % # of Lagrangian Pts Before Leaf to COUNT for...
%                           % ... Cylinder / Channel Points have TARGET PTS
%

xLag = [xLag xLag_Cy]; % Add xLagPts from Circle to xLag Pt. Vector (*no springs or beams*)
yLag = [yLag yLag_Cy]; % Add xLagPts from Circle to xLag Pt. Vector (*no springs or beams*)

Nbefore = length(xLag); % # of Lag Pts Before Leaf Starts

xLag = [xLag xLag_L];   % Update xLag Pts to Include Leaf
yLag = [yLag yLag_L];   % Update xLag Pts to Include Leaf

Ntot = length(xLag);    % Total # of Lag Pts

%------------------------
% PLOT GEOMETRY
%------------------------
plot(xLag,yLag,'.'); hold on;
xlabel('x'); ylabel('y');
axis([0 Lx 0 Ly]);

% Prints .vertex file!
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% Prints .spring file!
k_Spring = 2.5e4; %1e7
print_Lagrangian_Springs(xLag,yLag,k_Spring,struct_name,Nbefore,Ntot);

% Prints .beam file!
k_Beam = 2e8; C = 0.0; %2e12 -> 4e12
print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name,Nbefore,Ntot);

% Prints .target file!
k_Target = 2.5e4;
print_Lagrangian_Target_Pts(xLag,k_Target,struct_name,Nbefore);

% Prints .mass file!
k_Mass = 1e0;         % 'spring' stiffness parameter for tethering
Mass = 0.5e-3;        % "MASS" value for 'ghost' lag movement
print_Lagrangian_Mass_Pts(xLag,k_Mass,Mass,struct_name,Nbefore,Ntot);

% Call function for initial concentration
[Concentration]= give_Me_Initial_Concentration(Nx,Ny);

% Prints .concentration file!
print_Concentration_Info(Nx,Ny,Concentration,struct_name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called <struct_name>.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called <struct_name>.target
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name,Nbefore)

    %N = length(xLag);

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', Nbefore+3 );

    %Loops over any Channel/Cylinder Pts AND First 3 Pts Along LEAF
    for s = 1:Nbefore+3
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAMs (Torsional Springs) to a file called <struct_name>.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name,Nbefore,Ntot)

    % k_Beam: beam stiffness
    % C: beam curvature

    if Nbefore ~= 0
        N = Ntot-Nbefore-1;
    else
        N = Ntot-Nbefore-2; % NOTE: Total number of beams
    end

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %------------------------------------------------
    % BEAM BETWEEN CYLINDER AND LEAF
    %       (including those TETHERED in place by
    %                                   TARGET PTS)
    %------------------------------------------------
    if Nbefore ~= 0
        s1 = Nbefore;
        s2 = Nbefore+1;
        s3 = Nbefore+2;
        fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s1, s2, s3, k_Beam, C);
    end

    %------------------------------------------------
    % BEAM BETWEEN VERTICES ALONG "LEAF"
    %       (including those TETHERED in place by
    %                                   TARGET PTS)
    %------------------------------------------------
    for s = Nbefore+2:Ntot-1
        s1 = s-1;
        s2 = s;
        s3 = s+1;
        fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s1, s2, s3, k_Beam, C);
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called <struct_name>.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,k_Spring,struct_name,Nbefore,Ntot)

    if Nbefore ~= 0
        N = Ntot-Nbefore;
    else
        N = Ntot-Nbefore-1;
    end

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %------------------------------------------------
    % SPRINGS BETWEEN CYLINDER AND LEAF
    %       (including those TETHERED in place by
    %                                   TARGET PTS)
    %------------------------------------------------
    if Nbefore ~= 0

        % springs between adjacent pts on cylinder
        for i=1:Nbefore
            if i<Nbefore
                x1 = xLag(i); y1 = yLag(i);
                x2 = xLag(i+1); y2 = yLag(i+1);
                ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', i, i+1, k_Spring, ds);
            else
                x1 = xLag(i); y1 = yLag(i);
                x2 = xLag(1); y2 = yLag(1);
                ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', i, 1, k_Spring, ds);

            end
        end

        % springs across opposite sides of cylinder
        for i=1:floor(Nbefore/2)
            x1 = xLag(i); y1 = yLag(i);
            x2 = xLag( i+floor(Nbefore/2) ); y2 = yLag( i+floor(Nbefore/2) );
            ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
            fprintf(spring_fid, '%d %d %1.16e %1.16e\n', i, i+floor(Nbefore/2), k_Spring, ds);
        end

        % spring between cylinder and leaf
        x1 = xLag(Nbefore); y1 = yLag(Nbefore);
        x2 = xLag(Nbefore+1); y2 = yLag(Nbefore+1);
        ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', Nbefore, Nbefore+1, k_Spring, ds);

    end

    %------------------------------------------------
    % SPRINGS BETWEEN VERTICES ALONG LEAF
    %       (including those TETHERED in place by
    %                                   TARGET PTS)
    %------------------------------------------------
    for s = Nbefore+1:Ntot-1

        x1 = xLag(s); y1 = yLag(s);
        x2 = xLag(s+1); y2 = yLag(s+1);

        ds = sqrt( (x1-x2)^2 + (y1-y2)^2 );

        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);

    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called <struct_name>.mass
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Mass_Pts(xLag,kMass,Mass,struct_name,Nbefore,Ntot)

    %LOOP OVER LAG PTS FOR MASS PTS IN LAGRANGIAN INDEXING
    N = (Ntot-Nbefore);

    mass_fid = fopen([struct_name '.mass'], 'w');

    fprintf(mass_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = Nbefore+1:Ntot%Nbefore:Ntot
        fprintf(mass_fid, '%d %1.16e %1.16e\n', s, kMass, Mass );
    end

    fclose(mass_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for channel
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Channel_Immersed_Boundary_Geometry(ds,L,w,Lx,Ly)

% The immsersed structure is a channel %
x = (Lx-L)/2:ds:(L+(Lx-L)/2);  %xPts
yBot = (Ly-w)/2;               %yVal for bottom of Channel
yTop = Ly - (Ly-w)/2;          %yVal for top of Channel

xLag = [x x];
yLag = [yBot*ones(1,length(x)) yTop*ones(1,length(x))];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for cylinder
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLagN,yLagN] = give_Me_Cylinder_Immersed_Boundary_Geometry(ds,r,x0,y0)

% The immsersed structure is a cylinder %

dtheta = ds/ (4*r);
theta = 0; i=1;
while theta < 2*pi
   xLag(i) = (x0-r-ds) + r*cos(theta);
   yLag(i) = y0 + r*sin(theta);
   theta = theta + dtheta;
   i=i+1;
end

for i=1:length(xLag)
   xLagN(i) = xLag(end-(i-1));
   yLagN(i) = yLag(end-(i-1));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints initial concentration to a file called
% <struct_name>.concentration
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Concentration_Info(Nx,Ny,C,struct_name)

    con_fid = fopen([struct_name '.concentration'], 'w');

    for i=1:Ny
        for j=1:Nx
            fprintf(con_fid, '%1.16e ', C(i,j) );
        end
        fprintf(con_fid,'\n');
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: gives initial concentration gradient inside channel
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [C] = give_Me_Initial_Concentration(Nx,Ny)

C = zeros(Ny,Nx);


function [results] = vdpgm(given_data, opts)
%
% function [results] = vdpgm(given_data, opts)
%

start_time = clock;
if nargin == 1
  opts = struct();
end
if issparse(given_data)
  given_data = full(given_data);
end
if ~ isfield(opts, 'algorithm')
  % algorithm can be one of 'vdp', 'bj', 'cdp', 'csb' and 'non_dp'
  % vdp : variational DP
  % bj : Blei and Jordan
  % cdp : collapsed Dirichlet prior
  % csb : collapsed stick-breaking
  % non_dp : variational Bayes for Gaussian mixtures
  opts.algorithm = 'vdp';
end
if ~ isfield(opts, 'collapsed_means')
  opts.collapsed_means = 0;
end
if ~ isfield(opts, 'do_sort')
  opts.do_sort = '0';
end
if ~ isfield(opts, 'get_q_of_z')
  opts.get_q_of_z = 0;
end
if ~ isfield(opts, 'weight')
  opts.weight = 1;
end
if ~ isfield(opts, 'get_log_likelihood')
  opts.get_log_likelihood = 0;
end
if ~ isfield(opts, 'use_kd_tree')
  opts.use_kd_tree = 1;
end
if ~ isfield(opts, 'threshold')
  opts.threshold = 1.0e-5;
end
if ~ isfield(opts, 'sis')
  opts.sis = 0;
end
if ~ isfield(opts, 'initial_depth')
  opts.initial_depth = 3;
end
if ~ isfield(opts, 'initial_K')
  opts.initial_K = 1;
end
if ~ isfield(opts, 'ite')
  opts.ite = inf;
end
if ~ isfield(opts, 'do_split')
  opts.do_split = 0;
end
if ~ isfield(opts, 'do_merge')
  opts.do_merge = 0;
end
if ~ isfield(opts, 'do_greedy')
  opts.do_greedy = 1;
end
if ~ isfield(opts, 'max_target_ratio')
  opts.max_target_ratio = 0.5;
end
if ~ isfield(opts, 'init_of_split')
  % 'pc', 'rnd', 'rnd_close' or 'close_f'
  opts.init_of_split = 'pc';
end
if ~ isfield(opts, 'recursive_expanding_depth')
  opts.recursive_expanding_depth = 2;
end
if ~ isfield(opts, 'recursive_expanding_threshold')
  opts.recursive_expanding_threshold = 1.0e-1;
end
if ~ isfield(opts, 'recursive_expanding_frequency')
  opts.recursive_expanding_frequency = 3;
end
if isfield(opts, 'seed')
  rand('state', opts.seed);
else
  seed = rand('state');
  results.seed = seed;
end

data.given_data = given_data;
if opts.use_kd_tree
  partitions = init_kdtree_partitions(given_data, opts.initial_depth);
  data.kdtree = partitions;
end

% the hyperparameters of priors
hp_prior = mk_hp_prior(data, opts);

if isfield(opts, 'hp_posterior')
  opts.use_kd_tree = 0;
  if opts.get_q_of_z
    results.q_of_z = mk_q_of_z(data, opts.hp_posterior, opts.hp_prior, opts);
  end
  if opts.get_log_likelihood
    results.log_likelihood = mk_log_likelihood(data, opts.hp_posterior, opts.hp_prior, opts);
  end
  if isfield(opts, 'test_data')
    results.predictive_posterior = log_predictive_dist(data, opts.q_of_z, opts.hp_posterior, ...
                                                      opts.hp_prior, opts);
  end
  return
end

if opts.sis > 0
  q_of_z = sequential_importance_sampling(data, hp_prior, opts);
elseif isfield(opts, 'q_of_z')
  q_of_z = opts.q_of_z;
else
  q_of_z = rand_q_of_z(data, opts.initial_K, opts);
end

hp_posterior = mk_hp_posterior(data, q_of_z, hp_prior, opts);
if opts.do_greedy
  [free_energy, hp_posterior, data] = greedy(data, hp_posterior, hp_prior, opts);
else
  [free_energy, hp_posterior, data] = split_merge(data, hp_posterior, hp_prior, opts);
end

results.algorithm = opts.algorithm;
results.elapsed_time = etime(clock, start_time);
results.free_energy = free_energy;
results.hp_prior = hp_prior;
results.hp_posterior = hp_posterior;
results.K = length(hp_posterior.eta);
results.opts = opts;
if opts.get_q_of_z
  results.q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts);
  if opts.use_kd_tree
    results.q_of_z = mk_non_kdtree_q_of_z(data, results.q_of_z);
  end
end
if opts.get_log_likelihood
  results.log_likelihood = mk_log_likelihood(data, hp_posterior, hp_prior, opts);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_likelihood = mk_log_likelihood(data, hp_posterior, hp_prior, opts);
[D,N] = size(data.given_data);
K = size(hp_posterior.m, 2);
log_likelihood = zeros(K,N);
E_pi = mk_E_pi(hp_posterior, hp_prior, opts);
for c=1:K
  mu = hp_posterior.m(:,c);
  f = hp_posterior.eta(c) + 1 - D;
  Sigma = (hp_posterior.xi(c)+1) / hp_posterior.xi(c) / f * hp_posterior.B{c};
  log_likelihood(c,:) = log_no_w(E_pi(c)) + logmvtpdf(data.given_data, mu, f, Sigma);
end
log_likelihood = log_sum_exp(log_likelihood, 1); % 1 by N
log_likelihood = sum(log_likelihood, 2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_prob = log_predictive_dist(data, q_of_z, hp_posterior, hp_prior, opts);
if opts.use_kd_tree
  N = length(data.kdtree);
  D = size(data.kdtree(1).sum_x, 1);
  Na = [data.kdtree(:).N];
  if isequal(opts.algorithm, 'vdp')
    true_Nc = Na*q_of_z; % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = Na*q_of_z; % 1*K
  sum_x = [data.kdtree(:).sum_x] * q_of_z;
else
  [D,N] = size(data.given_data);
  if isequal(opts.algorithm, 'vdp')
    true_Nc = sum(q_of_z, 1); % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = sum(q_of_z, 1); % 1*K
  sum_x = data.given_data * q_of_z;
end
E_pi = mk_E_pi(hp_posterior, hp_prior, opts);
m = (sum_x + repmat(hp_prior.xi0*hp_prior.m0, 1, size(sum_x,2))) ./ repmat(Nc, D, 1);
E_pi(find(Nc==0)) = 0;
n = size(opts.test_data, 2);
log_prob = zeros(size(m,2), n);
for c=1:size(m,2)
  f = hp_posterior.eta(c) + 1 - D;
  sigma = hp_posterior.B{c} * (hp_posterior.xi(c)+1) / hp_posterior.xi(c) / f;
  if E_pi(c) > 0
    log_prob(c,:) = log(E_pi(c)) + log_T_dist(opts.test_data, m(:,c), ...
                                              sigma, f);
  else
    log_prob(c,:) = -inf;
  end
end
log_prob = log_sum_exp(log_prob,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_prob = log_T_dist(X, m, sigma, f);
% X is a D by n matrix
[D,n] = size(X);
diff = X-repmat(m,1,n);
log_prob = gammaln((f+D)/2) ...
    - D/2*log(f*pi) ...
    - gammaln(f/2) ...
    - 0.5*detln(sigma) ...
    - (f+D)/2*log(1 + sum( diff .* ((f*sigma) \ diff), 1));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function quad_term = mk_quad_term(data, q_of_z, hp_prior, opts)
% calculate the quad_term in marginalizing out means
% quad_term : D by D by K
%
[N, K] = size(q_of_z);
D = size(data.given_data, 1);
Nc = sum(q_of_z, 1); % 1 by K
xi = Nc + hp_prior.xi0;
f0 = 1 ./ (xi.^2);
f1 = - 2 ./ (xi.^3);
f2 = 3 ./ (xi.^4);
v = q_of_z.*(1 - q_of_z); % N by K
c = v.*((1-q_of_z).^2 + q_of_z.^2);
q = v.*((1-q_of_z).^3 + q_of_z.^3);
p_x = data.given_data * q_of_z; % D by K
v_x = data.given_data * v;      % D by K
c_x = data.given_data * c;      % D by K
v_p_x = data.given_data * (q_of_z + v); % D by K
c_p_x = data.given_data * (q_of_z + c); % D by K
for t=1:K
  first_term = f0(t) ...
      *((repmat(v(:,t)', D, 1).*data.given_data)*data.given_data' ...
        + p_x(:,t)*p_x(:,t)');
  second_term = f1(t) ...
      *((repmat(c(:,t)', D, 1).*data.given_data)*data.given_data' ...
        + v_p_x(:,t)*v_p_x(:,t)' - v_x(:,t)*v_x(:,t)' - p_x(:,t)*p_x(:,t)');
  tmp = q(:,t) - 3*v(:,t).^2 + v(:,t)*sum(v(:,t));
  third_term = f2(t) ...
      *((repmat(tmp', D, 1).*data.given_data)*data.given_data' ...
        + c_p_x(:,t)*c_p_x(:,t)' - c_x(:,t)*c_x(:,t)' - p_x(:,t)*p_x(:,t)' ...
        + 2*v_x(:,t)*v_x(:,t)' ...
        + sum(v(:,t))*p_x(:,t)*p_x(:,t)');
  quad_term(:,:,t) = first_term + second_term + third_term;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function E_log_p_of_z_given_other_z = mk_E_log_p_of_z_given_other_z(hp_posterior, hp_prior, opts);
% returns E[log p(z_n|Z^-n)]_q(Z^-n)
% E_log_p_of_z_given_other_z : N by K
q_of_z = hp_posterior.q_of_z;
[N,K] = size(q_of_z);
[E_Nc_minus_n, V_Nc_minus_n] = mk_E_Nc_minus_n(q_of_z); % N by K
use_variance = 1;
if isequal(opts.algorithm, 'cdp')
  if use_variance
    E_log_p_of_z_given_other_z = ...
        log(E_Nc_minus_n + hp_prior.alpha/K) ...
        - 0.5*V_Nc_minus_n./((E_Nc_minus_n+hp_prior.alpha/K).^2) ...
        - log(N - 1 + hp_prior.alpha);
  else
    E_log_p_of_z_given_other_z = ...
        log(E_Nc_minus_n + hp_prior.alpha/K) ...
        - log(N - 1 + hp_prior.alpha);
  end
elseif isequal(opts.algorithm, 'csb')
  E_Nc_minus_n_cumsum_geq = fliplr(cumsum(fliplr(E_Nc_minus_n), 2));
  q_of_z_cumsum_geq = fliplr(cumsum(fliplr(q_of_z), 2));
  dummy = q_of_z_cumsum_geq.*(1-q_of_z_cumsum_geq);
  v_Nc_cumsum_geq = repmat(sum(dummy, 1), N, 1) - dummy;

  E_Nc_minus_n_cumsum = E_Nc_minus_n_cumsum_geq - E_Nc_minus_n;
  q_of_z_cumsum = q_of_z_cumsum_geq - q_of_z;
  dummy = q_of_z_cumsum.*(1-q_of_z_cumsum);
  v_Nc_cumsum = repmat(sum(dummy, 1), N, 1) - dummy;

  if use_variance
    first_term = ...
        (log(1+E_Nc_minus_n) ...
         - 0.5*V_Nc_minus_n./((1+E_Nc_minus_n).^2) ...
         - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq) ...
         + 0.5*v_Nc_cumsum_geq./((1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq).^2));
    first_term(:,end) = 0;
    dummy = log(hp_prior.alpha+E_Nc_minus_n_cumsum) ...
            - 0.5*v_Nc_cumsum./((hp_prior.alpha+E_Nc_minus_n_cumsum).^2) ...
            - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq) ...
            + 0.5*v_Nc_cumsum_geq./((1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq).^2);
    second_term = cumsum(dummy, 2) - dummy;
  else
    first_term = log(1+E_Nc_minus_n) - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq);
    first_term(:,end) = 0;
    dummy = log(hp_prior.alpha+E_Nc_minus_n_cumsum) ...
            - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq);
    second_term = cumsum(dummy, 2) - dummy;
  end
  E_log_p_of_z_given_other_z = first_term + second_term;
else
  error('unsupported algorithm');
end
% Gaussian approximation may not give a proper distribution.
% note. E[log p] is not need to be proper

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [E_Nc_minus_n, V_Nc_minus_n] = mk_E_Nc_minus_n(q_of_z)
% returns E[Nc^-n]; the expected value of Nc-n; N by K
%         V[Nc^-n]; the variance of Nc-n;       N by K
% q_of_z : N by K
N = size(q_of_z, 1);
E_Nc_minus_n = repmat(sum(q_of_z, 1), N, 1) - q_of_z;
tmp = q_of_z.*(1-q_of_z);
V_Nc = sum(tmp, 1);
V_Nc_minus_n = repmat(V_Nc, N, 1) - tmp;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function E_pi = mk_E_pi(hp_posterior, hp_prior, opts);
if isequal(opts.algorithm, 'cdp') | isequal(opts.algorithm, 'non_dp')
  E_pi = hp_posterior.tilde_alpha / sum(hp_posterior.tilde_alpha);
elseif isequal(opts.algorithm, 'bj')
  second_term = [0 cumsum(log(hp_posterior.gamma(2,:)) - log(sum(hp_posterior.gamma)),2)]; % 1 by K
  E_pi = exp([(log(hp_posterior.gamma(1,:)) ...
               - log(sum(hp_posterior.gamma,1)) ...
               + second_term(1:end-1)) ...
              , second_term(end)]); % 1 by K
elseif isequal(opts.algorithm, 'vdp')
  opts_tmp = opts;
  opts_tmp.algorithm = 'bj';
  E_pi = mk_E_pi(hp_posterior, hp_prior, opts_tmp);
  E_pi(end) = max(1 - sum(E_pi(1:end-1)), 0);
elseif isequal(opts.algorithm, 'csb')
  [N,K] = size(hp_posterior.q_of_z);
  a = 1 + hp_posterior.Nc; % 1 by K
  b = hp_prior.alpha + N - cumsum(hp_posterior.Nc, 2); % 1 by K
  first_term = a ./ (a+b);
  first_term(end) = 1;
  second_term = cumprod(b./(a+b)) ./ (b./(a+b));
  E_pi = first_term .* second_term;
else
  error('not supported algorithm')
end

%
% p_TSB(z_test|Z)
%
% \begin{align*}
% p_{\text{TSB}}(Z) =&
% \alpha^{T-1}
% \prod_{i<T}
% \frac
% {\Gamma(1+N_i) \Gamma(\alpha + N_{>i})}
% {\Gamma(1+\alpha+N_{\geq i})}
% \\
% p_{\text{TSB}}(z_{test}=t,Z) = &
% \left(
% \frac{1+N_t}{1+\alpha+N_{\geq t}}
% \right)^{\mathbb{I}(t<T)}
% \left\{
% \prod_{j<t}\frac
% {\alpha + N_{>j}}
% {1+\alpha+N_{\geq j}}
% \right\}
% \alpha^{T-1}
% \prod_{i<T}
% \frac
% {\Gamma(1+N_i) \Gamma(\alpha + N_{>i})}
% {\Gamma(1+\alpha+N_{\geq i})}
% \\
% p_{\text{TSB}}(z_{test}=t|Z) = &
% \left(
% \frac{1+N_t}{1+\alpha+N_{\geq t}}
% \right)^{\mathbb{I}(t<T)}
% \prod_{j<t}\frac
% {\alpha + N_{>j}}
% {1+\alpha+N_{\geq j}}
% \end{align*}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q_of_z = sequential_importance_sampling(data, hp_prior, opts);
disp('start SIS.')
N = size(data.given_data, 2);
for r = 1:opts.sis
  I = randperm(N);
%   I = I(1:ceil(N*0.1));
  data_r.given_data = data.given_data(:,I);
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   data_r_sub.given_data = data_r.given_data(:,1);
%   q_of_z = ones(1,opts.initial_K) / opts.initial_K;
%   hp_posterior = mk_hp_posterior(data_r_sub, q_of_z, hp_prior, opts);
%   for n = 1:N
%     data_r_sub.given_data = data_r.given_data(:,1:n);
%     q_of_z = mk_q_of_z(data_r_sub, hp_posterior, hp_prior, opts);
%     q_of_z = sort_q_of_z(data, q_of_z, opts);
%     hp_posterior = mk_hp_posterior(data_r_sub, q_of_z, hp_prior, opts);
%   end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  q_of_z = [];
  for n = 1:size(data_r.given_data,2)
    data_r_sub.given_data = data_r.given_data(:,1:n);
    q_of_z(n,:) = ones(1, opts.initial_K) / opts.initial_K;
    hp_posterior = mk_hp_posterior(data_r_sub, q_of_z, hp_prior, opts);
    q_of_z = mk_q_of_z(data_r_sub, hp_posterior, hp_prior, opts);
%     q_of_z = sort_q_of_z(data, q_of_z, opts);
  end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  hist(r).hp_posterior = hp_posterior;
  hist(r).free_energy = mk_free_energy(data, hp_posterior, hp_prior, opts);
  [min_f, best_r] = min([hist(:).free_energy]);
  hp_posterior = hist(best_r).hp_posterior;
  disp(['SIS: ' num2str(r) ';  best f = ' num2str(min_f) ';  best Nc = ' num2str(hp_posterior.Nc)])
end
disp('SIS done.')
q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function nonkdtree_q_of_z = mk_non_kdtree_q_of_z(data, q_of_z);
nonkdtree_q_of_z = zeros(size(data.given_data,2), size(q_of_z, 2)); % N*K
for a=1:length(data.kdtree);
  nonkdtree_q_of_z(data.kdtree(a).indices,:) = repmat(q_of_z(a,:), length(data.kdtree(a).indices), 1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data] = greedy(data, hp_posterior, hp_prior, opts);
free_energy = mk_free_energy(data, hp_posterior, hp_prior, opts);
disp_status(free_energy, hp_posterior, opts);
while 1
  disp('finding the best one....')
  [new_free_energy, new_hp_posterior, new_data, c] = find_best_splitting(data, ...
                                                    hp_posterior, ...
                                                    hp_prior, opts);
  if c == -1
    break
  end
  disp(['finding the best one.... done.  component ' num2str(c) ' was split.'])
  disp_status(new_free_energy, new_hp_posterior, opts);
  [new_free_energy, new_hp_posterior, new_data] = update_posterior2(new_data, ...
                                                    new_hp_posterior, ...
                                                    hp_prior, opts, 1, opts.ite);
  if free_energy_improved(free_energy, new_free_energy, 0, opts) == 0
    break
  end
  free_energy = new_free_energy;
  hp_posterior = new_hp_posterior;
  data = new_data;
end
disp_status(free_energy, hp_posterior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data, c] = find_best_splitting(data, ...
                                                  hp_posterior, ...
                                                  hp_prior, opts);
c_max = 10;
K = size(hp_posterior.m, 2);
candidates = find(hp_posterior.Nc>2);
if isempty(candidates)
  free_energy = 0;
  c = -1;
  return
end
q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts);
new_free_energy = ones(1,max(candidates))*inf;
%%%%%%%%%%%%%%%%%%%%%
fc = mk_E_log_q_p_eta(data, hp_posterior, hp_prior, opts);
log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts);
%%%%%%%%%%%%%%%%%%%%%
for c = candidates(1:min(c_max, length(candidates)))
  disp(['splitting ' num2str(c) '...'])
  [new_data(c), new_q_of_z, info] = split(c, data, q_of_z, hp_posterior, hp_prior, opts, 1);
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  new_c = info.new_c;
  relating_n = find(sum(new_q_of_z(:,[c new_c]),2) > 0.5);
  if isempty(relating_n)
    continue
  end
  new_K = size(new_q_of_z, 2);
  sub_q_of_z = new_q_of_z(relating_n, [c new_c new_K]);
  if opts.use_kd_tree
    sub_data.kdtree = new_data(c).kdtree(relating_n);
  else
    sub_data.given_data = new_data(c).given_data(:,relating_n);
  end
  sub_hp_posteior = mk_hp_posterior(sub_data, sub_q_of_z, hp_prior, opts);
  [sub_f, sub_hp_posteior, dummy, sub_q_of_z] = update_posterior2(sub_data, ...
                                                    sub_hp_posteior, ...
                                                    hp_prior, opts, 0, 10, 0);
  if size(sub_q_of_z,2) < 3
    continue
  end
  if length(find(sum(sub_q_of_z,1)<1.0e-10)) > 1
    continue
  end
  new_log_lambda = log_lambda;
  if opts.use_kd_tree
    updated_data.kdtree = new_data(c).kdtree(info.updated_I);
    new_log_lambda(info.updated_I,:) = mk_log_lambda(updated_data, hp_posterior, hp_prior, opts);
  end
  sub_log_lambda = mk_log_lambda(new_data(c), sub_hp_posteior, hp_prior, opts);
  insert_indices = [c new_c new_K:(new_K+size(sub_q_of_z,2)-3)];
  new_log_lambda(:,insert_indices) = sub_log_lambda;
  new_fc = fc;
  new_fc(insert_indices) = mk_E_log_q_p_eta(sub_data, sub_hp_posteior, hp_prior, opts);
  new_free_energy(c) = mk_free_energy(new_data(c), sub_hp_posteior, hp_prior, opts, new_fc, new_log_lambda);
  new_q_of_z(relating_n,:) = 0;
  new_q_of_z(relating_n,insert_indices) = sub_q_of_z;
  new_q_of_z_cell{c} = new_q_of_z;
end
[free_energy, c] = min(new_free_energy);
if isinf(free_energy)
  c = -1;
  return
end
data = new_data(c);
hp_posterior = mk_hp_posterior(data, new_q_of_z_cell{c}, hp_prior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data] = split_merge(data, hp_posterior, hp_prior, opts);
%
% split:
% tdp: if an empty cluster exists, split is available.
%      otherwise, make a new empty cluster. =>  K<-K+1.
%
% non-dp: if
%
c_max = 3;
[free_energy, hp_posterior] = update_posterior2(data, hp_posterior, hp_prior, opts, 0, opts.ite);
while 1
  K = size(hp_posterior.m, 2);
  new_free_energy = inf;
  %%% split
  if ~ opts.do_split
    break
  end
  disp(['### start splitting'])
  [dummy, candidates_for_splliting] = sort(hp_posterior.Nc, 2, 'descend');
  candidates_for_splliting(find(hp_posterior.Nc(candidates_for_splliting)<1)) = [];
  q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts); % N*K
  for c=candidates_for_splliting(1:min(c_max, length(candidates_for_splliting)))
    disp(['### start splitting k=' num2str(c) '...'])
    [new_data, new_q_of_z] = split(c, data, q_of_z, hp_posterior, hp_prior, opts);
    new_hp_posterior = mk_hp_posterior(new_data, new_q_of_z, hp_prior, opts);
    [new_free_energy, new_hp_posterior, new_data] = update_posterior2(new_data, ...
                                                      new_hp_posterior, ...
                                                      hp_prior, ...
                                                      opts, 1);
    if free_energy_improved(free_energy, new_free_energy, 0, opts)
      disp(['### splitting k=' num2str(c) ' improved the free energy.  ' ...
           num2str(free_energy) ' -> ' num2str(new_free_energy)])
      break
    else
      disp(['### splitting k=' num2str(c) ' did not improve the free energy.'])
    end
  end
  disp('### end splitting')
  if free_energy_improved(free_energy, new_free_energy, 0, opts)
    free_energy = new_free_energy;
    hp_posterior = new_hp_posterior;
    data = new_data;
    continue
  end
  if ~ opts.do_merge
    break
  end
  %%% merge
  disp(['### start merging'])
  q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts); % N*K
  candidates_for_merging = mkcandidates_to_merge(q_of_z'); % 2*#pairs
  for i=1:min(c_max, size(candidates_for_merging,2))
    pair = candidates_for_merging(:,i);
    c1 = pair(1); c2 = pair(2);
    disp(sprintf('### start merging c1=%d, c2=%d ...', c1, c2))
    new_q_of_z = q_of_z;
    new_q_of_z(:,c1) = new_q_of_z(:,c1) + new_q_of_z(:,c2);
    new_q_of_z(:,c2) = zeros(size(new_q_of_z,1),1);
    new_hp_posterior = mk_hp_posterior(data, new_q_of_z, hp_prior, opts);
    [new_free_energy, new_hp_posterior] = update_posterior2(data, ...
                                                      new_hp_posterior, ...
                                                      hp_prior, opts);
    if free_energy_improved(free_energy, new_free_energy, 0, opts)
      disp(sprintf('### merging c1=%d, c2=%d improved the free energy.  %0.5g -> %0.5g', ...
                   c1, c2, free_energy, new_free_energy))
      break
    else
      disp(sprintf('### merging c1=%d, c2=%d did not improve the free energy.', c1, c2))
    end
  end
  disp(['### end merging'])
  if free_energy_improved(free_energy, new_free_energy, 0, opts)
    free_energy = new_free_energy;
    hp_posterior = new_hp_posterior;
    continue
  end
  break
end % while 1
disp_status(free_energy, hp_posterior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_p_of_x_given_c = mk_map_log_p_of_x_given_c(data, clusters, hp_posterior, opts);
% clusters: e.g [1:K]
[D,N] = size(data);
K = length(clusters);
log_p_of_x_given_c = zeros(K,N);
for i = 1:K
  c = clusters(i);
  m = hp_posterior.m(:,c);
  precision = hp_posterior.inv_B(:,:,c)*hp_posterior.eta(c);
  d = data - repmat(m, 1, N);
  log_p_of_x_given_c(i,:) = (-D*0.5)*log(2*pi)+0.5*detln(precision)-0.5*sum(d.*(precision*d),1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [new_data, new_q_of_z, info] = split(c, data, q_of_z, ...
                                              hp_posterior, hp_prior, opts, update_kdtree)
% q_of_z: N*K
if nargin < 7
  update_kdtree = 1;
end
new_data = data;
if opts.use_kd_tree & update_kdtree
  [dummy, indices] = max(q_of_z,[],2);
  target_partitions = find(indices==c)';
  if ~ isempty(target_partitions)
    m = [data.kdtree(target_partitions).mean];
    log_p_of_m_given_c = mk_map_log_p_of_x_given_c(m, c, hp_posterior, opts); % 1*|target_partitions|
    [dummy, I] = sort(log_p_of_m_given_c, 2, 'descend');
    target_partitions = target_partitions(I(1:ceil(length(I)*opts.max_target_ratio)));
    %   target_partitions = find(q_of_z(:,c)>0.01)';
    [new_data, q_of_z, updated_I] = expand_all_nodes(new_data, q_of_z, hp_posterior, ...
                                                     hp_prior, target_partitions, opts);
    info.updated_I = updated_I;
  else
    info.updated_I = [];
  end
end
if isequal(opts.init_of_split, 'pc')  % principal eigenvector
  if opts.use_kd_tree
    arg1_data = [new_data.kdtree(:).mean];
  else
    arg1_data = new_data.given_data;
  end
  dir = divide_by_principal_component(arg1_data, ...
                                      hp_posterior.B{c}/hp_posterior.eta(c), ...
                                      hp_posterior.m(:,c));
  q_of_z_c1 = zeros(size(q_of_z,1),1);
  q_of_z_c2 = q_of_z(:,c);
  I = find(dir>=0);
  q_of_z_c1(I) = q_of_z(I,c);
  q_of_z_c2(I) = 0;
else
  q_of_z_c = q_of_z(:,c);
  if isequal(opts.init_of_split, 'rnd')  % random
    r = rand(size(q_of_z,1),1);
  elseif isequal(opts.init_of_split, 'rnd_close')  % make close clusters
    r = 0.5 + (rand(size(q_of_z,1),1)-0.5)*0.01;
  elseif isequal(opts.init_of_split, 'close_f')  % one is almost zero.
    r = 0.98 + rand(size(q_of_z,1),1)*0.01;
  else
    init_of_split = opts.init_of_split
    error('unknown option')
  end
  q_of_z_c1 = q_of_z_c.*r;
  q_of_z_c2 = q_of_z_c.*(1-r);
end
new_q_of_z = zeros(size(q_of_z,1), size(q_of_z,2)+1);
new_q_of_z(:,[1:end-2 end]) = q_of_z;
new_q_of_z(:,c) = q_of_z_c1;
new_c = size(new_q_of_z, 2) - 1;
new_q_of_z(:,new_c) = q_of_z_c2;
info.new_c = new_c;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [new_data, new_q_of_z, updated_I] = expand_all_nodes(data, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, ...
                                                  target_partitions, opts);
if size(target_partitions, 1) ~= 1
  error('target_partitions must be a row vector.')
end
new_data = data;
for a=target_partitions
  if isempty(data.kdtree(a).children)
    children = mk_children_of_partition(data.kdtree(a));
    data.kdtree(a).children = children;
  else
    children = data.kdtree(a).children;
  end
  if length(children) == 1
    continue
  end
  new_data.kdtree(a) = children(1);
  new_data.kdtree(end+1) = children(2);
end % while a <= length(data.kdtree)
updated_I = [target_partitions length(data.kdtree)+1:length(new_data.kdtree)];
sub_data.given_data = data.given_data;
sub_data.kdtree = new_data.kdtree(updated_I);
sub_q_of_z = mk_q_of_z(sub_data, hp_posterior, hp_prior, opts);
new_q_of_z = zeros(length(new_data.kdtree),size(q_of_z,2));
new_q_of_z(1:size(q_of_z,1),:) = q_of_z;
new_q_of_z(updated_I,:) = sub_q_of_z;
disp(['### building kd-tree done; #partition ' num2str(length(data.kdtree)) ...
      ' -> ' num2str(length(new_data.kdtree))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [data, q_of_z] = expand_recursively_until_convergence(data, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts);
kdtree_size = length(data.kdtree);
for a=1:length(data.kdtree)
  [data, q_of_z] = expand_recursively_until_convergence2(data, a, ...
                                                    q_of_z, hp_posterior, ...
                                                    hp_prior, opts, ...
                                                    opts.recursive_expanding_depth);
end
[prob, best_c] = max(q_of_z, [], 2);
% for c=1:size(q_of_z,2);
%   I_n =
% end
disp(['### building kd-tree done; #partition ' num2str(kdtree_size) ...
      ' -> ' num2str(length(data.kdtree))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [data, q_of_z] = expand_recursively_until_convergence2(data, a, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts, depth);
% q_of_z : N*K
if depth == 0
  return
end
if isempty(data.kdtree(a).children)
  children = mk_children_of_partition(data.kdtree(a));
  data.kdtree(a).children = children;
else
  children = data.kdtree(a).children;
end
if length(children) == 1
  return
end
sub_data.given_data = data.given_data;
sub_data.kdtree = children;
sub_q_of_z = mk_q_of_z(sub_data, hp_posterior, hp_prior, opts); % 2*K
diff = sub_q_of_z - repmat(q_of_z(a,:), 2, 1);
if sum(sum(diff.*diff, 1), 2)/prod(size(sub_q_of_z)) < opts.recursive_expanding_threshold
  return
end
b = length(data.kdtree)+1;
data.kdtree(a) = children(1);
data.kdtree(b) = children(2);
q_of_z([a b],:) = sub_q_of_z;
[data, q_of_z] =  expand_recursively_until_convergence2(data, a, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts, depth-1);
[data, q_of_z] =  expand_recursively_until_convergence2(data, b, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts, depth-1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data, q_of_z] = update_posterior2(data, hp_posterior, hp_prior, opts, upkdtree, ite, do_sort);
% update q_of_z: N*K
disp(['### updating posterior ...'])
free_energy = inf;
if nargin == 4
  upkdtree = 0;
end
if nargin < 6
  ite = inf;
end
if nargin < 7
  do_sort = 1;
end
i = 0;
last_Nc = 0;
start_sort = 0;
while 1
  i = i+1;
  [new_free_energy, log_lambda] = mk_free_energy(data, hp_posterior, hp_prior, opts);
  disp_status(new_free_energy, hp_posterior, opts);
  if (~isinf(ite) && i>=ite) || ...
        (isinf(ite) && free_energy_improved(free_energy, new_free_energy, 0, opts) == 0)
    free_energy = new_free_energy;
    if do_sort && opts.do_sort && ~ start_sort
      start_sort = 1;
    else
      break
    end
  end
  last_Nc = hp_posterior.Nc;
  free_energy = new_free_energy;
  [q_of_z, data] = mk_q_of_z(data, hp_posterior, hp_prior, opts, log_lambda);
  freq = opts.recursive_expanding_frequency;
  if opts.use_kd_tree & upkdtree & (freq==1 | mod(i,freq)==1)
    [data, q_of_z] = expand_recursively_until_convergence(data, ...
                                                      q_of_z, ...
                                                      hp_posterior, ...
                                                      hp_prior, opts);
  end
  % check if the last component is small enough
  if isequal(opts.algorithm, 'vdp') & sum(q_of_z(:,end)) > 1.0e-20
    q_of_z(:,end+1) = 0;
  end
  if start_sort
    q_of_z = sort_q_of_z(data, q_of_z, opts);
  end
  if isequal(opts.algorithm, 'vdp') & sum(q_of_z(:,end-1)) < 1.0e-10
    q_of_z(:,end-1) = [];
  end
  hp_posterior = mk_hp_posterior(data, q_of_z, hp_prior, opts);
end
% disp_status(free_energy, hp_posterior, opts);
disp(['### updating posterior ... done.'])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [q_of_z, I] = sort_q_of_z(data, q_of_z, opts);
disp('sorting...')
if opts.use_kd_tree
  Nc = [data.kdtree(:).N]*q_of_z; % 1*K
else
  Nc = sum(q_of_z, 1); % 1*K
end
if isequal(opts.algorithm, 'vdp')
  [dummy,I] = sort(Nc(1:end-1), 2, 'descend');
  I(end+1) = length(Nc);
else
  [dummy,I] = sort(Nc, 2, 'descend');
end
q_of_z = q_of_z(:,I);
disp('sorting... done.')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function disp_status(free_energy, hp_posterior, opts);
if isequal(opts.algorithm, 'vdp')
  Nc = hp_posterior.true_Nc;
else
  Nc = hp_posterior.Nc;
end
disp(['F=' num2str(free_energy) ...
      ';    Nc=[' num2str(Nc, ' %0.5g ') ...
      '];'])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function bool = free_energy_improved(free_energy, new_free_energy, warn_when_increasing, opts);
diff = new_free_energy - free_energy;
if abs(diff/free_energy) < opts.threshold
  bool = 0;
elseif diff > 0
  if warn_when_increasing
    if abs(diff/free_energy) > 1.0e-3
      error(['the free energy increased.  the diff is ' num2str(new_free_energy-free_energy)])
    else
      warning(['the free energy increased.  the diff is ' num2str(new_free_energy-free_energy)])
    end
  end
  bool = 0;
elseif diff == 0
  bool = 0
else
  bool = 1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function partitions = init_kdtree_partitions(given_data, depth);
[D,N] = size(given_data);
root = mk_kdtree_partition(given_data, [1:N]);
partitions = expand_recursively(root, depth);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function partitions = expand_recursively(partition, depth);
children = mk_children_of_partition(partition);
if depth == 1 | length(children) == 1
  partitions = children;
else
  partitions1 = expand_recursively(children(1), depth-1);
  partitions2 = expand_recursively(children(2), depth-1);
  partitions = [partitions1 partitions2];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function children = mk_children_of_partition(partition);
if partition.N == 1
  children = partition;
  return
end
dir = divide_by_principal_component(partition.given_data(:,partition.indices), ...
                                    partition.sigma, ...
                                    partition.mean);
positive_I = find(dir>=0);
negative_I = find(dir<0);
if length(positive_I) == 0 | length(negative_I) == 0
  children = partition;
  return
end
children(1) = mk_kdtree_partition(partition.given_data, partition.indices(positive_I));
children(2) = mk_kdtree_partition(partition.given_data, partition.indices(negative_I));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function direction = divide_by_principal_component(data, covariance, mean);
N = size(data, 2);
if size(data,1) <= 16
  [V,D] = eig(covariance);
  [eig_val, principal_component_i] = max(diag(D));
  principal_component = V(:,principal_component_i);
else
  [principal_component,eig_val] = power_method(covariance);
end
direction = sum((data - repmat(mean, 1, N)).*repmat(principal_component, 1, N), 1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function partition = mk_kdtree_partition(given_data, indices);
partition.N = length(indices);
if partition.N == 0
  error('indices must have at least one index.')
end
data_a = given_data(:,indices);
partition.given_data = given_data;
partition.indices = indices;
partition.sum_x = sum(data_a, 2); % D*1
mean = partition.sum_x / partition.N; % D*1
partition.mean = mean;
partition.sum_xx = data_a*data_a'; % D*D
partition.sigma = partition.sum_xx / partition.N - mean*mean';
partition.children = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q_of_z = rand_q_of_z(data, K, opts);
% q_of_z: N*K
if opts.use_kd_tree
  N = length(data.kdtree);
else
  N = size(data.given_data, 2);
end
if isequal(opts.algorithm, 'vdp')
  q_of_z = zeros(N, K+1);
else
  q_of_z = zeros(N, K);
end
q_of_z(:,1:K) = rand(N, K);
q_of_z = normalize(q_of_z, 2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function hp_posterior = mk_hp_posterior(data, q_of_z, hp_prior, opts);
% the last component of q_of_z represents the infinite rest of components
% the last component is the prior.
% q_of_z: N*K
% q_of_z(:,end) is the rest of responsibilities.
threshold_for_N = 1.0e-200;
K = size(q_of_z, 2);
if opts.use_kd_tree
  N = length(data.kdtree);
  D = size(data.kdtree(1).sum_x, 1);
  Na = [data.kdtree(:).N];
  if isequal(opts.algorithm, 'vdp')
    true_Nc = Na*q_of_z; % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = Na*q_of_z; % 1*K
  sum_x = [data.kdtree(:).sum_x] * q_of_z;
else
  [D,N] = size(data.given_data);
  if isequal(opts.algorithm, 'vdp')
    true_Nc = sum(q_of_z, 1); % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = sum(q_of_z, 1); % 1*K
  sum_x = data.given_data * q_of_z;
end
I = find(Nc>threshold_for_N);
inv_Nc = zeros(1,K);
inv_Nc(I) = 1./Nc(I);
hp_posterior.eta = hp_prior.eta0 + Nc;
hp_posterior.xi = hp_prior.xi0 + Nc;
means = sum_x .* repmat(inv_Nc, D, 1); % D*K
hp_posterior.inv_B = zeros(D,D,K);
if opts.use_kd_tree
  t1 = reshape([data.kdtree(:).sum_xx],D,D,N);
  for c=1:K
    v0 = means(:,c) - hp_prior.m0;
    q_of_z_c = reshape(q_of_z(:,c), 1, 1, N);
    S = sum(repmat(q_of_z_c,[D,D,1]).*t1, 3) - Nc(c)*means(:,c)*means(:,c)';
    hp_posterior.B{c} = hp_prior.B0 ...
        + S ...
        + Nc(c)*hp_prior.xi0*v0*v0'/(hp_posterior.xi(c)); % D*D
  end
else
  if opts.collapsed_means
    quad_term = mk_quad_term(data, q_of_z, hp_prior, opts);
  end
  for c=1:K
    if opts.collapsed_means
      hp_posterior.B{c} = hp_prior.B0 ...
          + (repmat(q_of_z(:,c),1,D)'.*data.given_data)*data.given_data' ...
          + quad_term(:,:,c);
    else
      v = data.given_data - repmat(means(:,c),1,N); % D*N
      v0 = means(:,c) - hp_prior.m0;
      hp_posterior.B{c} = hp_prior.B0 ...
          + (repmat(q_of_z(:,c),1,D)'.*v)*v' ...
          + Nc(c)*hp_prior.xi0*v0*v0'/(hp_posterior.xi(c)); % D*D
    end
  end
end
for c=1:K
  hp_posterior.inv_B(:,:,c) = inv(hp_posterior.B{c});
end
hp_posterior.m = (sum_x + repmat(hp_prior.xi0*hp_prior.m0,1,K)) ...
    ./ repmat(Nc+hp_prior.xi0,D,1);
if isequal(opts.algorithm, 'vdp')
  % gamma: 2*K
  hp_posterior.gamma = zeros(2,K);
  hp_posterior.gamma(1,:) = 1 + true_Nc;
  hp_posterior.gamma(2,:) = hp_prior.alpha + sum(true_Nc) - cumsum(true_Nc,2);
elseif isequal(opts.algorithm, 'bj')
  hp_posterior.gamma = zeros(2,K-1);
  hp_posterior.gamma(1,:) = 1 + Nc(1:K-1);
  hp_posterior.gamma(2,:) = hp_prior.alpha + sum(Nc) - cumsum(Nc(1:K-1),2);
elseif isequal(opts.algorithm, 'non_dp') | isequal(opts.algorithm, 'cdp')
  hp_posterior.tilde_alpha = hp_prior.alpha/K + Nc;
elseif isequal(opts.algorithm, 'csb')
  1;
else
  error('unknown algorithm')
end

hp_posterior.Nc = Nc;
if isequal(opts.algorithm, 'vdp')
  hp_posterior.true_Nc = true_Nc;
end
hp_posterior.q_of_z = q_of_z; % q_of_z is a N by K matrix where N is
                              % #given_data or #nodes in a kd-tree.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function fc = mk_E_log_q_p_eta(data, hp_posterior, hp_prior, opts);
% returns E[ log q(eta)/p(eta) ]_q
% fc : 1 by K
D = size(hp_posterior.m, 1);
K = size(hp_posterior.eta, 2);
log_det_B = zeros(1,K);
for c=1:K
  log_det_B(c) = detln(hp_posterior.B{c});
  d = hp_posterior.m(:,c)-hp_prior.m0; % D*1
  term_eta(1,c) = sum(sum(hp_posterior.inv_B(:,:,c).*(hp_prior.xi0*d*d'),1),2);
  term_eta(2,c) = sum(sum(hp_posterior.inv_B(:,:,c).*hp_prior.B0,1),2) - D;
end
E_log_q_p_mean = ...
    + 0.5*D*(hp_prior.xi0./hp_posterior.xi ...
             - log(hp_prior.xi0./hp_posterior.xi) ...
             - 1) ...
    + 0.5*(hp_posterior.eta).* term_eta(1,:);

psi_sum = sum(psi( (repmat(hp_posterior.eta+1,D,1) - repmat([1:D]',1,K))*0.5 ), 1); % 1*K
E_log_q_p_cov = ...
    0.5*hp_prior.eta0*(log_det_B-detln(hp_prior.B0)) ...
    + 0.5*hp_posterior.Nc.*psi_sum ...
    + 0.5*(hp_posterior.eta).* term_eta(2,:) ...
    + gamma_multivariate_ln(hp_prior.eta0*0.5,D) ...
    - gamma_multivariate_ln(hp_posterior.eta*0.5,D);

%debug
if length(find(E_log_q_p_mean<-1.0e-8,1)) > 0
  E_log_q_p_mean
  error('E_log_q_p_mean is negative.')
end
if length(find(E_log_q_p_cov<-1.0e-8,1)) > 0
  E_log_q_p_cov
  error('E_log_q_p_mean is negative.')
end

fc = E_log_q_p_mean + E_log_q_p_cov;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, log_lambda] = mk_free_energy(data, hp_posterior, ...
                                                  hp_prior, opts, ...
                                                  fc, log_lambda);
if nargin == 4
  fc = mk_E_log_q_p_eta(data, hp_posterior, hp_prior, opts); % 1*K
  log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts); % N*K
end
[N,K] = size(log_lambda);
if isequal(opts.algorithm, 'vdp') || isequal(opts.algorithm, 'bj')
  % note when bj,  if full hp_posterior is given, len_gamma = K - 1.
  len_gamma = size(hp_posterior.gamma, 2);
  if isequal(opts.algorithm, 'bj') && len_gamma ~= K - 1
    error('invalid length')
  end
  E_log_p_of_V = ...
      gammaln(sum(hp_posterior.gamma, 1)) ...
      - gammaln(1+hp_prior.alpha) ...
      - sum(gammaln(hp_posterior.gamma), 1) ...
      + gammaln(hp_prior.alpha) ...
      + ((hp_posterior.gamma(1,:)-1) ...
         .*(psi(hp_posterior.gamma(1,:))-psi(sum(hp_posterior.gamma,1)))) ...
      + ((hp_posterior.gamma(2,:)-hp_prior.alpha) ...
         .*(psi(hp_posterior.gamma(2,:))-psi(sum(hp_posterior.gamma,1))));
  extra_term = sum(E_log_p_of_V);
elseif isequal(opts.algorithm, 'non_dp')
  E_log_p_of_pi = ...
      sum(gammaln(hp_prior.alpha/K) ...
          - gammaln(hp_posterior.tilde_alpha) ...
          + hp_posterior.Nc.*(psi(hp_posterior.tilde_alpha) ...
                              - psi(sum(hp_posterior.tilde_alpha)))) ...
      + gammaln(sum(hp_prior.alpha+size(data,2))) - gammaln(hp_prior.alpha);
  extra_term = E_log_p_of_pi;
elseif isequal(opts.algorithm, 'cdp') || isequal(opts.algorithm, 'csb')
  E_log_p_of_z_given_other_z = ...
      mk_E_log_p_of_z_given_other_z(hp_posterior, hp_prior, opts); % N by K
  q_of_z = mk_q_of_z(data,hp_posterior,hp_prior,opts,log_lambda);
%   q_of_z = hp_posterior.q_of_z;
  E_Nc = hp_posterior.Nc;
  V_Nc = sum(q_of_z.*(1-q_of_z), 1); % 1 by K
  if isequal(opts.algorithm, 'cdp')
    E_log_p_of_z = gammaln(hp_prior.alpha) - gammaln(N+hp_prior.alpha) ...
        + sum(gammaln(hp_prior.alpha/K + E_Nc) ...
              - 0.5*psi(1, hp_prior.alpha/K + E_Nc).*V_Nc - gammaln(hp_prior.alpha/K));
  else % csb
    E_Nc_geq_to_i = cumsum(E_Nc, 2);
    q_of_z_geq_to_i = cumsum(q_of_z, 2);
    V_Nc_geq_to_i = sum(q_of_z_geq_to_i.*(1-q_of_z_geq_to_i), 1);

    E_Nc_greater_than_i = cumsum(E_Nc, 2) - E_Nc;
    q_of_z_greater_than_i = q_of_z_geq_to_i - q_of_z;
    V_Nc_greater_than_i = sum(q_of_z_greater_than_i.*(1-q_of_z_greater_than_i), 1);

    tmp = gammaln(1 + E_Nc) - 0.5*psi(1, 1 + E_Nc).*V_Nc ...  % E[log p(1+Nc)]
          + (gammaln(1 + E_Nc_greater_than_i) ...             % E[log p(1+N_{>c})]
             - 0.5*psi(1, 1 + E_Nc_greater_than_i).*V_Nc_greater_than_i) ...
          - (gammaln(1 + hp_prior.alpha + E_Nc_geq_to_i) ...  % E[log p(1+alpha+N_{>=c})]
             - 0.5*psi(1, 1 + hp_prior.alpha + E_Nc_geq_to_i).*V_Nc_geq_to_i);
    E_log_p_of_z = sum(tmp(1:end-1));
  end
  extra_term = sum(sum(E_log_p_of_z_given_other_z.*q_of_z, 2), 1) - E_log_p_of_z;
else
  error('unknown algorithm')
end
if opts.use_kd_tree
  free_energy = extra_term + sum(fc) - [data.kdtree(:).N]*log_sum_exp(log_lambda, 2);
else
  free_energy = extra_term + sum(fc) - sum(log_sum_exp(log_lambda, 2), 1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts);
% log_lambda: N*K
% q(z_n=c|x_n) = lambda_n_c / sum_c lambda_n_c

if isequal(opts.algorithm, 'vdp')
  if abs(hp_posterior.gamma(2,end) - hp_prior.alpha) > 1.0e-5
    hp_posterior.gamma(2,end)
    hp_prior.alpha
    diff = hp_prior.alpha - hp_posterior.gamma(2,end)
    error('must be alpha')
  end
end

if opts.use_kd_tree
  N = length(data.kdtree);
  D = size(data.kdtree(1).sum_x, 1);
else
  [D,N] = size(data.given_data);
end
K = size(hp_posterior.eta, 2);

psi_sum = sum(psi( (repmat(hp_posterior.eta+1,D,1) - repmat([1:D]',1,K))*0.5 ), 1); % 1*K
log_lambda = zeros(N,K);
if opts.use_kd_tree
  t1 = reshape([data.kdtree(:).sum_xx],D,D,N);
  sum_x = repmat(reshape([data.kdtree(:).sum_x],D,1,N),[1,D,1]);
  Na = repmat(reshape([data.kdtree(:).N],1,1,N),[D,D,1]);
end
if isequal(opts.algorithm, 'csb') | isequal(opts.algorithm, 'cdp')
  E_log_p_of_z_given_other_z = mk_E_log_p_of_z_given_other_z(hp_posterior, hp_prior, opts);
end
for c=1:K
%   Precision = 0.5*hp_posterior.inv_B(:,:,c)*hp_posterior.eta(c);
%   if isequal(opts.algorithm, 'vdp')
%     constant = psi(hp_posterior.gamma(1,c)) - psi(sum(hp_posterior.gamma(:,c),1)) ...
%         + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
%   elseif isequal(opts.algorithm, 'bj')
%     if c < K
%       constant = psi(hp_posterior.gamma(1,c)) - psi(sum(hp_posterior.gamma(:,c),1)) ...
%           + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
%     else
%       constant = sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
%     end
%   elseif isequal(opts.algorithm, 'non_dp')
%     % E[log pi] ; pi is the weight of mixtures.
%     constant = psi(hp_posterior.tilde_alpha(c)) - psi(sum(hp_posterior.tilde_alpha));
%   elseif isequal(opts.algorithm, 'cdp')
%     constant = 0;
%   elseif isequal(opts.algorithm, 'csb')
%     constant = 0;
%   else
%     error('unknown algorithm')
%   end
%   constant = constant - 0.5*D*log(pi) - 0.5*detln(hp_posterior.B{c}) ...
%       + 0.5*psi_sum(c) - 0.5*D/(hp_posterior.xi(c));
%   if opts.use_kd_tree
%     t2 = sum_x.*repmat(hp_posterior.m(:,c)',[D,1,N]);
%     term_dependent_on_n = (t1 - t2 - permute(t2,[2,1,3]))./Na + ...
%         repmat(hp_posterior.m(:,c)*hp_posterior.m(:,c)',[1,1,N]);
%     column_c = - sum(sum(repmat(Precision,[1,1,N]).*term_dependent_on_n,2),1) + constant;
%   else
%     d = data.given_data - repmat(hp_posterior.m(:,c),1,N);
%     column_c = - sum(d.*(Precision*d),1) + constant; % 1*N
%   end
%   if isequal(opts.algorithm, 'csb') | isequal(opts.algorithm, 'cdp')
%     column_c = column_c + E_log_p_of_z_given_other_z(:,c)';
%   end
%   log_lambda(:,c) = column_c;
  if isequal(opts.algorithm, 'vdp')
    E_log_p_of_z_given_other_z_c = ...
        psi(hp_posterior.gamma(1,c)) ...
        - psi(sum(hp_posterior.gamma(:,c),1)) ...
        + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
  elseif isequal(opts.algorithm, 'bj')
    if c < K
      E_log_p_of_z_given_other_z_c = ...
          psi(hp_posterior.gamma(1,c)) ...
          - psi(sum(hp_posterior.gamma(:,c),1)) ...
          + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
    else
      E_log_p_of_z_given_other_z_c = sum(psi(hp_posterior.gamma(2,[1:c-1])) ...
                                         - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
    end
  elseif isequal(opts.algorithm, 'non_dp')
    % E[log pi] ; pi is the weight of mixtures.
    E_log_p_of_z_given_other_z_c = psi(hp_posterior.tilde_alpha(c)) ...
        - psi(sum(hp_posterior.tilde_alpha));
  elseif isequal(opts.algorithm, 'csb') | isequal(opts.algorithm, 'cdp')
    E_log_p_of_z_given_other_z_c = E_log_p_of_z_given_other_z(:,c)';
  else
    error('unknown algorithm')
  end

  Precision = 0.5*hp_posterior.inv_B(:,:,c)*hp_posterior.eta(c);
  E_log_p_of_x = - 0.5*D*log(pi) - 0.5*detln(hp_posterior.B{c}) ...
      + 0.5*psi_sum(c) - 0.5*D/(hp_posterior.xi(c));
  if opts.use_kd_tree
    t2 = sum_x.*repmat(hp_posterior.m(:,c)',[D,1,N]);
    term_dependent_on_n = (t1 - t2 - permute(t2,[2,1,3]))./Na + ...
        repmat(hp_posterior.m(:,c)*hp_posterior.m(:,c)',[1,1,N]);
    E_log_p_of_x = - sum(sum(repmat(Precision,[1,1,N]).*term_dependent_on_n,2),1) + E_log_p_of_x;
  else
    d = data.given_data - repmat(hp_posterior.m(:,c),1,N);
    E_log_p_of_x = - sum(d.*(Precision*d),1) + E_log_p_of_x; % 1*N
  end
  log_lambda(:,c) = E_log_p_of_x + E_log_p_of_z_given_other_z_c;
end
if isequal(opts.algorithm, 'vdp')
  log_lambda(:,end) = log_lambda(:,end) - log(1- exp(psi(hp_prior.alpha) - psi(1+hp_prior.alpha)));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [q_of_z, data, log_lambda] = mk_q_of_z(data, hp_posterior, hp_prior, opts, log_lambda);
% q_of_z: N*K
if nargin == 4
  log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts);
end
q_of_z = exp(normalizeln(log_lambda, 1));
if opts.weight ~= 1
  q_of_z = q_of_z .* repmat(opts.weight, 1, size(q_of_z,2));
else
  q_of_z = q_of_z;
end

function M =normalizeln(M ,dimension);
M = lpt2lcpt(M, dimension);

function lcpt=lpt2lcpt(lpt,dimension);
% function lcpt=lpt2lcpt(lpt,dimension);
%
% make a log conditional probability table with log probability table
%
% lpt is a matrix.
% dimension must be either 1 or 2.
%
% lcpt = log( exp(lpt) / sum(exp(lpt)) )
%      = lpt - log(sum(exp(lpt)))
%

the_other_dimension=-(dimension-1.5)+1.5;
lpt=permute(lpt,[dimension,the_other_dimension]);
% now we can calculate as if dimension=1.

log_sum_exp_lpt = log_sum_exp(lpt,2); % Mx1
lcpt = lpt - repmat(log_sum_exp_lpt,1,size(lpt,2));

lcpt=permute(lcpt,[dimension,the_other_dimension]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function hp_prior = mk_hp_prior(data, opts)
if isfield(opts, 'xi0')
  hp_prior.xi0 = opts.xi0;
else
  hp_prior.xi0 = 0.01;
end
if isfield(opts, 'eta_p')
  eta_p = opts.eta_p;
else
  eta_p = 1;
end
if isfield(opts, 'alpha')
  hp_prior.alpha = opts.alpha;
else
  hp_prior.alpha = 1;
end
[D,N] = size(data.given_data);
if opts.use_kd_tree
  sum_xx = sum(reshape([data.kdtree(:).sum_xx],D,D,length(data.kdtree)),3);
  sum_x = sum([data.kdtree(:).sum_x],2);
  covariance = sum_xx/N - sum_x*sum_x'/(N*N);
else
  covariance = cov(data.given_data');
end
hp_prior.m0 = mean(data.given_data, 2);
if D > 16
  [dummy, max_eig] = power_method(covariance);
else
  max_eig = max(eig(covariance));
end
hp_prior.eta0 = eta_p * D + 1;
hp_prior.B0 = hp_prior.eta0 * max_eig * eye(D) * hp_prior.xi0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [vec,value]=power_method(A, start, precision)

switch nargin
 case 1
  start = ones(length(A),1);
  precision = 1e-10;
 case 2
  precision = 1e-10;
 case 3
 otherwise
  error 'invalid number of arguments'
end

%
% xp = A^p * x
% x(p+1) = lambda * xp   when p is big enough
% lambda = x(p+1)'*xp / xp'*xp
%
diff=precision+1;
x=start;
n=norm(x)+diff;
i = 0;
while diff> precision
  i = i + 1;
  y=A*x;
  n2 = norm(x);
  diff=abs(n2-n);
  n=n2;
  if n < 1.0e-200
    x = zeros(length(A), 1);
    break
  else
    x=y/n;
  end
  if i > 100
    break
  end
end
n = norm(x);
if n < 1.0e-200
  vec = zeros(length(A), 1);
else
  vec = x/n;
end
value=n;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function m=normalize(m,dim)
% return m normalized with 'dimension'
%
% e.g.
% m : i by j by k by ...
% m = sum(normalize(m,2), 2);
% m(i, :, k, ...) = ones(1, J, 1, ...)
%

dims = ones(1, ndims(m));
dims(dim) = size(m, dim);
m = m ./ repmat(sum(m, dim), dims);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Y] = detln( X )
% Y = logdet( X )
% return a log determinant of X
[d err] = chol(X);
if err
  error('error in Choleski disposition for detln');
end
Y = sum(log(diag(d))) *2;

% Local Variables: ***
% mode: matlab ***
% End: ***


function y = kron(X,Y)
%KRON (overloaded)

if isequal(X,1)
    y = Y;
    return;
end

if isequal(Y,1);
    y = X;
end

if (isa(X,'sdpvar') & isa(Y,'sdpvar'))
    y = [];
    s.type = '()';
    for i = 1:size(X,1)
        this_row = [];
        for j = 1:size(X,2);
            s.subs = {i,j};
            this_row = [this_row subsref(X,s)*Y];
        end
        y = [y;this_row];
    end
    return
end

if isa(X,'sdpvar')
    lmi_variables = getvariables(X);
    nv = length(lmi_variables);
    y  = X;
    sparse_Y = sparse(Y);
    % This one used also for checking size
    temp = kron(getbasematrix(X,0),sparse_Y);
    if size(Y,2)==1
        % Special case
        % [kron([N1(:) N2(:)],vec(M))]=[vec(kron(N1,M)) vec(kron(N2,M))]
        y.basis = kron(X.basis,sparse_Y);
    else
        y.basis=temp(:);
        X_base = X.basis;
        for i = 1:nv
           % temp = kron(getbasematrix(X,lmi_variables(i)),sparse_Y);
            temp = kron(reshape(X_base(:,i+1),X.dim),sparse_Y);
            y.basis(:,i+1) = temp(:);
        end;
    end
end

if isa(Y,'sdpvar')
    lmi_variables = getvariables(Y);
    nv = length(lmi_variables);
    y  = Y;
    sparse_X = sparse(X);
    % This one used also for checking size
    temp = kron(sparse_X,getbasematrix(Y,0));
    if size(X,1)==1
        % In this special case
        %[kron(vec(M),[N1(:) N2(:)])]==[vec(kron(M,N1)) vec(kron(M,N2))]
        y.basis = kron(sparse_X(:),Y.basis);
    else
        y.basis = temp(:);
        y.basis = spalloc(length(temp(:)),nv+1,0);
        y.basis(:,1) = temp(:);
        Y_base = Y.basis;
        for i = 1:nv
            % temp = kron(sparse_X,getbasematrix(Y,lmi_variables(i)));
            temp = kron(sparse_X,reshape(Y_base(:,i+1),Y.dim));
            y.basis(:,i+1) = temp(:);
        end
    end
end
y.dim(1) = size(temp,1);
y.dim(2) = size(temp,2);
y = clean(y);
% Reset info about conic terms
if isa(y,'sdpvar')
    y.conicinfo = [0 0];
end


% sim_spinecho_xN.m
% Robin Simpson and Jamie Near, 2014.
%
% USAGE:
% out = sim_spinecho(n,sw,Bfield,linewidth,sys,tau,Nechoes)
%
% DESCRIPTION:
% This function simulates a multi-echo spin-echo experiment with 'Nechoes' instantaneous RF pulses.
%
% INPUTS:
% n         = number of points in fid/spectrum
% sw        = desired spectral width in [Hz]
% Bfield    = main magnetic field strength in [T]
% linewidth = linewidth in [Hz]
% sys       = spin system definition structure
% tau       = echo time in [ms]
% Nechoes   = number of spin echoes (optional.  Default = 10);
%
% OUTPUTS:
% out       = simulated spectrum, in FID-A structure format, using multi-echo
%             spin-echo sequence.

function out = sim_spinecho_xN(n,sw,Bfield,linewidth,sys,tau,Nechoes)

if nargin<7
    Nechoes=10;
end

%Set water to centre
centreFreq=4.65;
for k=1:length(sys)
    sys(k).shifts=sys(k).shifts-centreFreq;
end

%Calculate Hamiltonian matrices and starting density matrix.
[H,d]=sim_Hamiltonian(sys,Bfield);

delay=tau/(2*Nechoes);

%BEGIN PULSE SEQUENCE************
d=sim_excite(d,H,'x');                            %EXCITE
for k=1:Nechoes
    d=sim_evolve(d,H,delay/1000);                        %Evolve by tau/2
    d=sim_rotate(d,H,180,'y');                       %180 degree refocusing pulse about y' axis.
    d=sim_evolve(d,H,delay/1000);                        %Evolve by tau/2
end
[out,dout]=sim_readout(d,H,n,sw,linewidth,90);  %Readout along y (90 degree phase);
%END PULSE SEQUENCE**************

%Correct the ppm scale:
out.ppm=out.ppm-(4.65-centreFreq);

%Fill in structure header fields:
out.seq=['spinecho_x' num2str(Nechoes)];
out.te=tau;
out.sim='ideal';

%Additional fields for compatibility with FID-A processing tools.
out.sz=size(out.specs);
out.date=date;
out.dims.t=1;
out.dims.coils=0;
out.dims.averages=0;
out.dims.subSpecs=0;
out.dims.extras=0;
out.averages=1;
out.rawAverages=1;
out.subspecs=1;
out.rawSubspecs=1;
out.flags.writtentostruct=1;
out.flags.gotparams=1;
out.flags.leftshifted=0;
out.flags.filtered=0;
out.flags.zeropadded=0;
out.flags.freqcorrected=0;
out.flags.phasecorrected=0;
out.flags.averaged=1;
out.flags.addedrcvrs=1;
out.flags.subtracted=1;
out.flags.writtentotext=0;
out.flags.downsampled=0;
out.flags.isFourSteps=0;


%{
 * Copyright (C) 2020-2030, The Regents of The University of Michigan.
 * All rights reserved.
 * This software was developed in the Biped Lab (https://www.biped.solutions/)
 * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
 * be available under alternative licensing terms; contact the address above.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions 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.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 THE COPYRIGHT OWNER 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 INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * The views and conclusions contained in the software and documentation are those
 * of the authors and should not be interpreted as representing official policies,
 * either expressed or implied, of the Regents of The University of Michigan.
 *
 * AUTHOR: Bruce JK Huang (bjhuang[at]umich.edu)
 * WEBSITE: https://www.brucerobot.com/
%}

function difference = H_diff(H1, H2)
    difference = H1 \ H2;
    logR = logm(difference(1:3,1:3));
    v = [-logR(1,2) logR(1,3) -logR(2,3) difference(1:3,4)'];
    difference = norm(v);
end