function coords = combineCoords(coords1, coords2, action)
%
% coords = combineCoords(coords1, coords2, action)
%
% Combines coords1 and coords2, removing duplicates, used for
% example to merge ROI coordinates into one big ROI.
%
% Possible values of action:
%	'intersect' / 'and': Take only coordinates that lie in coords1 AND
%						 coords2.
%	'union' / 'or': Take coordinates that lie in coords1 OR coords2.
%
%	'xor': Take coordinates that only lie in EITHER coords1 or coords2,
%			but not both.
%	'a not b' / 'setdiff': Take coordinates that lie in coords1 BUT NOT
%			coords2.
%
% coords, coords1, and coords2: 3xN arrays of (y,x,z) coordinates
% dims is size of volume
%
% rmk 10/30/98
% djh, 2/2001, updated to use intersect(coords1,coords2,'rows')
% ras, 04/10/05, deals w/ empty coords
% ras, 03/28/07, added some comments, more flexibility in specifying the
% action.

% Matlab functions work on rows, not cols
coords1 = coords1';
coords2 = coords2';

% made a little more complex, in case we
% run into empty sets of coords:
if isempty(coords1)
	switch lower(action)
      case {'intersection' 'intersect' 'and'}
         coords = [];
      case {'union' 'or'}
         coords = coords2;
      case 'xor'
         coords = coords2;
      case {'a not b' 'anotb' 'setdiff'}
         coords = [];
	end
elseif isempty(coords2)
	switch lower(action)
      case {'intersection' 'intersect' 'and'}
         coords = [];
      case {'union' 'or'}
         coords = coords1;
      case 'xor'
         coords = coords1;
      case {'a not b' 'anotb' 'setdiff'}
         coords = coords1;
	end
else
	switch lower(action)
      case {'intersection' 'intersect' 'and'}
         coords = intersect(coords1, coords2, 'rows');
      case {'union' 'or'}
         coords = union(coords1, coords2,'rows');
      case 'xor'
         coords = setxor(coords1, coords2, 'rows');
      case {'a not b' 'anotb' 'setdiff'}
         coords = setdiff(coords1, coords2, 'rows');
	end
end

% Transpose back to 3xN
coords = coords';

return


%
% Usage:   A=mexOMPMask(X,D,B,param);
% or       [A path]=mexOMPMask(X,D,B,param);
%
% Name: mexOMPMask
%
% Description: mexOMPMask is a variant of mexOMP that allow using
%     a binary mask B
%
%     for all columns x of X, and columns beta of B, it computes a column
%         alpha of A by addressing
%         min_{alpha} ||alpha||_0  s.t  ||diag(beta)*(x-Dalpha)||_2^2
%                                                               <= eps*||beta||_0/m
%         or
%         min_{alpha} ||diag(beta)*(x-Dalpha)||_2^2  s.t. ||alpha||_0 <= L
%         or
%         min_{alpha} 0.5||diag(beta)*(x-Dalpha)||_2^2  + lambda||alpha||_0
%
%
% Inputs: X:  double m x n matrix   (input signals)
%            m is the signal size
%            n is the number of signals to decompose
%         D:  double m x p matrix   (dictionary)
%            p is the number of elements in the dictionary
%            All the columns of D should have unit-norm !
%         B:  boolean m x n matrix   (mask)
%               p is the number of elements in the dictionary
%         param: struct
%            param.L (optional, maximum number of elements in each decomposition,
%               min(m,p) by default)
%            param.eps (optional, threshold on the squared l2-norm of the residual,
%               0 by default
%            param.lambda (optional, penalty parameter, 0 by default
%            param.numThreads (optional, number of threads for exploiting
%            multi-core / multi-cpus. By default, it takes the value -1,
%            which automatically selects all the available CPUs/cores).
%
% Output: A: double sparse p x n matrix (output coefficients)
%         path (optional): double dense p x L matrix
%                                     (regularization path of the first signal)
%
% Note: this function admits a few experimental usages, which have not
%     been extensively tested:
%      - single precision setting (even though the output alpha is double
%        precision)
%      - Passing an int32 vector of length n to param.L provides
%        a different parameter L for each input signal x_i
%      - Passing a double vector of length n to param.eps and or param.lambda
%        provides a different parameter eps (or lambda) for each input signal x_i
%
% Author: Julien Mairal, 2010


function node_num = sphere_grid_q9_node_num ( nelemx, nelemy )

%*****************************************************************************80
%
%% SPHERE_GRID_Q9_NODE_NUM counts nodes in a Q9 sphere grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.
%
%    Output, integer NODE_NUM, the number of nodes in the grid.
%
  node_num = 4 * nelemx * nelemy - 2 * nelemx + 2;

  return
end


function y = log_normcdf( x )

%LOG_NORMCDF   Internal CVX version.

error(nargchk(1,1,nargin));
if ~isreal( x ),
    error( 'Argument must be real.' );
end

persistent a b nb ob
if isempty( a ),
    a =sqrt( [ 0.018102332171520
               0.011338501342044
               0.072727608432177
               0.184816581789135
               0.189354610912339
               0.023660365352785 ] );
    a = sparse(diag(a));
    b = [3 2.5 2 1 -1 -2]';
    nb = length(b);
    ob = ones(nb,1);
end

cx = cvx_isconstant( x );
sx = size(x);
nx = prod(sx);
y  = a * ( b * ones(1,nx) - ob * reshape( x, 1, nx ) );
if cx,
    y = cvx( sum( cvx_constant( max( y, 0 ) ) .^ 2 ) );
else
    y = sum_square_pos( y );
end
y = - reshape( y, sx );


function [NNpredBest, errors] = NNseq(Ytr, YtsOriginal, YtsMissing, windowSizes, startingPoint, replacePresent)

% NNSEQ Sequential NN
% COPYRIGHT: Andreas C. Damianou, 2012
% VARGPLVM

if nargin < 6 || isempty(replacePresent),   replacePresent = true;  end
if nargin < 5 || isempty(startingPoint),    startingPoint = 1;      end
if nargin < 4 || isempty(windowSizes),      windowSizes = 1:5;      end

errors=[];
indexMissing = find(isnan(YtsMissing(1,:)));
indexPresent = setdiff(1:size(YtsMissing,2), indexMissing);

fprintf('  Initialising')
indNN = [];
miniNN = [];
for i=1:size(YtsMissing,1)
    fprintf('.')
    % initialize the latent points using the nearest neighbour from the training data
    dst = dist2(YtsMissing(i,indexPresent), Ytr(:,indexPresent));
    [mind, mini(i)] = min(dst);
    [miniNN(i,:) indNN(i,:)] = sort(dst); %%% mini(i) == indNN(i,1);
end
fprintf('\n')

errInd = 1;
NNpredBest = [];
for ws=windowSizes % Default: 1:5 (ws 11 gives the best results)
    windowSize = ws;
    fprintf('# Testing for windowSize = %d...\n',ws)

    NNpred = Ytr(mini,:);
    NNpredDyn = [];
    mindNN = [];
    miniNN = [];
    dstNN = [];
    NNpredDyn(1,:) = NNpred(startingPoint,:);
    fprintf('  Predicting')
    % Predict for multiple candidates (as many as the window size) and
    % select the one which is more coherent with the up-to-now sequence.
    for i=2:size(YtsMissing,1)
        fprintf('.')
        candidate = [];
        for j=1:windowSize
            candidate(j,:) = Ytr(indNN(i,j),:);
            % If this is true, then the present dimensions will be replaced
            % by the given ones (as opposed to just returning the whole
            % training NN).
            if replacePresent
                candidate(j,indexPresent) = YtsMissing(i, indexPresent); %% OPTIONAL!!
            end
        end

        if i == 2 %%% OPTIONAL 2 !!!
            % Find the most coherent to the whole first training point (which we
            % know is smooth)
            dstNN = dist2(Ytr(indNN(i,1),:),candidate); %%% OPTIONAL 2 !!!
        else %%% OPTIONAL 2 !!!
            dstNN = dist2(NNpredDyn(i-1,:), candidate);
        end %%% OPTIONAL 2 !!!

        [mindNN, miniNN(i)] = min(dstNN);
        NNpredDyn(i,:) = candidate(miniNN(i),:);
    end
   fprintf('\n')
   errors(errInd)= ...
       sum(sum(abs(NNpredDyn(:,indexMissing) - YtsOriginal(:,indexMissing)) ))/prod(size(YtsOriginal(:,indexMissing)));

   if errors(errInd) == min(errors)
       NNpredBest = NNpredDyn;
   end
   errInd = errInd+1;
  % meanPose = repmat(mean(Ytr),size(YtsOriginal,1),1);

end


% Example_6b_GBAY_w_floodplain
% Continue on from Example_6_GBAY.m by building on
% a floodplain onto the mesh.

% Most mesh sizing functions can be enforced in a
% topographic elevation (-1*depth) range such as:
%
% sizing_parameter = [size_value1, z_min1, z_max1;
%                     size_value2, z_min2, z_max2;
%                    ];
% Note: multiple elevation ranges must be delineated with semi-colons ;

clearvars; clc;

addpath('..')
addpath(genpath('../utilities/'))
addpath(genpath('../datasets/'))
addpath(genpath('../m_map/'))

%% STEP 1: set mesh extents and set parameters for mesh.
min_el    = 60;  		     % Minimum mesh resolution in meters.
max_el    = [1e3,-inf,0 ;    % Underwater maximum mesh resolution in meters.
             500,0,+inf];    % Overland maximum mesh resolution in meters.
grade     = [0.25,-inf,0;    % Underwater gradation rate
             0.05,0,+inf] ;  % Overland gradation rate
angleOfReslope = 60 ;       % Control width of channel by changing angle of reslope.
ch = 0.1 ;                  % Scale resolution propotional to depth nearby thalweg.
fs = 3 ;                    % Place 3 vertices per width of shoreline feature.
%% STEP 2: specify geographical datasets and process the geographical data
%% to be used later with other OceanMesh classes...
coastline = 'us_medium_shoreline_polygon';
demfile   = 'galveston_13_mhw_2007.nc';

gdatuw = geodata('shp',coastline,...
                 'dem',demfile,...
                 'h0',min_el);

load ECGC_Thalwegs.mat % Load the Channel thalweg data
%% STEP 3: create an edge function class
fh = edgefx('geodata',gdatuw,...
            'fs',fs,...
            'ch',ch,...
            'AngOfRe',angleOfReslope,...% control the width
            'Channels',pts2,...
            'g',grade,...
            'max_el',max_el);
%% STEP 4: Pass your edgefx class object along with some meshing options and
% build the mesh...
mshopts = meshgen('ef',fh,'bou',gdatuw,'plot_on',1,'proj','lambert');
% now build the mesh with your options and the edge function.
mshopts = mshopts.build;
%% STEP 5: Get fixed constraints and update gdat with overland meshing domain.
muw = mshopts.grd ;
muw = makens(muw,'auto',gdatuw) ; % apply so that extractFixedConstraints only grabs the shoreline constraints.

[pfix,egfix] = extractFixedConstraints(muw) ;

% 10-m contour extracted from the Coastal Relief Model.
coastline = 'us_coastalreliefmodel_10mLMSL';
demfile   = 'galveston_13_mhw_2007.nc';
landuse   = 'galveston_2016_CCAP.nc';

gdat = geodata('shp',coastline,...
               'dem',demfile,...
               'h0',min_el);

%gdat.inpoly_flip =  mod(1,gdat.inpoly_flip) ; % if the meshing domain is inverted, you can always flip it .

% Here we pass our constraints to the mesh generator (pfix and egfix).
mshopts = meshgen('ef',fh,'bou',gdat,'plot_on',1,'proj','lambert',...
                  'pfix',pfix,'egfix',egfix);

% now build the mesh with your options and the edge function.
mshopts = mshopts.build;

m = mshopts.grd ;

% plot resolution on the mesh
plot(m,'type','resomesh','colormap',[10 0 1e3])

% interpolate bathy using special constraining technique
% for overland and underwater
m = interpFP(m,gdat,muw,gdatuw);
plot(m,'type','bmesh') % plot the bathy on the mesh

% computing mannings based on CCAP landcover data using
% cell-averaged interpolation with stencil 4*grid_size for stability
m = Calc_Mannings_Landcover(m,landuse,'ccap','N',4);
plot(m,'type','mann') % plot the mannings on the mesh


 function [sinit, bodycoil] = ir_mri_sensemap_init(ykj, varargin)
%function [sinit, bodycoil] = ir_mri_sensemap_init(ykj, varargin)
%|
%| Form initial sensitivity map estimates from surface coils and bodycoil.
%|
%| in
%| ykj [(N) ncoil] surface coil data (2D or 3D)
%|
%| option
%| 'type' initialization method (def: '' = 'ratio')
%|		options: 'median', 'order1', 'order2', ...
%|		if numeric, then simply return as "sinit"
%| 'bodycoil' [(N)] body coil data (def: SSoS of ykj with phase of 1st coil)
%| 'thresh' fraction of max |bodycoil| that is "good" (def: 0.05)
%| 'mask' [(N)] optional mask to pick good values instead of thresholding
%| 'chat' 0|1 verbosity
%|
%| out
%| sinit [(N) ncoil] initial sensitivity map estimates
%| bodycoil [(N)] computed SSoS bodycoil image (if not provided)
%|
%| used in ir_mri_sensemap_admm - see that m-file for details.
%|
%| Michael Allison
%| 2015-08-10 Jeff Fessler revise args, add tests, support 3D

if nargin < 1, ir_usage(), end
if streq(ykj, 'test', 4), ir_mri_sensemap_init_test(ykj), return, end

arg.type = '';
arg.thresh = 0.05;
arg.mask = [];
arg.bodycoil = [];
arg.chat = false;
arg = vararg_pair(arg, varargin);

bodycoil = arg.bodycoil;
if isempty(bodycoil)
	if ndims(ykj) < 3, fail('ykj must be 3D or 4D to infer bodycoil'), end
	ndim = ndims(ykj) - 1; % 2 or 3
	bodycoil = sqrt(sum(abs(ykj).^2, ndim+1)); % sum-of-squares
        tmp = stackpick(ykj, 1); % ykj(:,...,:,1)
        tmp = angle(tmp); % crude estimate of phase of image f_j
        bodycoil = bodycoil .* exp(1i * tmp);
else
	ndim = ndims(bodycoil);
	dim = size(bodycoil);
end

if ~isempty(arg.type) && isnumeric(arg.type) % trivial case
	sinit = arg.type;
return
end

dim = size(bodycoil);
ncoil = numel(ykj) / prod(dim);
if round(ncoil) ~= ncoil, fail('size'), end

if ~isempty(arg.mask)
	jf_equal(size(arg.mask), dim)
	if arg.chat
		warn('using mask instead of threshold to determine good pixels')
	end
	good = mask;
end

if arg.chat
	tell = @(s) printm(s);
else
	tell = @(s) nop(s);
end

sinit = zeros([prod(dim) ncoil], 'single');
for ic = 1:ncoil
	% cannot use stackpick because of case of just one coil
	if ndim == 2
		zj = ykj(:,:,ic);
	elseif ndim == 3
		zj = ykj(:,:,:,ic);
	end

	tmp = div0(zj, bodycoil); % usual ratio

	% determine "good" pixels
	if isempty(arg.mask)
		good = abs(bodycoil) > arg.thresh * max(abs(bodycoil(:)));
	end

	switch arg.type
	case {'', 'ratio'}
		% set all uncertain map values to median of good ones
		tell('Using zero background.')
		tmp(~good) = 0;

	case 'median'
		% set all uncertain map values to median of good ones
		tell('Using median background.')
		tmp1 = median(abs(tmp(good)));
		tmp2 = median(angle(tmp(good))); % dubious
		tmp(~good) = tmp1 .* exp(1i*tmp2);

	case 'avg'
		% set all uncertain map values to mean of good ones
		tell('Using mean background.')
		tmp1 = mean(abs(tmp(good)));
		tmp2 = mean(angle(tmp(good))); % dubious
		tmp(~good) = tmp1 .* exp(1i*tmp2);

	case 'zeros'
		tmp = zeros(dim);

	case 'order1'
		tell('Using 1st-order fit for background.')
		expo = [0 0; 1 0; 0 1];
		tmp = ortho_init(dim, expo, zj, bodycoil, good);

	case 'order2'
		tell('Using 2nd order fit for background.')
		expo = [0 0; 1 0; 0 1; 1 1; 2 0; 0 2];
		tmp = ortho_init(dim, expo, zj, bodycoil, good);

	case 'order3'
		tell('Using 3rd order fit for background.')
		expo = [0 0; 1 0; 0 1; 1 1; 2 0; 2 1; 0 2; 1 2; ...
			3 0; 0 3];
		tmp = ortho_init(dim, expo, zj, bodycoil, good);

	case 'order4'
		tell('Using 4th order fit for background.')
		expo = [0 0; 1 0; 0 1; 1 1; 2 0; 2 1; 0 2; 1 2; ...
			3 0; 0 3; 2 2; 3 1; 1 3; 4 0; 0 4];
		tmp = ortho_init(dim, expo, zj, bodycoil, good);

	otherwise
		fail('Unknown initialization type "%s"', init)
	end

	sinit(:,ic) = single(tmp(:));
end % ic
sinit = reshape(sinit, [dim ncoil]);

end %

function nop(varargin)
end % nop

% ortho_init()
% initialize using orthogonal polynomial basis fit
function init = ortho_init(dim, expo, zj, bodycoil, good)
switch numel(dim)
case 2
	%xx = ndgrid_jf('mat', 0:nx-1, 0:ny-1);
	A = cheby2d(dim(1), dim(2), expo);
case 3
	nx = dim(1);
	ny = dim(2);
	ny = dim(2);
	A = cheby3d(dim(1), dim(2), dim(3), expo);
otherwise
	fail('#dim = %d not done', numel(dim))
end

A = reshape(A, [prod(dim), size(expo,1)]);
maskBodyI = good .* bodycoil;
Aw = repmat(maskBodyI(:), [1 size(expo,1)]) .* A;
theta = pinv(Aw) * zj(:);
clear Aw
init = A * theta; % not Aw
init = reshape(init, size(bodycoil));

end % ortho_init()

% cheby2d()
function polys = cheby2d(nx,ny,expo)
nbases = length(expo);
x = linspace(-1,1,nx);
y = linspace(-1,1,ny);
polys = zeros(nx,ny,nbases);
for n = 1:nbases
	tmp = polyval(ir_chebyshev_poly(expo(n,1)),x);
	tmp = tmp';
	X = repmat(tmp,[1 ny]);
	tmp = polyval(ir_chebyshev_poly(expo(n,2)),y);
	Y = repmat(tmp,[nx 1]);
	polys(:,:,n) = X .* Y;
end

end % cheby2d()

% cheby3d()
function polys = cheby3d(nx,ny,expo)
nbases = size(expo,1);
x = linspace(-1,1,nx);
y = linspace(-1,1,ny);
z = linspace(-1,1,nz);
polys = zeros(nx, ny, nz, nbases, 'single');
for n = 1:nbases
	tmp = polyval(ir_chebyshev_poly(expo(n,1)), x);
	X = repmat(tmp', [1 ny]);
	tmp = polyval(ir_chebyshev_poly(expo(n,2)), y);
	Y = repmat(tmp, [nx 1]);
	tmp = polyval(ir_chebyshev_poly(expo(n,3)), z);
	Z = repmat(tmp, [nx 1]);
	polys(:,:,:,n) = X .* Y .* Z;
end

end % cheby3d()

% ir_mri_sensemap_init_test2()
% 2D
function ir_mri_sensemap_init_test2
nx = 28; ny = 32;
ig = image_geom('nx', nx, 'ny', ny, 'fov', 22);
tmp = ellipse_im(ig, 'shepplogan-emis', 'oversample', 2);
%tmp = ones(nx,ny);
xtrue = tmp .* exp(1i * (tmp-2));
im(abs(xtrue))
ncoil = 4;
strue = ir_mri_sensemap_sim('nx', ig.nx, 'ny', ig.ny, 'dx', ig.dx, ...
	'ncoil', ncoil);
scale = 1 / sqrt(sum(abs(strue(end/2,end/2,:).^2)));
strue = strue * scale; % normalize so SSoS=1 near center
rng(0)
ykj = strue .* repmat(xtrue, [1 1 ncoil]) ...
	+ 2^-8 * (randn(size(strue)) + 1i * randn(size(strue)));
im plc 5 2
clim = minmax(abs(strue))';
im(1, 'row', 1, abs(strue), clim, 'true'), cbar
im(2, 'row', 1, abs(ykj)), cbar
sinit1 = ir_mri_sensemap_init(ykj);
im(3, 'row', 1, abs(sinit1), clim), cbar
slist = {'ratio', 'median', 'avg', 'order1', 'order2', 'order3', 'order4', 'zeros'};
for is=1:numel(slist)
	stype = slist{is};
	sinit2 = ir_mri_sensemap_init(ykj, 'type', stype);
	im(2+is, 'row', 1, abs(sinit2), clim, stype), cbar
	drawnow
end

end % ir_mri_sensemap_init_test2()

% ir_mri_sensemap_init_test3()
% 3D
function ir_mri_sensemap_init_test3
nx = 28; ny = 32; nz=6;
ig = image_geom('nx', nx, 'ny', ny, 'nz', nz, 'fov', 22, 'zfov', 10);
tmp = ellipsoid_im(ig, 'shepp-logan-e3d', 'oversample', 2);
%tmp = ig.ones;
xtrue = tmp .* exp(1i * tmp); % arbitrary phase
%im(abs(xtrue))
nring = 3;
ncoil = 4 * nring;
strue = ir_mri_sensemap_sim('nx', ig.nx, 'ny', ig.ny, 'nz', ig.nz, ...
	'dx', ig.dx, 'nring', nring, 'ncoil', ncoil);
%im('row', 3, abs(strue))
scale = 1 / sqrt(sum(abs(strue(end/2,end/2,end/2,:).^2)));
strue = strue * scale; % normalize so SSoS=1 near center
rng(0)
ykj = strue .* repmat(xtrue, [1 1 1 ncoil]) ...
	+ 1 * 2^-12 * (randn(size(strue)) + 1i * randn(size(strue)));
im plc 4 1
clim = minmax(abs(strue))';
im(1, 'row', nring, abs(strue), clim, 'true'), cbar
titlef('True smaps for 3D [%d,%d,%d] with %d coils (%d rings of %d coils)', ...
	nx, ny, nz, ncoil, nring, ncoil/nring)
im(2, 'row', nring, abs(ykj), 'surface coils'), cbar
sinit1 = ir_mri_sensemap_init(ykj);
im(3, 'row', nring, abs(sinit1), clim, 'ratio'), cbar
slist = {'median'}; % 'avg', 'order1', 'order2', 'order3', 'order4', 'zeros'};
for is=1:numel(slist)
	stype = slist{is};
	sinit2 = ir_mri_sensemap_init(ykj, 'type', stype);
	im(3+is, 'row', nring, abs(sinit2), clim, stype), cbar
	drawnow
end
end % ir_mri_sensemap_init_test3()

% ir_mri_sensemap_init_test()
function ir_mri_sensemap_init_test(arg)
switch arg
case 'test2'
	ir_mri_sensemap_init_test2
case 'test3'
	ir_mri_sensemap_init_test3
case 'test'
	ir_mri_sensemap_init_test2
	if im, prompt, end
	ir_mri_sensemap_init_test3
otherwise
	fail('unknown test "%s"', arg)
end
end % ir_mri_sensemap_init_test()


function Stat=clusterstat(S,partition,between)
%function Stat=clusterstat(S,partition,[between])
%
%PURPOSE
%
%To compute various intra- and extra-cluster statistics.
%
%INPUT
%
%[An argument in brackets is optional. If it isn't  given or it's
% an empty matrix/string, the function will use a default value.]
%
% S         (matrix) NxN similarity or distance matrix between N
%             objects. Must be symmetric.
% partition (1xN vector) partition vector (see explanation for
%             'partition vector' in function hcluster)
% [between] (scalar) 1: compute between-cluster similarity/distance matrix,
%                    0: (default) don't compute
%
%OUTPUT
%
% All fields are vectors of size 1xK, where K is the number of clusters
%
% Stat.N(i)             the number of objects in cluster i
%
% Stat.internal.min(i)  minimum/average/max internal similarity/distance
% Stat.internal.avg(i)  in cluster i
% Stat.internal.max(i)
%  Note: if there is only one item in the cluster, the internal statistics
%  are set to NaN
%
% Stat.external.min(i)  minimum/average/max external similarity/distance from
% Stat.external.avg(i)  objects of cluster i to the objects of other clusters
% Stat.external.max(i)
%
%DETAILS
%
%Partition vector divides items  into clusters: partition(i) is
%the label  of cluster that item i belongs to. Cluster labels must
%be integers 1,2,...,K where K is the number of clusters.
%
%"Internal" for cluster k refers to all pairwise
%distances/similarities between objects belonging to the same
%cluster, i.e., for objects i for which partition(i)==k.
%
%"External" for cluster k refers to all pairwise
%distances/similarities between the members of the cluster k and
%members of the other clusters, i.e., the whole similarity matrix
%excluding the internal similarities of cluster k.
%
%If 'between' is set to 1 the function computes also the following
%KxK matrices
%
%Stat.between.min(i,j)
%Stat.between.avg(i,j)
%Stat.between.max(i,j)
%
%These include distances/similarities  between clusters i and j
%defined as min/average/max pairwise distances between objects
%belonging to clusters i and j, respectively.
%
%SEE ALSO
% clusterquality
% icassoStability
% icassoRindex
% rindex

%COPYRIGHT NOTICE
%This function is a part of Icasso software library
%Copyright (C) 2003-2005 Johan Himberg
%
%This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2
%of the License, or any later version.
%
%This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%GNU General Public License for more details.
%
%You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software
%Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

% ver 1.2. 100105

if nargin<2,
   error('You must give at least two input arguments.');
end

if nargin<3|isempty(between)
  between=0;
end

% Number of clusters
Ncluster=max(partition);

%Initialize the struct
Stat.internal.sum(1:Ncluster,1)=NaN;
Stat.internal.min(1:Ncluster,1)=NaN;
Stat.internal.avg(1:Ncluster,1)=NaN;
Stat.internal.max(1:Ncluster,1)=NaN;
Stat.external.sum(1:Ncluster,1)=NaN;
Stat.external.min(1:Ncluster,1)=NaN;
Stat.external.avg(1:Ncluster,1)=NaN;
Stat.external.max(1:Ncluster,1)=NaN;
Stat.index = cell(1,Ncluster);

for cluster=1:Ncluster,
  thisPartition=(partition==cluster);
  Stat.index{cluster} = find(thisPartition);
  S_=S(thisPartition,thisPartition);
  Stat.N(cluster)=size(S_,1);
  S_(eye(size(S_))==1)=[];
  if ~isempty(S_),
    Stat.internal.sum(cluster)=sum(S_);
    Stat.internal.min(cluster)=min(S_);
    Stat.internal.avg(cluster)=mean(S_);
    Stat.internal.max(cluster)=max(S_);
  end
  if Ncluster>1,
    S_=S(thisPartition,~thisPartition);
    Stat.external.sum(cluster)=sum(S_(:));
    Stat.external.min(cluster)=min(S_(:));
    Stat.external.avg(cluster)=mean(S_(:));
    Stat.external.max(cluster)=max(S_(:));
  end
end

if between,
  Stat.between.min=zeros(Ncluster,Ncluster);
  Stat.between.max=zeros(Ncluster,Ncluster);
  Stat.between.avg=zeros(Ncluster,Ncluster);

  for i=1:Ncluster,
    Pi=find(i==partition);
    for j=i+1:Ncluster,
      Pj=find(j==partition);
      d_=S(Pi,Pj);
      Stat.between.min(i,j)=min(d_(:));
      Stat.between.avg(i,j)=mean(d_(:));
      Stat.between.max(i,j)=max(d_(:));		;
    end
  end

  Stat.between.min=Stat.between.min+ ...
    Stat.between.min';
  Stat.between.max=Stat.between.max+ ...
      Stat.between.max';
  Stat.between.avg=Stat.between.avg+ ...
      Stat.between.avg';
end


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled
% 	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:  nickabattista[at]gmail.com
% IB2d Created: May 27th, 2015
% Institution: TCNJ
%
% 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 (nickabattista[at]gmail.co) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the beam attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function beams_info = update_nonInv_Beams(dt,current_time,beams_info)

% beams_info:   col 1: 1ST PT.
%               col 2: MIDDLE PT. (where force is exerted)
%               col 3: 3RD PT.
%               col 4: beam stiffness
%               col 5: x-curvature
%               col 6: y-curvature

%IDs = beams_info(:,1);   % Gives Middle Pt.

% Time initialization
t1 = 0.25;                          % Period A->B (down-stroke)
t2 = 0.25;                          % Period B->A (up-stroke)
tR = 0.0;                           % Resting time between A->B and B->A
period = (t1+t2+tR);                % Time it takes to move to the right
t = rem(current_time,period);       % Recalculate Time (using modular arithmetic)

% Cubic Interpolation Information
p1 = 0.10;
p2 = 0.90;

% a COEFFICIENTS
a0 = 0;
a1 = 0;
a2 = 0;
a3 = 11.111111111111088;

% b COEFFICIENTS
b0 = 0.013888888888889;
b1 = -0.416666666666666;
b2 = 4.166666666666658;
b3 = -2.777777777777770;

% c COEFFICIENTS
c0 =  -10.111111111111073;
c1 =  33.333333333333222;
c2 = -33.333333333333222;
c3 =  11.111111111111073;

% Read In y_Pts for two Phases!
[xP1,yP1,yP2] = read_File_In('swimmer.phases'); % NOTE xP1 = xP2
xP2 = xP1;

%
% FIRST WE COMPUTE THE INTERPOLATE GEOMETRY BETWEEN BOTH PHASES
%

%
% START THE INTERPOLATING BETWEEN STATES!
%
if t <= t1 % STATE A -> STATE B

    % Scaling time for appropriate use in interp. function so tTilde\in[0,1]
    tTilde = (t/t1);

    % Evaluate Pieceise Cubic Interpolation Poly
    if ( tTilde<=p1 )
        gFUNC = a0 + a1*tTilde + a2*tTilde^2 + a3*tTilde^3;
    elseif ( (tTilde>p1) && (tTilde<=p2) )
        gFUNC = b0 + b1*tTilde + b2*tTilde^2 + b3*tTilde^3;
    else
        gFUNC = c0 + c1*tTilde + c2*tTilde^2 + c3*tTilde^3;
    end

    %xPts = xP1 + gFUNC*( xP2 - xP1 );
    yPts = yP1 + gFUNC*( yP2 - yP1 );

elseif ( t >= t1+tR ) % STATE B -> A

    % Scaling time for appropriate use in interp. function so tTilde\in[0,1]
    tTilde = (t-t1-tR)/(t2);

    % Evaluate Pieceise Cubic Interpolation Poly
    if tTilde<=p1
        gFUNC = a0 + a1*tTilde + a2*tTilde^2 + a3*tTilde^3;
    elseif tTilde<=p2
        gFUNC = b0 + b1*tTilde + b2*tTilde^2 + b3*tTilde^3;
    else
        gFUNC = c0 + c1*tTilde + c2*tTilde^2 + c3*tTilde^3;
    end

    %xPts = xP2 + gFUNC*( xP1 - xP2 );
    yPts = yP2 + gFUNC*( yP1 - yP2 );

end

%
% NOW WE UPDATE THE CURAVTURES APPROPRIATELY
%
%beams_info(:,5) = xPts(1:end-2)+xPts(3:end)-2*xPts(2:end-1);
beams_info(:,6) = yPts(1:end-2)+yPts(3:end)-2*yPts(2:end-1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: reads in info from file
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x1,y1,y2] = read_File_In(file_name)

filename = file_name;  %Name of file to read in

fileID = fopen(filename);

    % Read in the file, use 'CollectOutput' to gather all similar data together
    % and 'CommentStyle' to to end and be able to skip lines in file.
    C = textscan(fileID,'%f %f %f','CollectOutput',1);

fclose(fileID);        %Close the data file.

mat_info = C{1};   %Stores all read in data

%Store all elements in matrix
mat = mat_info(1:end,1:end);

x1 =  mat(:,1); %store xVals1/2
y1 =  mat(:,2); %store yVals1
y2 =  mat(:,3); %store yVals2


function estimation_results = REKF_SLAM(data, NumberOfSteps)
% R-EKF SLAM

% load pre-given data: odometry and observations
if nargin < 1
    load('./data.mat');
end

data_matrix = data.state;

odom_sigma = data.odom_sigma;
obsv_sigma = data.obsv_sigma;

% odoCov = data.odom_cov;   % constant variable
% obsCov = data.obse_cov;   % constant variable

%%%%%%%%%%%%%%%%%%%% Estimation_X is used to save the state in each step %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%% In every step, all elements of Estimation_X will be changed %%%%%%%%%%%%
Estimation_X.orientation = data.poses.orientation(1:3,1:3);
Estimation_X.position    = data.poses.position(:,1);
Estimation_X.cov         = sparse(6,6);
Estimation_X.landmarks   = [];       % the landmarks observed until this step (included), 4*N format, the 4-th row is the index
Estimation_X0.IndexObservedNew=[];
Estimation_X0.IndexObservedAlreadyThis=[];
%%%%%%%%%%%%%%%%%%%% Estimation_X is used to save the state in each step %%%%%%%%%%%%%%%%%%%%

% Initialize
if nargin < 2
    NumberOfSteps = max(data_matrix(:,4));  % step instead of pose,  hence, it does not include pose 0
elseif NumberOfSteps > max(data_matrix(:,4))
    NumberOfSteps = max(data_matrix(:,4));
end
estimation_results = cell(1, NumberOfSteps+1);
estimation_results{1} = Estimation_X;
row_idx = (data_matrix(:, end) <= NumberOfSteps+1);
data_matrix = data_matrix(row_idx, :);

for i = 0:NumberOfSteps
    IndexOfCurrentStepInDataMatrix = find(data_matrix(:,4) == i);
    m = size(IndexOfCurrentStepInDataMatrix, 1);
    if ( mod(i, 50) == 0 )
        disp(['Processing pose ', int2str(i)]);
    end
    % det(Estimation_X.cov)
    if i==NumberOfSteps-1
    a=1;
    end

    if i ~= NumberOfSteps
        OdometryFromThis2Next = data_matrix(IndexOfCurrentStepInDataMatrix(m-5):IndexOfCurrentStepInDataMatrix(m),1);
        if m > 6
            CameraMeasurementThis = [ data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(m-6) , 1 ),...
                                      data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(m-6) , 3 ),...
                                      data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(m-6) , 5 )];
           [Estimation_X] = REKF_update(Estimation_X, CameraMeasurementThis, obsv_sigma );
        end

        estimation_results{i+1} = Estimation_X;

%        propagation using odometry info
        [Estimation_X] = REKF_propagate(Estimation_X, OdometryFromThis2Next, odom_sigma );

    else
        a=2;
        if m > 6
            CameraMeasurementThis = [ data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(end) , 1 ) , ...
                                      data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(end) , 3 ),...
                                      data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(end) , 5 )];
            CameraMeasurementThis=CameraMeasurementThis(1:end-6,:);

            [Estimation_X] = REKF_update(Estimation_X, CameraMeasurementThis, obsv_sigma );
        end
        estimation_results{i+1} = Estimation_X;
    end
end
clearvars -except estimation_results


function [r_corr, dat, sl_size] = searchlight_modcomp(mask1, rdm, varargin)
% Calculate the local pattern similarity between two pattern maps using
% the searchlight approach.
%
% :Usage:
% ::
%
%     [r_corr, dat, sl_size] = searchlight_correlation(mask1, mask2, [additional_inputs])
%
%
% :Inputs:
%
%   **mask1:**
%        pattern or activation maps 1
%
%   **mask2:**
%        pattern or activation mediaps 2
%
% :Optional inputs:
%
%   **'r':**
%        searchlight sphere radius (in voxel) (default: r = 3 voxels)
%
%   **'type':**
%        This calls corr.m, and can take 'type' option.
%
%        'Pearson' (default), 'Kendall', 'Spearman'.
%
% :Outputs:
%
%   **r_corr:**
%        Correlation between weights of two pattern masks
%
%   **dat:**
%        This contains a statistic_image object that contain
%        correlation values between weights of two pattern masks
%        (=r_corr; in .dat) and p values for the correlation values
%        (in .p).
%
%   **sl_size:**
%        The number of voxels within each searchlight. Based on this
%        number, you can detect searchlights on the edge (searchlights
%        with low sl_size should be on the edge of the brain.
%
% ..
%     Author and copyright information:
%
%     Copyright (C) 2017  Phil Kragel
%     Copyright (C) 2014  Wani Woo
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
% ..
%
% :Examples:
% ::
%
%    mask1 = which('weights_NSF_grouppred_cvpcr.img');
%    mask2 = which('nonnoc_v6_109subjmap_mean.nii');
%
%    [r, dat] = searchlight_correlation(mask1, mask2, 'r', 5);

r = 4; % default radius (in voxel)
corr_type = 'Pearson';

% parsing varargin

for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}
            % functional commands
            case 'type'
                corr_type = varargin{i+1};
            case 'r' % radius
                r = varargin{i+1};
        end
    end
end

dat1 = fmri_data(mask1); dat1 = remove_empty(dat1);

dat1 = remove_empty(dat1);
n = size(dat1.dat,1);

r_corr = NaN(n,1);
p_corr = NaN(n,1);
sl_size = zeros(n,1);

fprintf('\n Calculating corrleation for voxel                 ');
xyzlist=dat1.volInfo.xyzlist(~dat1.removed_voxels,:);
dat=dat1.dat;
for i = 1:n %(1):vox_to_run(10)
    fprintf('\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b%07d/%07d', i, n);
    searchlight_indx = searchlight_sphere_prep(xyzlist, i, r);
    [r_corr(i), p_corr(i)] = corr(pdist(dat(searchlight_indx,:)','correlation')',rdm', 'type', corr_type);
    sl_size(i) = sum(searchlight_indx);
end

dat = statistic_image;
eval(['dat.volInfo = dat' num2str(1) '.volInfo;']);
eval(['dat.removed_voxels = dat' num2str(1) '.removed_voxels;']);
dat.dat = r_corr;
dat.p = p_corr;

end

% ========== SUBFUNCTION ===========

function indx = searchlight_sphere_prep(xyz, i, r)
seed = xyz(i,:);
indx = sum([xyz(:,1)-seed(1) xyz(:,2)-seed(2) xyz(:,3)-seed(3)].^2, 2) <= r.^2;
end


function strategy = beatTheBookie(dat, bet, marg)

money = 0; % initial amount of money
m = 1; % counter for money
accuracy = [];
acc = 1;
nGames = size(dat, 1);
bet_games = [];
nValidOdds = 3;

for gm = 1 : nGames

    score_home = dat(gm, 1);
    score_away = dat(gm, 2);
    if score_home > score_away
        result = 1;
    elseif score_home == score_away
        result = 2;
    else
        result = 3;
    end

    averages = dat(gm, 3:5); % [avg_home avg_draw avg_away];
    maximums = dat(gm, 6:8); % [max_home max_draw max_away];
    counts = dat(gm, 9:11);

    % check that there is at least one valid bet. If the bets are still not
    % online for this time, continue to the next game
    if any(isnan(averages)) || any(isnan(maximums))
        continue
    end

    % Apply formula and decide whether to bet
    earn_margin(1) = ((1 ./ averages(1) - marg) * maximums(1) - 1) * (counts(1) > nValidOdds) ;
    earn_margin(2) = ((1 ./ averages(2) - marg) * maximums(2) - 1) * (counts(2) > nValidOdds);
    earn_margin(3) = ((1 ./ averages(3) - marg) * maximums(3) - 1) * (counts(3) > nValidOdds);

    if sum(earn_margin > 0) >= 1

        [~, bet_result] = max(earn_margin);
        possible_earn = bet  * (maximums(bet_result) - 1);

        max_odds(m) = maximums(bet_result); %#ok
        mean_odds(m) = averages(bet_result); %#ok
        ids(m) = bet_result;

        % calculate loss / earning
        if isequal(bet_result, result)
            money(m + 1) = money(m) + possible_earn;
            accuracy(acc) = 1;
        else
            money(m + 1) = money(m) - bet;
            accuracy(acc) = 0;

        end

        bet_games(m) = gm;
        m = m + 1;
        acc = acc + 1;
    end

end

strategy.money = money;
strategy.odds = max_odds;
strategy.mean_odds = mean_odds;
strategy.accuracy = accuracy;
strategy.ids = ids;

end


function obslik = mk_hmm_obs_lik_matrix(engine, evidence)

T  = size(evidence,2);
Q = length(engine.startprob);
obslik = ones(Q, T);
bnet = bnet_from_engine(engine);
% P(o1,o2| Q1,Q2) = P(o1|Q1,Q2) * P(o2|Q1,Q2)
onodes = bnet.observed;
for i=1:length(onodes)
  data = cell2num(evidence(onodes(i),:));
  if bnet.auto_regressive(onodes(i))
    params = engine.obsprob{i};
    mu = params.big_mu;
    Sigma = params.big_Sigma,
    W = params.big_W;
    mu0 = params.big_mu0;
    Sigma0 = params.big_Sigma0;
    %obslik_i = mk_arhmm_obs_lik(data, mu, Sigma, W, mu0, Sigma0
    obslik_i = clg_prob(data(:,1:T-1), data(:,2:T), mu, Sigma, W);
    obslik_i = [mixgauss_prob(data(:,1), mu0, Sigma0) obslik_i];
  elseif myismember(onodes(i), bnet.dnodes)
    %obslik_i = eval_pdf_cond_multinomial(data, engine.obsprob{i}.big_CPT);
    obslik_i = multinomial_prob(data, engine.obsprob{i}.big_CPT);
  else
    %obslik_i = eval_pdf_cond_gauss(data, engine.obsprob{i}.big_mu, engine.obsprob{i}.big_Sigma);
    obslik_i = mixgauss_prob(data, engine.obsprob{i}.big_mu, engine.obsprob{i}.big_Sigma);
  end
  obslik = obslik .* obslik_i;
end


function [LL, LLS, ht] = tarch_likelihood(parameters, data, fdata, fIdata, p, o, q, error_type, tarch_type, back_cast, T, estim_flag)
% Log likelihood for TARCH(P,O,Q) estimation
%
% USAGE:
%   [LL, LLS, HT] = tarch_likelihood(PARAMETERS, DATA, FDATA, FIDATA, P, O, Q, ERROR_TYPE, TARCH_TYPE, BACK_CAST, T, ESTIM_FLAG)
%
% INPUTS:
%   PARAMETERS    - A vector of GARCH process parameters
%                   [omega alpha gamma beta [nu lambda]]
%   DATA          - Vector of mean zero residuals
%   FDATA         - Either abs(data) or data.^2, depending on tarch_type
%   FIDATA        - fdata times an indicator for negative, e.g. fdata.*(data<0)
%   P             - The lag order length for ARCH
%   O             - The lag order of asymmetric terms
%   Q             - The lag order length for GARCH
%   ERROR_TYPE    - The type of error being assumed, valid types are:
%                     1 if 'NORMAL'
%                     2 if 'STUDENTST'
%                     3 if 'GED'
%                     4 if 'SKEWT'
%   TARCH_TYPE    - 1 for absolute vale of return
%                 - 2 for squared returns (standard case)
%   BACK_CAST     - The value used for variance recursion
%   T             - Length of data
%   ESTIM_FLAG    - [OPTIONAL] Flag (0 or 1) to indicate if the function
%                   is being used in estimation.  If it is 1, then the parameters are
%                   transformed from unconstrained values to constrained by standard
%                   garch model constraints
%
% OUTPUTS:
%   LL             - Minus 1 times the log likelihood
%   LLS            - Time series of log likelihoods (Also multiplied by -1)
%   HT             - Time series of conditional variances
%
% COMMENTS:
%   See also TARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 9/1/2005

if nargin==12 && estim_flag
    %If for estimation, transform the parameters
    [parameters,nu,lambda]=tarch_itransform(parameters,p,o,q,error_type);
else
    %Otherwise the parameters simply must be parsed
    if error_type==2 || error_type==3
        %Seperate nu from the remaning parameters
        nu=parameters(p+o+q+2);
        parameters=parameters(1:1+p+o+q);
    elseif error_type==4
        lambda=parameters(p+o+q+3);
        nu=parameters(p+o+q+2);
        parameters=parameters(1:1+p+o+q);
    end
end

%Backcast length
m  =  max([p o q]);
%Compute the conditional variances
ht=tarch_core(fdata,fIdata,parameters,back_cast,p,o,q,m,T,tarch_type);

%Indices for the relevant opservations
t = (m + 1):T;
ht=ht(t);
data=data(t);
%Compute the log likelihoods
switch error_type
    case 1
        [LL, LLS] = normloglik(data,0,ht);
        LLS = -LLS;
        LL = -LL;
    case 2
        [LL, LLS] = stdtloglik(data,0,ht,nu);
        LLS = -LLS;
        LL = -LL;
    case 3
        [LL, LLS] = gedloglik(data,0,ht,nu);
        LLS = -LLS;
        LL = -LL;
    case 4
        [LL, LLS] = skewtloglik(data,0,ht,nu,lambda);
        LLS = -LLS;
        LL = -LL;
end


function [Estimation_X0,FirstLandmarks] = FEKF_update(Estimation_X0, CameraMeasurementThis, Sigma_OB , FirstLandmarks)
NumberOfLandmarksObInThisStep = size(CameraMeasurementThis,1)/3;

% initialise the IndexOfFeature if possible
if size(Estimation_X0.landmarks,2) > 0
    IndexOfFeature = Estimation_X0.landmarks(4,:)';
else
    IndexOfFeature = [];
end

IndexObservedAlreadyThis = [];
IndexObservedNew = [];
for i = 1:NumberOfLandmarksObInThisStep
    % check whether the feature is observed before or not
    M = find( IndexOfFeature== CameraMeasurementThis(3*i,2) );
    if isempty(M)
        IndexObservedNew = [IndexObservedNew;CameraMeasurementThis(3*i,2) ];
    else
        IndexObservedAlreadyThis = [IndexObservedAlreadyThis;CameraMeasurementThis(3*i,2)];
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% IndexObservedNew=[78; 97; 18] indicates that the
% robot firstly observes landmarks 78 97 18 in this step
% IndexObservedAlreadyThis=[19; 20; 53] indicates
% that the robot observes again landmarks 19 20 53 in this step
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

orientation = Estimation_X0.orientation;
position    = Estimation_X0.position;
cov         = Estimation_X0.cov;

NumberOfFeature = size( IndexOfFeature,1);
NumberOfOldFeatureInThisStep = size(IndexObservedAlreadyThis,1);
NumberOfNewFeatureInThisStep = size(IndexObservedNew,1);

% update state vector and covariance by considering
% new feature into state and covariance
if ~isempty(IndexObservedNew)

    orientation = Estimation_X0.orientation;
    position    = Estimation_X0.position;

   % copy previous covariance
    temp    = repmat({eye(3)}, NumberOfNewFeatureInThisStep, 1 );
    tempKK  = blkdiag(temp{:});
    Sigma   = blkdiag(Estimation_X0.cov,tempKK);
    KK      = eye(6+3*(NumberOfFeature+NumberOfNewFeatureInThisStep));

 %   add new features
    for i = 1:NumberOfNewFeatureInThisStep
        indNewf = IndexObservedNew(i);
        Estimation_X0.landmarks(4,NumberOfFeature+i) = indNewf;
        m2 = find( CameraMeasurementThis(:,2) == indNewf );
        nf = CameraMeasurementThis( m2, 1 );

        Estimation_X0.landmarks(1:3,NumberOfFeature+i) = orientation*nf+position;
        Newlandmark=[ orientation*nf+position;  indNewf  ];

        FirstLandmarks=[FirstLandmarks  Newlandmark];

%         KK( 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i,1:6 ) = [-skew(Estimation_X0.orientation*(nf)) eye(3)];
%         KK (6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i, 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i )=Estimation_X0.orientation;

         KK( 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i,1:6 ) = [-skew(orientation*(nf)) eye(3)];
        KK (6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i, 6+3*NumberOfFeature+3*i-2:6+3*NumberOfFeature+3*i )=orientation;
                tempKK(3*i-2:3*i,3*i-2:3*i)=diag(nf.^2)*Sigma_OB^2;
    end
    Sigma   = blkdiag(Estimation_X0.cov,tempKK);
    Estimation_X0.cov = KK*Sigma*KK';

    NumberOfFeature=NumberOfFeature+NumberOfNewFeatureInThisStep;
end

orientation = Estimation_X0.orientation;
position    = Estimation_X0.position;
cov         = Estimation_X0.cov;

% update state and covariance
if ~isempty(IndexObservedAlreadyThis)
    Z = zeros( NumberOfOldFeatureInThisStep*3 , 1);
    Y = zeros( NumberOfOldFeatureInThisStep*3 , 1);
    H = zeros(3*NumberOfOldFeatureInThisStep, 6+3*NumberOfFeature);

    temp = repmat({eye(3)}, NumberOfOldFeatureInThisStep,1 );
    R = blkdiag(temp{:});

    % update old features
    for i = 1:NumberOfOldFeatureInThisStep
        ind = find(IndexOfFeature == IndexObservedAlreadyThis(i));

        fi  = Estimation_X0.landmarks(1:3,ind);
        Y(3*i-2:3*i,1) = ObservationModel( orientation, position, fi );

        ind2 = find(CameraMeasurementThis(:,2) == IndexObservedAlreadyThis(i));
        Z(3*i-2:3*i,1 ) = CameraMeasurementThis(ind2,1);

        FirstLandmark=FirstLandmarks(1:3,ind);

        H(3*i-2:3*i, 1:6) = [-orientation'*skew(FirstLandmark-position) orientation'];
        H(3*i-2:3*i, 6+3*ind-2:6+3*ind) = -orientation';
        R(3*i-2:3*i,3*i-2:3*i) = diag(CameraMeasurementThis(ind2,1).^2)*Sigma_OB^2;
    end

    % question @RomaTeng, different computaton scheme
    z = Z-Y;
    S = H*cov*H'+R;
    K = cov*H'*inv(S);
    s = K*z;

    Estimation_X0 = SpecialAdd(Estimation_X0,-s);
    cov = ( eye(6+3*NumberOfFeature) -K*H )*cov;
    Estimation_X0.cov = cov;

end


function [candidates, exact] = project_candidates(candidates, H)
  n = size(candidates, 1);

  % project all 4 coordinates of the bbox
  %   1 ---- 2
  %   |      |
  %   4 ---- 3
  points = [cat(1, ...
    candidates(:,1:2), candidates(:,[3,2]), ...
    candidates(:,3:4), candidates(:,[1,4])), ...
    ones(4*n, 1)];
  projected_points = points * H';
  projected_points = projected_points ./ repmat(projected_points(:,3), [1 3]);
%   p1 = projected_points(1:n, 1:2);
%   p2 = projected_points((n+1):(2*n), 1:2);
%   p3 = projected_points((2*n+1):(3*n), 1:2);
%   p4 = projected_points((3*n+1):end, 1:2);
  xs = reshape(projected_points(:,1), [n 4]);
  ys = reshape(projected_points(:,2), [n 4]);
  exact = [xs(:,1), ys(:,1), xs(:,2), ys(:,2), xs(:,3), ys(:,3), xs(:,4), ys(:,4)];

  xs = sort(xs, 2);
  ys = sort(ys, 2);
  candidates(:,1) = mean(xs(:,1:2), 2);
  candidates(:,3) = mean(xs(:,3:4), 2);
  candidates(:,2) = mean(ys(:,1:2), 2);
  candidates(:,4) = mean(ys(:,3:4), 2);
end


function [g_all, vv_all]=vifsub_est_M(org,dist, subbands, M);

% uses convolution for determining the parameters of the distortion channel
% Called by vifvec.m

tol = 1e-15; % tolernace for zero variance. Variance below this is set to zero, and zero is set to this value to avoid numerical issues.

for i=1:length(subbands)
    sub=subbands(i);
    y=org{sub};
    yn=dist{sub};

    % compute the size of the window used in the distortion channel estimation
    lev=ceil((sub-1)/6);
    winsize=2^lev+1; offset=(winsize-1)/2;
    win = ones(winsize);

    % force subband size to be multiple of M
    newsize=floor(size(y)./M)*M;
    y=y(1:newsize(1),1:newsize(2));
    yn=yn(1:newsize(1),1:newsize(2));

    % Correlation with downsampling. This is faster than downsampling after
    % computing full correlation.
    winstep=[M M];
    winstart=[1 1].*floor(M/2)+1;
    winstop=size(y)-ceil(M/2)+1;

    % mean
    mean_x = corrDn(y,win/sum(win(:)),'reflect1',winstep, winstart,winstop);
    mean_y = corrDn(yn,win/sum(win(:)),'reflect1',winstep, winstart,winstop);
    % cov
    cov_xy = corrDn(y.*yn, win, 'reflect1',winstep, winstart,winstop) - sum(win(:)).*mean_x.*mean_y;
    % var
    ss_x = corrDn(y.^2,win, 'reflect1',winstep, winstart,winstop) - sum(win(:)).*mean_x.^2;
    ss_y = corrDn(yn.^2,win, 'reflect1',winstep, winstart,winstop) - sum(win(:)).*mean_y.^2;

    % get rid of numerical problems, very small negative numbers, or very
    % small positive numbers, or other theoretical impossibilities.
    ss_x(ss_x<0)=0;
    ss_y(ss_y<0)=0;

    % Regression
    g = cov_xy./(ss_x+tol);

    % Variance of error in regression
    vv = (ss_y - g.*cov_xy)/(sum(win(:)));

    % get rid of numerical problems, very small negative numbers, or very
    % small positive numbers, or other theoretical impossibilities.
    g (ss_x < tol) = 0;
    vv (ss_x < tol) = ss_y (ss_x < tol);
    ss_x(ss_x<tol)=0;

    g (ss_y < tol) = 0;
    vv (ss_y < tol) = 0;

    % constrain g to be non-negative.
    vv(g<0)=ss_y(g<0);
    g(g<0)=0;

    % take care of numerical errors, vv could be very small negative
    vv( vv <= tol) = tol;

    g_all{i}=g;
    vv_all{i}=vv;

end


function Y = vl_nndwt2( X, dzdy, varargin )
% dwt multi_channel
% only support haart and db2 for 1-level transformation

opts.padding = 0 ;
opts.wavename = 'haart';
opts = vl_argparse(opts, varargin, 'nonrecursive') ;
padding = opts.padding;

if  nargin <= 1 || isempty(dzdy)
    sz = size(X);
    X = X/2;
    if size(X, 3) == 1
        sz(3) = 1;
    end
    if size(X, 4) == 1
        sz(4) = 1;
    end
            im_c1 = X(1:2:end, 1:2:end, :, :);
            im_c2 = X(1:2:end, 2:2:end, :, :);
            im_c3 = X(2:2:end, 1:2:end, :, :);
            im_c4 = X(2:2:end, 2:2:end, :, :);
            Y = zeros([sz(1)/2 sz(2)/2 sz(3)*4 sz(4)], 'like', X);
            Y(:,:,1:sz(3),:)           = im_c1 + im_c2 + im_c3 + im_c4;
            Y(:,:,sz(3)+1:sz(3)*2,:)   = -im_c1 - im_c2 + im_c3 + im_c4;
            Y(:,:,sz(3)*2+1:sz(3)*3,:) = -im_c1 + im_c2 - im_c3 + im_c4;
            Y(:,:,sz(3)*3+1:end,:)     = im_c1 - im_c2 - im_c3 + im_c4;
else
    sz = size(dzdy);
    if size(X, 4) == 1
        sz(4) = 1;
    end

            dzdy = dzdy/2;
            Y = zeros([sz(1)*2 sz(2)*2 sz(3)/4 sz(4)],'like',dzdy);
            Y(1:2:end, 1:2:end, : , :) = dzdy(:,:,1:sz(3)/4,:) - dzdy(:,:,sz(3)/4+1:sz(3)/2,:) - ...
                                         dzdy(:,:,sz(3)/2+1:3*sz(3)/4,:) + dzdy(:,:,sz(3)/4*3+1:end,:);
            Y(1:2:end, 2:2:end, : , :) = dzdy(:,:,1:sz(3)/4,:) - dzdy(:,:,sz(3)/4+1:sz(3)/2,:) + ...
                                         dzdy(:,:,sz(3)/2+1:3*sz(3)/4,:) - dzdy(:,:,sz(3)/4*3+1:end,:);
            Y(2:2:end, 1:2:end, : , :) = dzdy(:,:,1:sz(3)/4,:) + dzdy(:,:,sz(3)/4+1:sz(3)/2,:) - ...
                                         dzdy(:,:,sz(3)/2+1:3*sz(3)/4,:) - dzdy(:,:,sz(3)/4*3+1:end,:);
            Y(2:2:end, 2:2:end, : , :) = dzdy(:,:,1:sz(3)/4,:) + dzdy(:,:,sz(3)/4+1:sz(3)/2,:) + ...
                                         dzdy(:,:,sz(3)/2+1:3*sz(3)/4,:) + dzdy(:,:,sz(3)/4*3+1:end,:);

end


function [divI, I_dx, I_dy] = computeDivergence(I_gx, I_gy)
%
%
%       [divI, I_dx, I_dy] = computeDivergence(I_gx, I_gy)
%
%       Input:
%           -I_gx: an input image
%           -I_gx
%
%       Output:
%           -I_dx:
%           -I_dy:
%           -divI:
%
%     Copyright (C) 2011-15  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if(~exist('I_gx', 'var') || ~exist('I_gy', 'var'))
    error('Gradients are needed to compute divergence');
end

kernelX = [0 0 0; -1 1 0; 0  0 0];
kernelY = [0 0 0;  0 1 0; 0 -1 0];

I_dx = imfilter(I_gx, kernelX, 'same');
I_dy = imfilter(I_gy, kernelY, 'same');

divI = RemoveSpecials(I_dx + I_dy);

end


function [ degrees, minutes, seconds] = latlonvec(decimal_degrees)
%LATLONVEC Convert decimal degrees into degrees/minutes/seconds
%
% Description:
% Return degrees, minutes, seconds given decimal degrees. Sign of
% coordinate will be returned in degrees portion of coordinate.
%
% Sample fuction call:
% [ degrees, minutes, seconds] = latlonvec(decimal_degrees)
%
% Author: Scott Lee
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

degrees = fix(abs(decimal_degrees));	% integer degrees
frac = abs(decimal_degrees) - degrees;	% fractional degrees, used to compute minutes
minutes = fix(frac*60);	    % integer minutes
frac = frac - minutes/60;	% fractional minutes, used to compute seconds
seconds = frac*3600;		% decimal seconds

% Handle sign.  Degrees portion will contain the sign of the coordinate.
% Minutes and seconds will always be positive.
% sign function returns -1, 0, +1 for x < 0, x == 0, x > 0, respectively
degrees = sign(decimal_degrees).*degrees;

if nargout<2
  degrees = [degrees minutes seconds];
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function [RHS, RHSx, RHSy, RHSz] = convectionTvdRHS(u, phi, FL)
% This function uses the TVD scheme to discretize a
% convection term in the form $\grad (u \phi)$ where u is a face vactor
% It also returns the x, y, x parts of the matrix of coefficient.
%
% SYNOPSIS:
%   [RHS, RHSx, RHSy, RHSz] = convectionTvdRHS(u, phi, FL)
%
% PARAMETERS:
%   u  - velocity vector, FaceVariable
%   phi  - value of phi from the previous time step or iteration, CellVariable
%   FL  - Flux Limiter function
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

d = u.domain.dimension;
switch d
    case 1
        RHS = convectionTvdRHS1D(u, phi, FL);
    case 1.5
        RHS = convectionTvdRHSCylindrical1D(u, phi, FL);
    case 1.8
        RHS = convectionTvdRHSSpherical1D(u, phi, FL);
    case 2
        [RHS, RHSx, RHSy] = convectionTvdRHS2D(u, phi, FL);
    case 2.5
        [RHS, RHSx, RHSy] = convectionTvdRHSCylindrical2D(u, phi, FL);
    case 2.8
        [RHS, RHSx, RHSy] = ...
            convectionTvdRHSRadial2D(u, phi, FL);
    case 3
        [RHS, RHSx, RHSy, RHSz] = convectionTvdRHS3D(u, phi, FL);
    case 3.2
        [RHS, RHSx, RHSy, RHSz] = ...
            convectionTvdRHSCylindrical3D(u, phi, FL);
end


function sF = sqrt(sF, varargin)
% square root of a function
%
% Syntax
%   sF = sqrt(sF)
%   sF = sqrt(sF, 'bandwidth', bandwidth)
%
% Input
%  sF - @S2FunHarmonic
%
% Output
%  sF - @S2FunHarmonic
%
% Options
%  bandwidth - minimal degree of the spherical harmonic
%

sF = sF.quadrature(@(v) sqrt(sF.eval(v)),varargin{:});

end


% Test file for chebtech1 constructor.
% Here, we check populate().  (This function is not user-facing.)

function pass = test_constructor(pref)

% Get preferences:
if ( nargin < 1 )
    pref = chebtech.techPref();
end

% Initialize with default data:
data = chebtech.parseDataInputs(struct());

%%
% Test on a scalar-valued function:
pref.refinementFunction = 'nested';
f = @(x) sin(x);
g = populate(chebtech1, f, data, pref);
x = chebtech1.chebpts(length(g.coeffs));
values = g.coeffs2vals(g.coeffs);
pass(1) = norm(f(x) - values, inf) < 10*vscale(g)*eps;

% Test on an array-valued function:
pref.refinementFunction = 'nested';
f = @(x) [sin(x) cos(x) exp(x)];
g = populate(chebtech1, f, data, pref);
x = chebtech1.chebpts(length(g.coeffs));
values = g.coeffs2vals(g.coeffs);
pass(2) = norm(f(x) - values, inf) < 10*max(vscale(g)*eps);

%%
% Test on a scalar-valued function:
pref.refinementFunction = 'resampling';
f = @(x) sin(x);
[g, values] = populate(chebtech1, f, data, pref);
x = chebtech1.chebpts(length(values));
pass(3) = norm(f(x) - values, inf) < 10*vscale(g)*eps;

% Test on an array-valued function:
pref.refinementFunction = 'resampling';
f = @(x) [sin(x) cos(x) exp(x)];
[g, values] = populate(chebtech1, f, data, pref);
x = chebtech1.chebpts(length(values));
pass(4) = norm(f(x) - values, inf) < 10*max(vscale(g)*eps);

%%
% Some other tests:

% This should fail with an error:
try
    f = @(x) x + NaN;
    populate(chebtech1, f, data, pref);
    pass(5) = false;
catch ME
    pass(5) = strcmp(ME.message, 'Too many NaNs/Infs to handle.');
end

% As should this:
try
    f = @(x) x + Inf;
    populate(chebtech1, f, data, pref);
    pass(6) = false;
catch ME
    pass(6) = strcmp(ME.message, 'Too many NaNs/Infs to handle.');
end

% Check that things don't crash if pref.minSamples and pref.maxLength are equal.
try
    pref.minSamples = 8;
    pref.maxLength = 8;
    populate(chebtech1, @sin, data, pref);
    pass(7) = true;
catch
    pass(7) = false;
end

% Test logical-valued functions:
f = chebtech1(@(x) x > -2);
g = chebtech1(1);
pass(8) = normest(f - g) < eps;

f = chebtech1(@(x) x < -2);
g = chebtech1(0);
pass(9) = normest(f - g) < eps;

end


function dist = codedist_bay(C1,C2,Py)
% compute the distance using a Bayesian metric between 'C1' and  'C2'
%
%    dist = codedist_bay(C1,C2,{Py})
%
% 'C1' contains the result code,
% 'C2' contains the codebooks prototype. '0' or  an infine small number 'eps'  represents the don't care
% 'Py' is a matrix containing the moderated outputs of the binary classifiers
%
%
% An example:
%  >> Ye = [1 1; 1 1];
%  >> codebook = [1 2 3];
%  >> old_codebook = [1 1 -1; 1 -1 -1];
%  >> Py = [.9 -.1; -.1 .13];
%  >> code(Ye, codebook, [],old_codebook,'codedist_bay',{Py})
% in this call, 'Ye' is not used explicitly.
%
% To use this distance measure in LS-SVMlab, the following
% procedure is to be followed, assume input data 'X' and multiclass
% output 'Y':
%
%  >> [Ycode,codebook,old_codebook] = code(Y,'code_MOC');
%  >> [alpha,b] = trainlssvm({X,Yc,'c',gam,sig2});
%  >> Yhc = simlssvm({X,Yc,'c',gam,sig2},{alpha,b},Xt);
%
%  The moderated output for the LS-SVM can be computed using the bayesian inference
%  framework for the LS-SVM:
%  >> Ymod = bay_modoutClass(model,Xt);
%  >> Yh = code(Yhc,old_codebook,[],codebook,'codedist_bay',{Ymod});
%
% see also:
%   bay_modoutClass, codedist_hamming, code_ECOC

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

% % encode for training
% >> model = initlssvm(X,Y,'classification',gam,sig2,'preprocess','RBF_kernel');
% >> model = changelssvm(model,'codetype','code_MOC');
% >> model = changelssvm(model,'codedist_fct','codedist_hamming');
% >> model = trainlssvm(model);
%
% % decode for simulating
% >> model = changelssvm(model,'codedist_fct','codedist_bay');
% >> model = changelssvm(model,'codedist_args',{bay_modoutClass(model,Xt)});
% >>   Yt  = simlssvm(model,Xt);

if nargin<3,
  error(['moderated output needed as function arguments' ...
	 '(for LS-SVM, model.codedist_args =' ...
	 ' bay_modoutClass(model,X)).']);
end

[nb,nbin] = size(Py);
[~,dim] = size(C2);
dist = zeros(nb,dim);

for d = 1:dim,
  for n= 1:nb,
    dist(n,d) = sum((1-Py(n,:).*C2(:,d)'))-sum(C2(:,d)==eps);
  end
end


function [U,E,J] = slice_triangles(V,F,plane,varargin)
  % SLICE_TRIANGLES
  %
  % Inputs:
  %   V  #V by 3 list of vertex positions
  %   F  #F by 3 list of triangles indices into V
  %   plane  4-long plane equation [nx ny nz -d]
  % Outputs:
  %   U  #U by 3 list of vertex positions
  %   E  #E by 2 list of edge indices into U
  %   J  #E by 1 list of birth triangle indices into F
  %

  % HACK!
  [U,E,J] = slice_tets(V,F(:,[1 2 3 3]),plane);
  % matlab fails to reutrn one face as row vecto
  if size(E,2) == 1 && size(E,1) == 3
    E = E';
  end
  if isempty(E)
    U = [];
    J = [];
    return
  end
  [U,~,I] = remove_duplicate_vertices(U,eps);
  size(E)
  E(E(:,2) == E(:,1),2) = E(E(:,2) == E(:,1),3);
  E = E(:,1:2);
  N = normals(V,F(J,:));
  N = N-sum(N.*plane(1:3),2).*plane(1:3);
  M = U(E(:,2),:)-U(E(:,1),:);
  Q = [1 plane(1:3)].*[cos(pi/4) sin(pi/4)*[1 1 1]];
  W = quatmultiply(quatmultiply(Q,[zeros(size(M,1),1) M]),Q.*[1 -1 -1 -1]);
  W = W(:,2:4);
  R = sign(sum(W.*N,2))>0;
  E(R,:) = fliplr(E(R,:));
  E = unique(E,'rows');

end


function best = findWeakRuleSamplesFeatures2(data,labels,dist,binVals,bins)

numDim = size(data,2);

best.dim = 1;
best.error = 0.5;
best.dir = 1;
best.tr = 0;

curBestErr = 0.5*ones(1,numDim);
binNo = ones(1,numDim);
bestDir = ones(1,numDim);

numS = 2500;
curSel = zeros(1,numS);
cumD = cumsum(dist);
for ndx = 1:numS
  curSel(ndx) = sum(cumD<rand(1,1))+1;
end

curBins = bins(curSel,:);
curLabels = labels(curSel);
curSelDim = find(rand(1,numDim)>0.9);

parfor dimNdx = 1:numel(curSelDim)
  dim = curSelDim(dimNdx);
  numBins = size(binVals,1)+1;
  binNdx =  curBins(:,dim)+numBins*(curLabels>0);
  allCount = histc(binNdx,.5:(2*numBins+0.5));
  allCount = allCount/sum(allCount);
  posCount = allCount(numBins+1:end-1);
  negCount = allCount(1:numBins);

  posLeft = 0;
  posRight = sum(posCount);
  negLeft = 0;
  negRight = sum(negCount);

  err = zeros(1,size(binVals,1));
  dir = ones(1,size(binVals,1));
  for ndx = 1:size(binVals,1)
    posLeft = posLeft + posCount(ndx);
    posRight = posRight - posCount(ndx);
    negLeft = negLeft + negCount(ndx);
    negRight = negRight - negCount(ndx);

    err(ndx) = posRight+negLeft-posLeft-negRight;
    if(err(ndx)<0)
      err(ndx) = - err(ndx);
      dir(ndx) = -1;
    end
  end
  err = 0.5-err/2;
  [curBestErr(dimNdx) binNo(dimNdx)]= min(err);
  bestDir(dimNdx) = dir(binNo(dimNdx));
end

[minError minDimNdx] = min(curBestErr);
minDim = curSelDim(minDimNdx);
best.error = minError;   best.dim = minDim;
best.dir = bestDir(minDimNdx);   best.tr = binVals(binNo(minDim),minDim);

end

%{
function [predError dir] = getError(data,label,tr, dist)

dir = 1;
predLabel = 2*(data>tr)-1;
predError = sum(( (predLabel.*label) ~=1).*dist);

if(predError>0.5); predError = 1-predError; dir = -1; end
end
%}


function Population = GDE3_EnvironmentalSelection(Population,Offspring,N)
% The environmental selection of CCGDE3

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    % Select the offsprings dominating their corresponding parents firstly,
    % and then select the offspring population non-dominated with the
    % parent population
    PopObj = Population.objs;
    OffObj = Offspring.objs;
    % The offsprings which can replace its parent
    updated = all(PopObj>=OffObj,2);
    % The offsprings which can add to the population
    selected = any(PopObj<OffObj,2) & any(PopObj>OffObj,2);
    % Update the population
    Population(updated) = Offspring(updated);
    Population          = [Population,Offspring(selected)];

    %% Non-dominated sorting
    [FrontNo,MaxFNo] = NDSort(Population.objs,N);
    Next = FrontNo < MaxFNo;

    %% Calculate the crowding distance of each solution
    CrowdDis = CrowdingDistance(Population.objs,FrontNo);

    %% Select the solutions in the last front based on their crowding distances
    Last     = find(FrontNo==MaxFNo);
    [~,Rank] = sort(CrowdDis(Last),'descend');
    Next(Last(Rank(1:N-sum(Next)))) = true;

    %% Population for next generation
    Population = Population(Next);
end


function M = moveUDRVC(F, d)
%------------------------------------------------------------------------------
% Moves gridfunction F in vertical direction.
% Excess area is filled by Vertex-Centered (VC) Reflection across boundaries.
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: June 23, 2000.
% (c) 1999-2002 Stichting CWI, Amsterdam
%------------------------------------------------------------------------------
[n, m] = size(F);
if d > 0
  if (d+1) > n
    error(' moveUDRVC    d >= n ')
  else
    M = [flipud(F(2:(d+1),:)); F(1:(n-d),:)];
  end
elseif d < 0
  if (-d+1) > n
    error(' moveUDRVC   -d >= n ')
  else
    M = [F((-d+1):n,:); flipud(F((n+d):(n-1),:))];
  end
else
  M = F;
end
%------------------------------------------------------------------------------


% Cho, K., Raiko, T., Ilin, A., and Karhunen, J.
% A Two-stage Pretraining Algorithm for Deep Boltzmann Machines
% http://users.ics.aalto.fi/kcho/papers/nips12workshop.pdf

% add the path of RBM code
addpath('..');

% load MNIST
load 'mnist_14x14.mat';

% shuffle the training data
perm_idx = randperm (size(X,1));
X = X(perm_idx, :);
X_labels = X_labels(perm_idx);

X_labels = X_labels + 1;
X_test_labels = X_test_labels + 1;

% structure of DBM: 784 - 500 - 500
layers = [size(X,2), 500, 500];
n_layers = length(layers);

pretrain = 0;
centering = 1;

if pretrain
    % pretraining (stage 1)
    Xp = X;
    Rs = cell(n_layers, 1);
    Qpre = cell(n_layers, 1);
    Qpre_mask = zeros(n_layers, 1);

    for l = 1:n_layers
        if mod(l, 2) == 0
            continue;
        end

        if l+1 > n_layers
            break;
        end

        R = default_rbm (size(Xp, 2), layers(l+2));

        R.learning.lrate = 1e-2;
        R.learning.weight_decay = 1e-4;

        R.learning.persistent_cd = 0;

        R.fast.use = 0;
        R.fast.lrate = 1e-2;

        R.parallel_tempering.use = 0;

        R.adaptive_lrate.use = 1;
        R.adaptive_lrate.lrate_ub = R.learning.lrate;

        R.enhanced_grad.use = 1;
        R.learning.minibatch_sz = 128;

        % max. 100 epochs
        R.iteration.n_epochs = 100;

        if R.data.binary
            mH = mean(Xp, 1)';
            R.vbias = min(max(log(mH./(1 - mH)), -4), 4);
            R.fast.vbias = min(max(log(mH./(1 - mH)), -4), 4);
        else
            R.vbias = mean(Xp, 1)';
            R.fast.vbias = mean(Xp, 1)';
        end

        % set the stopping criterion
        R.stop.criterion = 0;
        R.stop.recon_error.tolerate_count = 1000;

        % save the intermediate data after every epoch
        R.hook.per_epoch = {@save_intermediate, {sprintf('rbm_%d.mat', l)}};
        R.hook.per_update = {};

        % print learining process
        R.verbose = 0;
        R.debug.do_display = 0;
        R.debug.display_interval = 10;
        R.debug.display_fid = 1;
        R.debug.display_function = @visualize_rbm;

        % train RBM
        fprintf(1, 'Training RBM\n');
        tic;
        R = train_rbm (R, Xp);
        fprintf(1, 'Training is done after %f seconds\n', toc);

        Rs{l} = R;

        Xp = rbm_get_hidden(Xp, R);

        Qpre{l+2} = Xp;
        Qpre_mask(l+2) = 1;
    end
end

[D] = default_dbm (layers);

if pretrain
    % pretraining (stage 2)
    D.learning.persistent_cd = 0;
    D.learning.lrate = 1e-2;
    D.learning.lrate0 = 1000;

    D.enhanced_grad.use = 0;
    D.adaptive_lrate.use = 1;
    D.adaptive_lrate.lrate_ub = D.learning.lrate;

    D.iteration.n_epochs = 200;
    D.hook.per_epoch = {@save_intermediate, {'dbm_pre.mat'}};

    fprintf(1, 'Training DBM\n');
    tic;
    [D] = dbm (D, X, 1, Qpre, Qpre_mask);
    fprintf(1, 'Training is done after %f seconds\n', toc);
end

% finetuning (stage 3)
D.learning.persistent_cd = 1;
if pretrain
    D.learning.lrate = 1e-4;
    D.learning.lrate0 = 5000;
else
    D.learning.lrate = 1e-3;
    D.learning.lrate0 = 5000;
end

D.enhanced_grad.use = 0;
D.adaptive_lrate.use = 1;
D.adaptive_lrate.lrate_ub = D.learning.lrate;

D.iteration.n_epochs = 100;
D.hook.per_epoch = {@save_intermediate, {'dbm_mnist.mat'}};

mH = mean(X, 1)';
D.biases{1} = min(max(log(mH./(1 - mH)), -4), 4);

if centering
    D = set_dbm_centers(D);
end

fprintf(1, 'Finetuning DBM\n');
tic;
[D] = dbm (D, X);
fprintf(1, 'Finetuning is done after %f seconds\n', toc);

% classification
[Q_mf] = dbm_get_hidden(X, D, 30, 1e-6, D.mf.reg);

perm_idx = randperm (size(X,1));

n_all = size(X, 1);
n_train = ceil(n_all * 3 / 4);
n_valid = floor(n_all /4);

X_valid = X(perm_idx(n_train+1:end), :);
X_valid_labels = X_labels(perm_idx(n_train+1:end));
X = X(perm_idx(1:n_train), :);
X_labels = X_labels(perm_idx(1:n_train));

for l = 1:n_layers
    Q_train{l} = Q_mf{l}(perm_idx(1:n_train), :);
    Q_valid{l} = Q_mf{l}(perm_idx(n_train+1:end), :);
end

[Q_test] = dbm_get_hidden(X_test, D, 30, 1e-6, D.mf.reg);

M = default_mlp ([layers, 10]);
M = set_mlp_dbm (M);

M.output.binary = 1;
M.hidden.use_tanh = 0;

M.valid_min_epochs = 10;
M.dropout.use = 0;

M.hook.per_epoch = {@save_intermediate, {'mlp_dbm_mnist.mat'}};

M.learning.lrate = 1e-2;
M.learning.lrate0 = 5000;
%M.learning.momentum = 0.9;
%M.learning.weight_decay = 0.0001;
M.learning.minibatch_sz = 128; M.adagrad.use = 1;
M.adagrad.epsilon = 1e-8;

M.noise.drop = 0;
M.noise.level = 0;

M.iteration.n_epochs = 100;

for l = 1:n_layers
    M.biases{l} = D.biases{l};
    if centering
        if l > 1
            M.biases{l} = M.biases{l} - M.W{l-1}' * D.centering.centers{l-1};
        end
        if l < n_layers
            M.biases{l} = M.biases{l} - M.W{l} * D.centering.centers{l+1};
        end
    end
    if l < n_layers
        M.W{l} = D.W{l};
        M.dbm.W{l} = D.W{l};
    end
end

fprintf(1, 'Training MLP\n');
tic;
M = mlp_dbm (M, X, Q_train, X_labels, X_valid, Q_valid, X_valid_labels, 0.1);
fprintf(1, 'Training is done after %f seconds\n', toc);

[pred] = mlp_classify (M, X_test, Q_test);
n_correct = sum(X_test_labels == pred);

fprintf(2, 'Correctly classified test samples: %d/%d\n', n_correct, size(X_test, 1));


function rs = fft(s)

%tstoolbox/@signal/fft
%   Syntax:
%     * f = fft(s)
%
%   Output arguments:
%     * f - n by 2 array, the first column contains the magnitudes, the
%       second one the phases.
%
%   Fourier transform of scalar signal s.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

narginchk(1,1);

if ndim(s) > 1
	error('Only for scalar signals');
end

x = fftshift(fft(data(s)));
rs = signal(core([abs(x)/dlens(s,1) (abs(x) > 10 * sqrt(eps)) .* angle(x)]), s);
a = getaxis(rs, 1);
rs = setaxis(rs, 1, achse(unit(a)^(-1), -samplerate(a)/2, samplerate(a)/dlens(s,1)));
rs = setaxis(rs, 2, achse);
rs = setplothint(rs, 'subplotgraph');
rs = addhistory(rs, ['Fourier transform']);
rs = addcommandlines(rs, 's = fft(s');


function lik = lik_binomial(varargin)
%LIK_BINOMIAL  Create a Binomial likelihood structure
%
%  Description
%    LIK = LIK_BINOMIAL creates Binomial likelihood structure.
%
%    The likelihood is defined as follows:
%                  __ n
%      p(y|f, z) = || i=1 [ p_i^(y_i)*(1-p_i)^(z_i-y_i)) *
%                           gamma(z_i+1)/(gamma(y_i+1)*gamma(z_i-y_i+1))]
%    where p_i = exp(f_i)/ (1+exp(f_i)) is the succes probability,
%    which is a function of the latent variable f_i and z is a
%    vector of numbers of trials.
%
%    When using Binomial likelihood you need to give the vector z
%    as an extra parameter to each function that requires y also.
%    For example, you should call gp_optim as follows
%      gp_optim(gp, x, y, 'z', z)
%
%  See also
%    GP_SET, LIK_*
%
% Copyright (c) 2009-2010 Jaakko Riihimäki & Jarno Vanhatalo
% Copyright (c) 2010-2011 Aki Vehtari

% 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_BINOMIAL';
  ip.addOptional('lik', [], @isstruct);
  ip.parse(varargin{:});
  lik=ip.Results.lik;

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

  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_binomial_pak;
    lik.fh.unpak = @lik_binomial_unpak;
    lik.fh.ll = @lik_binomial_ll;
    lik.fh.llg = @lik_binomial_llg;
    lik.fh.llg2 = @lik_binomial_llg2;
    lik.fh.llg3 = @lik_binomial_llg3;
    lik.fh.tiltedMoments = @lik_binomial_tiltedMoments;
    lik.fh.predy = @lik_binomial_predy;
    lik.fh.predprcty = @lik_binomial_predprcty;
    lik.fh.invlink = @lik_binomial_invlink;
    lik.fh.recappend = @lik_binomial_recappend;
  end

end

function [w,s,h] = lik_binomial_pak(lik)
%LIK_BINOMIAL_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_BINOMIAL_PAK(LIK) takes a likelihood structure LIK
%    and returns an empty verctor W. If Binomial likelihood had
%    parameters this would combine them into a single row vector
%    W (see e.g. likelih_negbin). This is a mandatory subfunction
%    used for example in energy and gradient computations.
%
%  See also
%    LIK_NEGBIN_UNPAK, GP_PAK

  w = []; s = {}; h=[];
end

function [lik, w] = lik_binomial_unpak(lik, w)
%LIK_BINOMIAL_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    W = LIK_BINOMIAL_UNPAK(W, LIK) Doesn't do anything.
%
%    If Binomial likelihood had parameters this would extracts
%    them parameters from the vector W to the LIK structure.
%    This is a mandatory subfunction used for example in energy
%    and gradient computations.
%
%  See also
%    LIK_BINOMIAL_PAK, GP_UNPAK

  lik=lik;
  w=w;

end

function ll = lik_binomial_ll(lik, y, f, z)
%LIK_BINOMIAL_LL  Log likelihood
%
%  Description
%    LL = LIK_BINOMIAL_LL(LIK, Y, F, Z) takes a likelihood
%    structure LIK, succes counts Y, numbers of trials 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_BINOMIAL_LLG, LIK_BINOMIAL_LLG3, LIK_BINOMIAL_LLG2, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_ll: missing z!'...
           'Binomial likelihood needs the expected number of   '...
           'occurrences as an extra input z. See, for         '...
           'example, lik_binomial and gp_optim.             ']);
  end

  expf = exp(f);
  p = expf ./ (1+expf);
  N = z;
  ll =  sum(gammaln(N+1)-gammaln(y+1)-gammaln(N-y+1)+y.*log(p)+(N-y).*log(1-p));
end

function llg = lik_binomial_llg(lik, y, f, param, z)
%LIK_BINOMIAL_LLG    Gradient of the log likelihood
%
%  Description
%    LLG = LIK_BINOMIAL_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, succes counts Y, numbers of trials 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_BINOMIAL_LL, LIK_BINOMIAL_LLG2, LIK_BINOMIAL_LLG3, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg: missing z!'...
           'Binomial likelihood needs the expected number of   '...
           'occurrences as an extra input z. See, for         '...
           'example, lik_binomial and gp_optim.             ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;

      llg = y./(1+expf) - (N-y).*expf./(1+expf);
  end
end

function llg2 = lik_binomial_llg2(lik, y, f, param, z)
%LIK_BINOMIAL_LLG2  Second gradients of the log likelihood
%
%  Description
%    LLG2 = LIK_BINOMIAL_LLG2(LIK, Y, F, PARAM) takes a
%    likelihood structure LIK, succes counts Y, numbers of trials
%    Z, and latent values F. Returns the Hessian of the log
%    likelihood with respect to PARAM. At the moment PARAM can be
%    only 'latent'. G2 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_BINOMIAL_LL, LIK_BINOMIAL_LLG, LIK_BINOMIAL_LLG3, GPLA_E

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg2: missing z!'...
           'Binomial likelihood needs the expected number of    '...
           'occurrences as an extra input z. See, for          '...
           'example, lik_binomial and gp_optim.              ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;

      llg2 = -N.*expf./(1+expf).^2;
  end
end

function llg3 = lik_binomial_llg3(lik, y, f, param, z)
%LIK_BINOMIAL_LLG3  Third gradients of the log likelihood
%
%  Description
%    LLG3 = LIK_BINOMIAL_LLG3(LIK, Y, F, PARAM) takes a
%    likelihood structure LIK, succes counts Y, numbers of trials
%    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'. G3 is a vector with third gradients.
%    This subfunction is needed when using Laplace appoximation
%    for inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_BINOMIAL_LL, LIK_BINOMIAL_LLG, LIK_BINOMIAL_LLG2, GPLA_E, GPLA_G

  if isempty(z)
    error(['lik_binomial -> lik_binomial_llg3: missing z!'...
           'Binomial likelihood needs the expected number of    '...
           'occurrences as an extra input z. See, for          '...
           'example, lik_binomial and gp_optim.              ']);
  end

  switch param
    case 'latent'
      expf = exp(f);
      N = z;
      llg3 = N.*(expf.*(expf-1))./(1+expf).^3;
  end
end

function [logM_0, m_1, sigm2hati1] = lik_binomial_tiltedMoments(lik, y, i1, sigma2_i, myy_i, z)
%LIK_BINOMIAL_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_BINOMIAL_TILTEDMOMENTS(LIK, Y, I, S2,
%    MYY, Z) takes a likelihood structure LIK, succes counts Y,
%    numbers of trials Z, index I and cavity variance S2 and mean
%    MYY. Returns the zeroth moment M_0, mean M_1 and variance
%    M_2 of the posterior marginal (see Rasmussen and Williams
%    (2006): Gaussian processes for Machine Learning, page 55).
%    This subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods.
%
%  See also
%    GPEP_E

%  if isempty(z)
%    error(['lik_binomial -> lik_binomial_tiltedMoments: missing z!'...
%           'Binomial likelihood needs the expected number of               '...
%           'occurrences as an extra input z. See, for                     '...
%           'example, lik_binomial and gp_optim.                         ']);
%  end

  yy = y(i1);
  N = z(i1);
  logM_0=zeros(size(yy));
  m_1=zeros(size(yy));
  sigm2hati1=zeros(size(yy));

  for i=1:length(i1)
    if isscalar(sigma2_i)
      sigma2ii = sigma2_i;
    else
      sigma2ii = sigma2_i(i);
    end

    % Create function handle for the function to be integrated
    % (likelihood * cavity) and useful integration limits
    [tf,minf,maxf]=init_binomial_norm(yy(i),myy_i(i),sigma2ii,N(i));

    % Integrate with quadrature
    RTOL = 1.e-6;
    ATOL = 1.e-10;
    [m_0, m_1(i), m_2] = quad_moments(tf,minf, maxf, RTOL, ATOL);
    if isnan(m_0)
      logM_0=NaN;
      return
    end
    sigm2hati1(i) = m_2 - m_1(i).^2;

    % If the second central moment is less than cavity variance
    % integrate more precisely. Theoretically for log-concave
    % likelihood should be sigm2hati1 < sigm2_i.
    if sigm2hati1(i) >= sigma2ii
      ATOL = ATOL.^2;
      RTOL = RTOL.^2;
      [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
      sigm2hati1(i) = m_2 - m_1(i).^2;
      %    if sigm2hati1 >= sigm2_i
      %      error('lik_binomial_tilted_moments: sigm2hati1 >= sigm2_i');
      %    end
    end
    logM_0(i) = log(m_0);
  end
end

function [lpy, Ey, Vary] = lik_binomial_predy(lik, Ef, Varf, yt, zt)
%LIK_BINOMIAL_PREDY  Returns the predictive mean, variance and density of y
%
%  Description
%    [LPY] = LIK_BINOMIAL_PREDY(LIK, EF, VARF YT, ZT)
%    Returns logarithm of the predictive density PY of YT, that is
%        p(yt | y, zt) = \int p(yt | f, zt) p(f|y) df.
%    This requires also the succes counts YT, numbers of trials ZT.
%    This subfunction is needed when computing posterior predictive
%    distributions for future observations.
%
%    [LPY, EY, VARY] = LIK_BINOMIAL_PREDY(LIK, EF, VARF) takes a
%    likelihood structure LIK, posterior mean EF and posterior
%    Variance VARF of the latent variable and returns 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
%    GPEP_PRED, GPLA_PRED, GPMC_PRED

  if isempty(zt)
    error(['lik_binomial -> lik_binomial_predy: missing z!'...
           'Binomial likelihood needs the expected number of       '...
           'occurrences as an extra input z. See, for             '...
           'example, lik_binomial and gp_optim.                 ']);
  end

  if nargout > 1
  % Here we approximate the inverse logistic function by a linear combination of inverse probit function
  % logitinv(f) = a probit_inv(f/c1S) + (1 - a) probit_inv(f/c2s)
  % see Demidenko (2004). Mixed models: Theory and applications
      p1 = 0.4353;   p2 = 0.5647; % these are a and (1 - a)
      c1S = 2.2967^2;  c2S = 1.3017^2;

      z1 = Ef ./ sqrt(c1S + Varf);
      z2 = Ef ./ sqrt(c2S + Varf);

      % unconditional expectation
      p = (p1 .* normcdf(z1) + p2 .* normcdf(z2));
      Ey = zt .* p;

      % unconditional variance
      if any(zt ~= 1)
          nt = length(Ef);
          zz11 = zeros(nt, 1);
          zz12 = zeros(nt, 1);
          zz22 = zeros(nt, 1);

          for i = 1:length(Ef)
              zz11(i) = mvncdf([Ef(i) Ef(i)], [0 0], ...
                  [Varf(i) + c1S Varf(i); Varf(i) Varf(i) + c1S]);

              zz12(i) = mvncdf([Ef(i) Ef(i)], [0 0], ...
                  [Varf(i) + c1S Varf(i); Varf(i) Varf(i) + c2S]);

              zz22(i) = mvncdf([Ef(i) Ef(i)], [0 0], ...
                  [Varf(i) + c2S Varf(i); Varf(i) Varf(i) + c2S]);

          end
          Ey12 = p1^2 * zz11 + 2*p1*p2 * zz12 + p2^2 * zz22;

      else
          Ey12 = 0;

      end

      % unconditional variance
      Vary = zt .* (p - Ey12) + zt.^2 .* (Ey12 - p.^2);

  %     nt=length(Ef);
  %     Ey=zeros(nt,1);
  %     EVary = zeros(nt,1);
  %     VarEy = zeros(nt,1);
  %     for i1=1:nt
  %        ci = sqrt(Varf(i1));
  %       F  = @(x)zt(i1)./(1+exp(-x)).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
  %        Ey(i1) = quadgk(F,Ef(i1)-6*ci,Ef(i1)+6*ci);
  %
  %       F2  = @(x)zt(i1)./(1+exp(-x)).*(1-1./(1+exp(-x))).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
  %       EVary(i1) = quadgk(F2,Ef(i1)-6*ci,Ef(i1)+6*ci);
  %
  %       F3  = @(x)(zt(i1)./(1+exp(-x))).^2.*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
  %       VarEy(i1) = quadgk(F3,Ef(i1)-6*ci,Ef(i1)+6*ci) - Ey(i1).^2;
  %     end
  %     Vary = EVary+VarEy;
  end

  nt=length(yt);
  lpy=zeros(nt,1);
  if (size(Ef,2) > 1) && (size(Ef,2) > 1) && size(yt,2) == 1
    % Approximate integral with sum of grid points when using corrected
    % marginal posterior pf
    for i1=1:length(yt)
      py = arrayfun(@(f) exp(lik.fh.ll(lik, yt(i1), f, zt(i1))), Ef(i1,:));
      pf = Varf(i1,:)./sum(Varf(i1,:));
      lpy(i1) = log(sum(py.*pf));
    end
  else
    for i1=1:nt
      ci = sqrt(Varf(i1));
      F  = @(x)exp(gammaln(zt(i1)+1)-gammaln(yt(i1)+1)-gammaln(zt(i1)-yt(i1)+1) + yt(i1).*log(1./(1+exp(-x))) + (zt(i1)-yt(i1)).*log(1-(1./(1+exp(-x))))).*norm_pdf(x,Ef(i1),sqrt(Varf(i1)));
      lpy(i1) = log(quadgk(F,Ef(i1)-6*ci,Ef(i1)+6*ci));
    end
  end

end

function prctys = lik_binomial_predprcty(lik, Ef, Varf, zt, prcty)
%LIK_BINOMIAL_PREDPRCTY  Returns the percentiled 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

  if isempty(zt)
    error(['lik_binomial -> lik_binomial_predprcty: missing z!'...
           'Binomial likelihood needs the expected number of       '...
           'occurrences as an extra input z. See, for             '...
           'example, lik_binomial and gp_optim.                 ']);
  end

  opt=optimset('TolX',.5,'Display','off');
  nt=size(Ef,1);
  prctys = zeros(nt,numel(prcty));
  prcty=prcty/100;
  for i1=1:nt
    ci = sqrt(Varf(i1));
    for i2=1:numel(prcty)
      a=floor(fminbnd(@(a) (quadgk(@(f) binocdf(a,zt(i1),logitinv(f)).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2,binoinv(prcty(i2),zt(i1),logitinv(Ef(i1)-1.96*ci)),binoinv(prcty(i2),zt(i1),logitinv(Ef(i1)+1.96*ci)),opt));
      if quadgk(@(f) binocdf(a,zt(i1),logitinv(f)).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)<prcty(i2)
        a=a+1;
      end
      prctys(i1,i2)=a;
    end
  end
end

function [df,minf,maxf] = init_binomial_norm(yy,myy_i,sigm2_i,N)
%INIT_BINOMIAL_NORM
%
%  Description
%    Return function handle to a function evaluating Binomial *
%    Gaussian which is used for evaluating (likelihood * cavity)
%    or (likelihood * posterior) Return also useful limits for
%    integration. This is private function for lik_binomial. This
%    subfunction is needed by subfunctions tiltedMoments and predy.
%
% See also
%   LIK_BINOMIAL_TILTEDMOMENTS, LIK_BINOMIAL_PREDY

% avoid repetitive evaluation of constant part
  ldconst = gammaln(N+1)-gammaln(yy+1)-gammaln(N-yy+1) - log(sigm2_i)/2 - log(2*pi)/2;
%   ldconst = log(factorial(N)/(factorial(yy)*factorial(N-yy))-log(sigm2_i)/2 -log(2*pi)/2;

 % Create function handle for the function to be integrated
  df = @binomial_norm;
 % use log to avoid underflow, and derivates for faster search
  ld = @log_binomial_norm;
  ldg = @log_binomial_norm_g;
  ldg2 = @log_binomial_norm_g2;

  % Set the limits for integration
  % Binomial likelihood is log-concave so the binomial_norm
  % function is unimodal, which makes things easier
  if yy==0 || yy==N
    % with yy==0 or yy==N the mode of the likelihood is not defined
    % use the mode of the Gaussian (cavity or posterior) as a first guess
    modef = myy_i;
  else
    % use precision weighted mean of the Gaussian approximation of the
    % binomial likelihood and Gaussian
    mean_app = log(yy./(N-yy));
    ld0=1/(1+exp(-mean_app));
    ld1=(1-ld0)*ld0;
    ld2=ld0-3*ld0^2+2*ld0^3;
    var_app=inv(-( yy*(ld2*ld0-ld1^2)/ld0^2 + (N-yy)*(ld2*(ld0-1)-ld1^2)/(ld0-1)^2 ));

    modef = (myy_i/sigm2_i + mean_app/var_app)/(1/sigm2_i + 1/var_app);
%     sigm_app = sqrt((1/sigm2_i + 1/var_app)^-1);
  end
  % find the mode of the integrand using Newton iterations
  % few iterations is enough, since the first guess in the right direction
  niter=3;       % number of Newton iterations
  mindelta=1e-6; % tolerance in stopping Newton iterations
  for ni=1:niter
      g = ldg(modef);
      h = ldg2(modef);
      delta=-g/h;
      modef=modef+delta;
      if abs(delta)<mindelta
          break
      end
  end
  if abs(delta)>1 || isinf(delta) || isnan(delta)
    % Newton algorithm didn't work properly so do binary search
    modef=myy_i;
    a=modef-5.*sqrt(sigm2_i); b=modef+5.*sqrt(sigm2_i); delta=1;
    while ldg(a)<0
      a=a-5.*sqrt(sigm2_i);
    end
    while ldg(b)>0
      b=b+5.*sqrt(sigm2_i);
    end
    while delta > 0.1
      modef=(a+b)/2;
      if ldg(modef) > 0
        a=modef;
      else
        b=modef;
      end
      delta=b-a;
    end
    h=ldg2(modef);
  end
  % integrand limits based on Gaussian approximation at mode
  modes=sqrt(-1/h);
  minf=modef-4*modes;
  maxf=modef+4*modes;
  modeld=ld(modef);
  iter=0;
  % check that density at end points is low enough
  lddiff=12; % min difference in log-density between mode and end-points
  minld=ld(minf);
  step=1;
  while minld>(modeld-lddiff)
    minf=minf-step*modes;
    minld=ld(minf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_binomial -> init_binomial_norm: ' ...
             'integration interval minimun not found ' ...
             'even after looking hard!'])
    end
  end
  maxld=ld(maxf);
  iter=0;
  step=1;
  while maxld>(modeld-lddiff)
    maxf=maxf+step*modes;
    maxld=ld(maxf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_binomial -> init_binomial_norm: ' ...
             'integration interval maximun not found ' ...
             'even after looking hard!'])
    end
  end

  function integrand = binomial_norm(f)
  % Logit * Gaussian
    integrand = exp(ldconst + yy*log(1./(1.+exp(-f)))+(N-yy)*log(1-1./(1.+exp(-f)))...
                   - 0.5 * (f-myy_i).^2./sigm2_i);
%     integrand = exp(ldconst ...
%                     +yy*log(x)+(N-yy)*log(1-x) ...
%                     -0.5*(f-myy_i).^2./sigm2_i);
    integrand(isnan(integrand)|isinf(integrand))=0;
  end

  function log_int = log_binomial_norm(f)
  % log(Binomial * Gaussian)
  % log_binomial_norm is used to avoid underflow when searching
  % integration interval

    log_int = ldconst + yy*log(1./(1.+exp(-f)))+(N-yy)*log(1-1./(1.+exp(-f)))...
                   - 0.5 * (f-myy_i).^2./sigm2_i;
%     log_int = ldconst ...
%               -log(1+exp(-yy.*f)) ...
%               -0.5*(f-myy_i).^2./sigm2_i;
    log_int(isnan(log_int)|isinf(log_int))=-Inf;
  end

  function g = log_binomial_norm_g(f)
  % d/df log(Binomial * Gaussian)
  % derivative of log_binomial_norm
    g = -(f-myy_i)./sigm2_i - exp(-f).*(N-yy)./((1+exp(-f)).^2.*(1-1./(1+exp(-f)))) ...
        + exp(-f).*yy./(1+exp(-f));
%     g = yy./(exp(f*yy)+1)...
%         + (myy_i - f)./sigm2_i;
  end

  function g2 = log_binomial_norm_g2(f)
  % d^2/df^2 log(Binomial * Gaussian)
  % second derivate of log_binomial_norm
    g2 = - (1+exp(2.*f)+exp(f).*(2+N*sigm2_i)./((1+exp(f))^2*sigm2_i));
%     a=exp(f*yy);
%     g2 = -a*(yy./(a+1)).^2 ...
%          -1/sigm2_i;
  end

end

function p = lik_binomial_invlink(lik, f, z)
%LIK_BINOMIAL_INVLINK  Returns values of inverse link function
%
%  Description
%    P = LIK_BINOMIAL_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 gp_predprctmu.
%
%     See also
%     LIK_BINOMIAL_LL, LIK_BINOMIAL_PREDY

  p = logitinv(f);
end

function reclik = lik_binomial_recappend(reclik, ri, lik)
%RECAPPEND  Append the parameters to the record
%
%  Description
%    RECLIK = GPCF_BINOMIAL_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

  if nargin == 2
    reclik.type = 'Binomial';

    % Set the function handles
    reclik.fh.pak = @lik_binomial_pak;
    reclik.fh.unpak = @lik_binomial_unpak;
    reclik.fh.ll = @lik_binomial_ll;
    reclik.fh.llg = @lik_binomial_llg;
    reclik.fh.llg2 = @lik_binomial_llg2;
    reclik.fh.llg3 = @lik_binomial_llg3;
    reclik.fh.tiltedMoments = @lik_binomial_tiltedMoments;
    reclik.fh.invlink = @lik_binomial_invlink;
    reclik.fh.predprcty = @lik_binomial_predprcty;
    reclik.fh.predy = @lik_binomial_predy;
    reclik.fh.recappend = @likelih_binomial_recappend;
    return
  end

end


// BLAS 3 - Dense Matrix-Dense Matrix multiplication
// Constants are in GEMM convention; (MxK matrix) x (KxN matrix) = (MxN matrix)
Constant SzM 100;
Constant SzN 100;
Constant SzK 100;

// Optimize for data reuse
// Constant MTileSz 10;
// Constant NTileSz 50;
// Constant KTileSz 100;

// Optimize for throughput
Constant MTileSz 1;
Constant NTileSz 1;
Constant KTileSz 100;

Network BLAS3 {
	Layer BLAS {
		Type: GEMM
		Dimensions { K: SzK, M: SzM, N: SzN }
		Dataflow {
			TemporalMap(NTileSz,NTileSz) N;
			SpatialMap(MTileSz,MTileSz) M;
			TemporalMap(KTileSz,KTileSz) K;
			Cluster(KTileSz, P);
			TemporalMap(NTileSz,NTileSz) N;
			TemporalMap(MTileSz,MTileSz) M;
			SpatialMap(1,1) K;
		}
	}
}

// Network BLAS3 {
//	Layer BLAS {
//		Type: CONV
//		Stride { X: 1, Y: 1 }
//		Dimensions { K: SzN, C: 1, R: 1, S: SzK, Y: SzM, X: SzK }
//		Dataflow {
//			TemporalMap(NTileSz,NTileSz) K;
//			SpatialMap(MTileSz,MTileSz) Y;
//			TemporalMap(SzK,SzK) X;
//			TemporalMap(SzK,SzK) S;
//			Cluster(1,P);
//			SpatialMap(KTileSz,KTileSz) X;
//			TemporalMap(KTileSz,KTileSz) S;
//			Cluster(KTileSz,P);
//			SpatialMap(1,1) X;
//			SpatialMap(1,1) S;
//		}
// 	}
// }


function data = standardisedata(data,T,standardise,valid_dims)

N = length(T);

if standardise
    for i = 1:N
        t = (1:T(i)) + sum(T(1:i-1));
        if isstruct(data)
            data.X(t,:) = bsxfun(@minus,data.X(t,:),mean(data.X(t,:)));
            sdx = std(data.X(t,:));
            if any(sdx==0)
                error('At least one of the trials/segments/subjects has variance equal to zero (use cleandata4hmm?)');
            end
            data.X(t,:) = bsxfun(@rdivide,data.X(t,:),sdx);
        else
            data(t,:) = bsxfun(@minus,data(t,:),mean(data(t,:)));
            sdx = std(data(t,:));
            if any(sdx==0)
                error('At least one of the trials/segments/subjects has variance equal to zero (use cleandata4hmm?)');
            end
            data(t,:) = bsxfun(@rdivide,data(t,:),sdx);
        end
    end
else
    if nargin<4 % this is to avoid the function complaining when TUDA is used
        if isstruct(data)
            valid_dims = [1:size(data.X,2)];
        else
            valid_dims = [1:size(data,2)];
        end
    end
    for i = 1:N
        t = (1:T(i)) + sum(T(1:i-1));
        if isstruct(data)
            if any(std(data.X(t,valid_dims))==0)
                error('At least one of the trials/segments/subjects has variance equal to zero (use cleandata4hmm?)');
            end
        else
            if any(std(data(t,valid_dims))==0)
                error('At least one of the trials/segments/subjects has variance equal to zero (use cleandata4hmm?)');
            end
        end
    end
end

end


function [infstack] = Intf_filt(infstack,SHP,phi_PL,Coh_cal,reference_ind,BroNumthre,Cohthre)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   This file is part of TomoSAR.
%
%   TomoSAR is distributed in the hope that it will be useful,
%   but without warranty of any kind; without even the implied
%   warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
%   See the Apache License for more details.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Author : Dinh Ho Tong Minh (INRAE) and Yen Nhi Ngo, Jan. 2022
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if not(exist('BroNumthre', 'var'))
     BroNumthre=5;
end

if not(exist('Cohthre', 'var'))
     Cohthre=0.65;
end

[~,~,n_interf] = size(infstack);

phi_PL(:,:,reference_ind) = [];
mask_coh = Coh_cal > Cohthre;
mask_PS = SHP.BroNum>BroNumthre;  %PS keep

mask = and(mask_PS,mask_coh);
mask = repmat(mask,[1,1,n_interf]);

infstack(mask) = abs(infstack(mask)).*exp(1i*phi_PL(mask));


function cost = computecost(f,c,Q,x,p)

if isempty(f)
    f = 0;
end
cost = f+c'*x+x'*Q*x;
if ~isequal(p.K.m,0)
    top = size(p.F_struc,1)-sum(p.K.m.^2)+1;
    for i = 1:length(p.K.m)
        X = p.F_struc(top:top + p.K.m(i)^2-1,:)*[1;x];
        X = reshape(X,p.K.m(i),p.K.m(i));
        cost = cost + p.K.maxdetgain(i)*sum(real(log(real(eig(X)))));
        top = top + p.K.m(i)^2;
    end
end


function rowMatrix = computeRowsOfToeplitzHankelMatrix(rowNumber,...
        nColumns, crossCorrelationVectors, hankelMatrixIsAdded, dcIsIncluded)
    if rowNumber == 1
        toeplitzRows = crossCorrelationVectors(1:nColumns,:);
    else
        toeplitzRows = ...
            [flip(crossCorrelationVectors(2:rowNumber,:),1);...
            crossCorrelationVectors(1:nColumns-rowNumber+1,:)];
    end

    if dcIsIncluded && hankelMatrixIsAdded
        hankelOffset = 1;
    else
        hankelOffset = 3;
    end

    hankelRows = crossCorrelationVectors((0:nColumns-1)+...
                                         hankelOffset+rowNumber-1,:);

    if hankelMatrixIsAdded
        rowMatrix = toeplitzRows + hankelRows;
    else
        rowMatrix = toeplitzRows - hankelRows;
    end

end