function a = r82row_permute ( n, a, p )

%*****************************************************************************80
%
%% R82ROW_PERMUTE permutes an R82ROW in place.
%
%  Discussion:
%
%    An R82ROW is a (2,N) array of R8's.
%
%    This routine permutes an array of real "objects", but the same
%    logic can be used to permute an array of objects of any arithmetic
%    type, or an array of objects of any complexity.  The only temporary
%    storage required is enough to store a single object.  The number
%    of data movements made is N + the number of cycles of order 2 or more,
%    which is never more than N + N/2.
%
%  Example:
%
%    Input:
%
%      N = 5
%      P = (   2,    4,    5,    1,    3 )
%      A = ( 1.0,  2.0,  3.0,  4.0,  5.0 )
%          (11.0, 22.0, 33.0, 44.0, 55.0 )
%
%    Output:
%
%      A    = (  2.0,  4.0,  5.0,  1.0,  3.0 )
%             ( 22.0, 44.0, 55.0, 11.0, 33.0 ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of objects.
%
%    Input, real A(2,N), the array to be permuted.
%
%    Input, integer P(N), the permutation.  P(I) = J means
%    that the I-th element of the output array should be the J-th
%    element of the input array.  P must be a legal permutation
%    of the integers from 1 to N, otherwise the algorithm will
%    fail catastrophically.
%
%    Output, real A(2,N), the permuted array.
%

%
%  Search for the next element of the permutation that has not been used.
%
  for istart = 1 : n

    if ( p(istart) < 0 )

      continue

    elseif ( p(istart) == istart )

      p(istart) = - p(istart);
      continue

    else

      a_temp(1:2) = a(1:2,istart);
      iget = istart;
%
%  Copy the new value into the vacated entry.
%
      while ( 1 )

        iput = iget;
        iget = p(iget);

        p(iput) = - p(iput);

        if ( iget < 1 || n < iget )
          fprintf ( 1, '\n' );
          fprintf ( 1, 'R82ROW_PERMUTE - Fatal error!\n' );
          fprintf ( 1, '  A permutation index is out of range.\n' );
          fprintf ( 1, '  P(%d) = %d\n', iput, iget );
          error ( 'R82ROW_PERMUTE - Fatal error!' );
        end

        if ( iget == istart )
          a(1:2,iput) = a_temp(1:2)';
          break
        end

        a(1:2,iput) = a(1:2,iget);

      end

    end

  end
%
%  Restore the signs of the entries.
%
% p(1:n) = -p(1:n);

  return
end


function g = linardKernGradient(kern, x, varargin)

% LINARDKERNGRADIENT Gradient of LINARD kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% automatic relevance determination linear
% kernel's parameters. As well as the kernel structure and the
% input positions, the user provides a matrix PARTIAL which gives
% the partial derivatives of the function with respect to the
% relevant elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x : the input locations for which the gradients are being
% computed.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in X.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations associated with the rows of the
% kernel matrix.
% ARG x2 : the input locations associated with the columns of the
% kernel matrix.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as X1 and the same number of columns
% as X2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% SEEALSO linardKernParamInit, kernGradient, linardKernDiagGradient, kernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006, 2009

% KERN

  g = zeros(1, size(x, 2)+1);
  if nargin < 4
    [k, sk] = linardKernCompute(kern, x);
  else
    [k, sk] = linardKernCompute(kern, x, varargin{1});
  end
  g(1) = sum(sum(varargin{end}.*sk));
  if nargin < 4
    for i = 1:size(x, 2)
      g(1+i) =  x(:, i)'*varargin{end}*x(:, i)*kern.variance;
    end
  else
    for i = 1:size(x, 2)
      g(1+i) =  x(:, i)'*varargin{end}*varargin{1}(:, i)*kern.variance;
    end
  end
end


function faces = compute_voronoi_triangulation(Q, vertex)

% compute_voronoi_triangulation - compute a triangulation
%
%   face = compute_voronoi_triangulation(Q);
%
%   Q is a Voronoi partition function, computed using
%       perform_fast_marching.
%   face(:,i) is the ith face.
%
%   Works in 2D and in 3D.
%
%   Copyright (c) 2008 Gabriel Peyre

d = nb_dims(Q);

if d==2
    faces = compute_voronoi_triangulation_2d(Q, vertex);
elseif d==3
    faces = compute_voronoi_triangulation_3d(Q, vertex);
else
    error('Works only for 2D or 3D data.');
end

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function faces = compute_voronoi_triangulation_3d(Q, vertex)

[dx dy dz] = meshgrid(0:1,0:1,0:1);
V = [];
for i=1:8
    v = Q(1+dx(i):end-1+dx(i),1+dy(i):end-1+dy(i),1+dz(i):end-1+dz(i));
    V = [V v(:)];
end

V = sort(V,2);
V = unique(V, 'rows');
V = V( prod(V,2)>0 ,:);

d = V(:,1)*0;
for i=1:7
   d = d+(V(:,i)~=V(:,i+1));
end
if sum(d==4)>1
%    warning('Problem with triangulation.');
end

% split 5 folds into 2
I = find(d==4);
faces = [];
for i=1:length(I)
    v = unique(V(I(i),:));
    x = vertex(:,v); % points
    % barycenter
    a = sum( x, 2 ) / 5;
    x = x-repmat(a, [1 size(x,2)]);
    t = atan2(x(2,:),x(1,:));
    [tmp,s] = sort(t);
    faces = [faces v( s([1 2 3 5]))'];
    faces = [faces v( s([3 4 1 5]))'];

end
%    faces = [V(I,1:3);V(I,[3 4 1])];

% remaining triangles
V = V( d==3 ,:);
for i=1:size(V,1)
    V(i,1:4) = unique(V(i,:));
end
faces = [faces, V(:,1:4)'];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function faces = compute_voronoi_triangulation_2d(Q, vertex)

V = [];
v = Q(1:end-1,1:end-1); V = [V v(:)];
v = Q(2:end,1:end-1); V = [V v(:)];
v = Q(1:end-1,2:end); V = [V v(:)];
v = Q(2:end,2:end); V = [V v(:)];

V = sort(V,2);
V = unique(V, 'rows');
V = V( prod(V,2)>0 ,:);

d = (V(:,1)~=V(:,2)) + (V(:,2)~=V(:,3)) + (V(:,3)~=V(:,4));
if sum(d==3)>1
    warning('Problem with triangulation.');
end

% split squares into 2 triangles
I = find(d==3);
faces = [];
for i=1:length(I)
    v = V(I(i),:);
    x = vertex(:,v); % points
    % barycenter
    a = sum( x, 2 ) / 4;
    x = x-repmat(a, [1 size(x,2)]);
    t = atan2(x(2,:),x(1,:));
    [tmp,s] = sort(t);
    faces = [faces; v( s([1 2 3]))];
    faces = [faces; v( s([3 4 1]))];

end
%    faces = [V(I,1:3);V(I,[3 4 1])];

% remaining triangles
V = V( d==2 ,:);
for i=1:size(V,1)
    V(i,1:3) = unique(V(i,:));
end
faces = [faces; V(:,1:3)];

% return in correct format
faces = faces';


function newTree = expandTree(tree, maxOrder)
%EXPANDTREE   Convert expressions like 5*(diff(u) + u) to 5*diff(u) + 5*u.
%   The role of the EXPANDTREE method is to split up syntax trees, so that
%   expressions involving the highest derivative appearing in the equation stand
%   alone, that is, not inside parenthesis. For example, EXPANDTREE will convert
%   expressions like 5*(diff(u) + u) to 5*diff(u) + 5*u. This is necessary in
%   order to be able to put the correct expression on the right-hand side when
%   calling MATLAB's built-in solvers.
%
%   Calling sequence:
%      NEWTREE = EXPANDTREE(TREE, MAXORDER)
%   where the inputs are:
%      TREE:       The syntax tree we want to split.
%      MAXORDER:   A vector containing the maximum differential order of each
%                  variable that appear in the problem under consideration.
%   and the output is:
%      NEWTREE:    A syntax tree where the highest order deriative has been
%                  taken out of any parenthesis where other variables with lower
%                  differential order appear.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

if ( ~isstruct(tree) || tree.height <= 1 || all(tree.diffOrder < maxOrder) ||...
    ( ~tree.hasTerms && sum(tree.ID) <= 1) )
    % We don't need to worry about expanding the tree any of the following
    % conditions apply:
    %   * The input is not a tree
    %   * It is a very short tree
    %   * Its diffOrder is less than the maxOrder we consider
    %   * It doesn't any operators that split it up in terms (that is, + or -),
    %     and it doesn't include more than one variable (which would imply
    %     nonlinearity, such as u.*w).
    newTree = tree;

elseif ( tree.numArgs == 1 )
    % If we have a unary operator, we expand the center node recursively.
    newTree = expandTree(tree.center, maxOrder);

elseif ( any(strcmp(tree.method, {'plus', 'minus'})) )
    % We're at a + or a -, so expand the syntax tree recursively:
    tree.left = treeVar.expandTree(tree.left, maxOrder);
    tree.right = treeVar.expandTree(tree.right, maxOrder);
    newTree = tree;

else
    % Must be at .*, ./ or .^.

    % If both left and right arguments to the operator are syntax trees, and the
    % highest order derivative appear in any, we know that we must have
    % nonlinearities in the highest order derivative, e.g. in the case of a
    % first order IVP, u.*diff(u). This is not supported, so throw an error:
    if ( isstruct(tree.left) && isstruct(tree.right) && ...
            ( any(tree.diffOrder == maxOrder) ) )
        error('CHEBFUN:TREEVAR:expandTree:nonlinearity', ...
            ['Nonlinearity in highest order derivative detected.\n' ...
            'Unable to convert to first order format.']);
    end

    % Find out what kind of argument we're dealing with for the left argument
    % of the operator
    if ( ~isstruct(tree.left) )
        % Left argument is not a syntax tree, must be either a CHEBFUN or a
        % scalar. In either case, the left tree has zero children.
        leftNumArgs = 0;
    else
        % Left tree is indeed a syntax tree, store its number of children.
        leftNumArgs = tree.left.numArgs;
    end

    % Find out what kind of argument we're dealing with for the right argument
    % of the operator:
    if ( ~isstruct(tree.right) )
        % Right argument is not a syntax tree, must be either a CHEBFUN or a
        % scalar. In either case, the right tree has zero children.
        rightDiffOrder = 0;
        rightNumArgs = 0;
    else
        % Left tree is indeed a syntax tree, store its number of children.
        rightNumArgs = tree.right.numArgs;
    end

    % Split up the left tree depending on how many children it has. In case we
    % don't need to split at the current level, we store the differential
    % order and the height of the left tree in the variables LEFTDIFFORDER
    % and LEFTHEIGHT. Finally, we introduce the Boolean variables SPLITLEFT
    % which indicates whether the left tree had to be split:
    if ( leftNumArgs == 0 || ...
            (strcmp(tree.left.method,'diff') && tree.left.height <= 1 )  )
        % We're either dealing with a CHEBFUN/scalar left tree, or it's
        % differentiating one of the basis variables directly (as that's the
        % only case that tree.left.height <= 1). In this case, no splitting is
        % needed.
        leftTree = tree.left;
        leftDiffOrder = 0*tree.diffOrder;
        leftHeight = 0;
        splitLeft = false;
    elseif ( leftNumArgs == 1 )
        % Here we're dealing with the unary operator case. In this case, we
        % don't need to split at the current level, however, we recursively
        % split up the child tree.
        leftTree = treeVar.expandTree(tree.left, maxOrder);
        leftDiffOrder = leftTree.diffOrder;
        leftHeight = leftTree.height;
        splitLeft = false;
    else
        % Here we're dealing with the binary operator case. In this case, we do
        % need to split at the current level, as well as recursively splitting
        % up both the child trees. LEFTLEFT and LEFTRIGHT will be the syntax
        % trees stored in the left child of the current level, once we've split
        % up the child trees of the current level.
        leftLeft = treeVar.expandTree(tree.left.left, maxOrder);
        leftRight = treeVar.expandTree(tree.left.right, maxOrder);
        splitLeft = true;
    end

    % Split up the right tree depending on how many children it has. In case we
    % don't need to split at the current level, we store the differential order
    % and the height of the right tree in the variables RIGHTDIFFORDER and
    % RIGHTHEIGHT. Finally, we introduce the Boolean variables SPLITRIGHT which
    % indicates whether the right tree had to be split:
    if ( rightNumArgs == 0 || ...
            (strcmp(tree.right.method,'diff') && tree.right.height <= 1 )  )
        % We're either dealing with a CHEBFUN/scalar right tree, or it's
        % differentiating one of the basis variables directly (as that's the
        % only case that tree.right.height <= 1). In this case, no splitting is
        % needed.
        rightTree = tree.right;
        rightDiffOrder = 0*tree.diffOrder;
        rightHeight = 0;
        splitRight = false;
    elseif ( rightNumArgs == 1 )
        % He're we're dealing with the unary operator case. In this case, we
        % don't need to split at the current level, however, we recursively
        % split up the child tree.
        rightTree = treeVar.expandTree(tree.right, maxOrder);
        rightDiffOrder = rightTree.diffOrder;
        rightHeight = 0;
        splitRight = false;
    else
        % He're we're dealing with the binary operator case. In this case, we do
        % need to split at the current level, as well as recursively splitting
        % up both the child trees. RIGHTLEFT and RIGHTRIGHT will be the syntax
        % trees stored in the right child of the current level, once we've split
        % up the child trees of the current level.
        rightLeft  = treeVar.expandTree(tree.right.left, maxOrder);
        rightRight = treeVar.expandTree(tree.right.right, maxOrder);
        % Only need to worry about splitting if both left and right trees are
        % syntax trees, not if we have CHEBFUN/scalars
        splitRight = true;
    end

    % If we had to split either the left tree or the right tree, construct new
    % syntax trees which we'll then glue together below. Note that we should
    % never expect to split both the left and right trees, as that indicates
    % nonlinearities in the highest order derivative.
    if ( ~splitLeft && ~splitRight )
        % The simple case, no splitting required, so the new left and right
        % syntax trees will be the current ones:
        newLeft = leftTree;
        newRight = rightTree;

    elseif ( splitLeft && ~splitRight )
        % Had to split on the left, not the right.

        % Multiply together the new left factor of the left tree, and the
        % current right tree:
        newLeft = struct('method','times', 'numArgs', 2, ...
            'left', leftLeft, ...
            'right', rightTree,...
            'diffOrder', max(getDifforder(leftLeft), rightDiffOrder), ...
            'height', max(getHeight(leftLeft), rightHeight) + 1, ...
            'hasTerms', getHasTerms(leftLeft) || getHasTerms(rightTree) );

        % Multiply together the new right factor of the left tree, and the
        % current right tree:
        newRight = struct('method','times', 'numArgs', 2, ...
            'left', leftRight, ...
            'right', rightTree, ...
            'diffOrder', max(getDifforder(leftRight), rightDiffOrder) + 1, ...
            'height', max(getHeight(leftRight), rightHeight) + 1, ...
            'hasTerms', getHasTerms(leftRight) || getHasTerms(rightTree) );

    elseif ( ~splitLeft && splitRight )
        % Had to split on the right, not the left.

        % Multiply together the current left tree, and the new left factor of
        % the right tree:
        newLeft = struct('method','times', 'numArgs', 2, ...
            'left', leftTree, ...
            'right', rightLeft,...
            'diffOrder', max(leftDiffOrder, getDifforder(rightLeft)), ...
            'height', max(leftHeight, getHeight(rightLeft)) + 1, ...
            'hasTerms', getHasTerms(leftTree) || getHasTerms(rightLeft) );

        % Multiply together the current left tree, and the new right factor of
        % the right tree:
        newRight = struct('method','times', 'numArgs', 2, ...
            'left', leftTree, ...
            'right', rightRight,...
            'diffOrder', max(leftDiffOrder, getDifforder(rightRight)), ...
            'height', max(leftHeight, getHeight(rightRight) + 1), ...
            'hasTerms', getHasTerms(leftTree) || getHasTerms(rightRight) );

    end

    % If the left or right trees still have multiple terms left in them, we need
    % to split them up recursively:
    if ( newLeft.hasTerms )
        newLeft = treeVar.expandTree(newLeft, maxOrder);
    end
    if ( newRight.hasTerms )
        newRight = treeVar.expandTree(newRight, maxOrder);
    end

    % Add together the new left and right trees, splitting complete!
    newTree = struct('method', 'plus', 'numArgs', 2, ...
        'left', newLeft, 'right', newRight, ...
        'diffOrder', max(newLeft.diffOrder, newRight.diffOrder), ...
        'height',max(newLeft.height, newRight.height) + 1, ...
        'hasTerms', newLeft.hasTerms || newRight.hasTerms);
end
end

function out = getDifforder(treeIn)
%GETDIFFORDER   The diffOrder of a TREEVAR.
%   We often encounter child nodes that may or may not be a syntax tree, as
%   opposed to chebfuns or scalars. We are usually interested in the heights
%   and diffOrders of the syntax trees, however, if the tree is actually a
%   CHEBFUN/scalar, they won't have the necessary fields. This method takes
%   care of checking the class of the argument, and return the correct
%   information.
if ( isstruct(treeIn) )
    out = treeIn.diffOrder;
else
    out = 0;
end

end

function out = getHeight(treeIn)
%GETHEIGHT   The height of a TREEVAR.
%   Same as GETDIFFORDER() method but for the height of a syntax tree.
if ( isstruct(treeIn) )
    out = treeIn.height;
else
    out = 0;
end

end

function out = getHasTerms(treeIn)
%GETHASTERMS   Test if a TREEVAR contains a 'plus' or 'minus' node.
%   Same as GETDIFFORDER() method but for whether the syntax tree consists of
%   multiple terms.
if ( isstruct(treeIn) )
    out = treeIn.hasTerms;
else
    out = 0;
end

end


function prediction = predict(model,Xt,n, simfct, args)
% Iterative prediction of a trained LS-SVM NARX model (in recurrent mode)
%
% >> Yp = predict({Xw,Yw,type,gam,sig2}, Xt, nb)
% >> Yp = predict(model,                 Xt, nb)
%
% The model needs to be trained using Xw, Yw which is the result of
% windowize or windowizeNARX. The number of time lags for the model
% is determined by the dimension of the input, or if not
% appropriate, by the number of given starting values.
%
% By default, the model is evaluated on the past points using
% simlssvm. However, if one wants to use this procedure for other
% models, this default can be overwritten by your favorite training
% function. This function (denoted by simfct) has to follow the following syntax:
%
% >> simfct(model,inputs,arguments)
%
% thus:
%
% >> Yp = predict(model, Xt, nb, simfct)
% >> Yp = predict(model, Xt, nb, simfct, arguments)
%
%
% Full syntax
%
%     1. Using the functional interface for the LS-SVMs:
%
% >> Yp = predict({Xw,Yw,type,gam,sig2,kernel,preprocess}, Xt, nb)
%
%       Outputs
%         Yp            : nb x m matrix with the predictions
%       Inputs
%         Xw            : N x d matrix with the inputs of the training data
%         Yw            : N x w matrix with the outputs of the training data
%         type          : 'function estimation' ('f') or 'classifier' ('c')
%         gam           : Regularization parameter
%         sig2          : Kernel parameter (bandwidth in the case of the 'RBF_kernel')
%         kernel(*)     : Kernel type (by default 'RBF_kernel')
%         preprocess(*) : 'preprocess' or 'original' (by default)
%         Xt            : nb*d matrix of the starting points for the prediction
%         nb(*)         : Number of outputs to predict
%
%
%     2. Using the object oriented interface with LS-SVMs:
%
% >> Yp = predict(model, Xt, nb)
%
%       Outputs
%         Yp    : nb x m matrix with the predictions
%       Inputs
%         Xt    : nb x d matrix of the starting points for the prediction
%         nb(*) : Number of outputs to predict
%
%
%     3. Using another model:
%
% >> Yp = predict(model, Xt, nb, simfct, arguments)
%
%       Outputs
%         Yp           : nb x m matrix with the predictions
%       Inputs
%         Xt           : nb x d matrix of the starting points for the prediction
%         nb           : Number of outputs to predict
%         simfct       : Function used to evaluate a test point
%         arguments(*) : Cell with the extra arguments passed to simfct
%
% See also:
%   windowize, trainlssvm, simlssvm.

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

if size(Xt,2)~=1 & size(Xt,1)~=1,
  error('prediction only implemented for the one-dimensional autonomous case');
end
eval('model = initlssvm(model{:});',' ');
eval('xdim = model.x_dim;','xdim = max(size(Xt)); ');
eval('n; alle=0;','n=length(Xt)-xdim; alle=1;');
Xt = Xt(1:xdim);
Xt = reshape(Xt,length(Xt),1);
eval('simfct;','simfct=''simlssvm'';');
eval('model = trainlssvm(model);');

xdelays = length(Xt);
prediction = zeros(xdelays+n,1);
prediction(1:xdelays) = Xt;

% closed loop
eval('for i=1:n,  prediction(xdelays+i) = feval(simfct,model, prediction(i-1+(1:xdelays))'',args); end',...
     'for i=1:n,  prediction(xdelays+i) = feval(simfct,model,prediction(i-1+(1:xdelays))''); end');

if ~alle,
  prediction = prediction(xdelays+1:end);
end


function h = min(f, g, dim)
%MIN   Minimum value of a CHEBFUN3 in one direction.
%   MIN(F) returns a CHEBFUN2 representing the minimum of the CHEBFUN3
%   object F along the x direction, i.e, MIN(F) = @(y,z) min(F(:, y, z)).
%
%   MIN(F, [], DIM) returns a CHEBFUN2 representing the minimum of F along
%   the dimension DIM. DIM should be 1, 2 or 3 to minimize over X, Y or Z,
%   respectively.
%
%   WARNING: This function is not always accurate to the expected precision.
%
% See also CHEBFUN3/MIN2 and CHEBFUN3/MIN3.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Empty check:
if ( isempty(f) )
    error('CHEBFUN:CHEBFUN3:min:input', 'CHEBFUN3 is empty');
end

% Default to min of one chebfun3:
if ( nargin < 2 )
    g = [];
end

% Default to minimum along the x direction:
if ( nargin < 3 )
    dim = 1;
end

% Do not allow min(F, G):
if ( nargin > 1 && ~isempty(g) )
    error('CHEBFUN:CHEBFUN3:min:twoCHEBFUN3Inputs', ...
        'Unable to minimize two CHEBFUN3 objects.');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% We have no idea how to achieve this in an efficient way. This
% is an attempt to return a result, but typically it won't be accurate to
% more than 4-5 digits.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dom = f.domain;
n = 129;
if ( dim == 1 )
    vals = sample(f, n, n, n);
    h = chebfun2(squeeze(min(vals, [], 1)), dom(3:6));
    h = simplify(h);

elseif ( dim == 2 )
    vals = sample(f, n, n, n);
    h = chebfun2(squeeze(min(vals, [], 2)), [dom(1:2), dom(5:6)]);
    h = simplify(h);

elseif ( dim == 3 )
    vals = sample(f, n, n, n);
    h = chebfun2(min(vals, [], 3), dom(1:4));
    h = simplify(h);

elseif ( dim == 0 )
    error('CHEBFUN:CHEBFUN3:min:dim', 'dim must be 1, 2, or 3.')
else
   % Return the CHEBFUN3 object. This is analogous to that MIN() command in
   % MATLAB.
   h = f;
end

end


function [imageCha, ssim_map, metrics] = algo_ADMM_proxA_2D( obj,input )
% 2D variant of ADMM
% based on Afonso et al. paper on SALSA
%
% input:
% obj           CS reconstruction object (holding all parameters)
% input         struct containing recon parameters and image
%
% output:
% imageCha      reconstructed channel individual image
% ssim_map      structural similarity map
% metrics       evaluation metrics
%
% (c) Marc Fischer, Thomas Kuestner, May 2015
% -------------------------------------------------------------------------

%%
timer_proxA = tic;

%% variables:
% internal flags
flag_wavetree = true;
flag_fast = true;
flag_extendImage = true;

% internal variables:
L = 1;
t_old = 1;
NLTV_struct.kernelratio = 3;
NLTV_struct.windowratio = 6;
NLTV_struct.nThreads = 1;  % mind this option if used with -singleCompThread on BWCluster
itrNLTV = obj.iNINNER - 20;
% chambolle tv:
parsin.MAXITER=100; parsin.tv='iso'; % 'iso' or 'l1'

% from obj:
% maxitr = obj.maxitr;
maxitr = obj.iNINNER;
% n1 = obj.measPara.dim(1);
% n2 = obj.measPara.dim(2);
n1 = input.n1;
n2 = input.n2;
% nSlices = obj.measPara.dim(3);
nCha = obj.measPara.dim(5);
lambdaWave = obj.lambda;
lambdaTV = obj.lambdaTV;
lambdaGroup = obj.lambdaGroup;
NLTV_struct.filterstrength = obj.lambdaNLTV_h; % 0.03 % converted from NLTV h = 0.01 %old: used: 3e-10
lambdaNLTV = obj.lambdaNLTV;
lambdaNLTV_h = obj.lambdaNLTV_h;
mue = obj.mue;
regularizerWeights = obj.regularizerWeights;
flagTV = obj.flagTV;
flagTV_iso = obj.flagTV_iso;
flagWave = obj.flagWave;
flagGroup = obj.flagGroup;
flagNLTV = obj.flagNLTV;
flagSBNLTV = obj.flagSBNLTV;
flagRealAndImag = obj.flagRealAndImag;
waveletStages = obj.trafo.waveletStages;
waveletFilterName_l1 = obj.trafo.waveletFilterName_l1;
waveletFilterName_l12 = obj.trafo.waveletFilterName_l12;

% from input:
b=input.b;
mask = input.mask;
G_prox = input.G_prox;
Gt_prox = input.Gt_prox;
groupnorm_index = input.groupnorm_index;
waveS_l1 = input.waveS_l1;
waveS_l12 = input.waveS_l12;
waveS_l12proxA = input.waveS_l12proxA;
proxA_extend_y = waveS_l12proxA(waveletStages+2,1) - waveS_l12(waveletStages+2,1);
proxA_extend_x = waveS_l12proxA(waveletStages+2,2) - waveS_l12(waveletStages+2,2);
x_proxA = cell(1,nCha);

% im_ref = input.im_ref;
% im_ref_full = zeros(n1,n2);
% for j = 1:nCha
%     im_ref_full = im_ref_full + abs(im_ref{1,j}).^2;
% end;
% im_ref_full = sqrt(im_ref_full);
clear input

% initialize cells/vectors
for j=1:nCha
    FTb{1,j} = iFFT2D(b{1,j});
    x_cmplx{1,j} = real(FTb{1,j});
    x_cmplx{1,j+nCha} = imag(FTb{1,j});
end;
% starting point:
x = FTb; % (x0 = FTb)
rhs = FTb; % different start (x0 = F^-1 K^-1 F FTb); %used atm
% rhs: right hand side

x_g_helper = cell(2,nCha);
x_wave =  cell(1,2*nCha);
x_g_proxA = x_wave;
x_wave_helper = x_wave;
v_wave = x_wave;
d_wave = x_wave;
x_tv = x_wave;
v_tv = x_wave;
d_tv = x_wave;
x_group = x_wave;
v_group = x_wave;
d_group = x_wave;
x_nltv = x_wave;
v_nltv = x_wave;
d_nltv = x_wave;
K_inv = x_wave;
for j = 1:2*nCha
    x_wave{1,j} = 0;
    v_wave{1,j} = 0;
    d_wave{1,j} = 0;
    x_tv{1,j} = zeros(n1,n2);
    v_tv{1,j} = 0;
    d_tv{1,j} = 0;
    x_group{1,j} = 0;
    v_group{1,j} = 0;
    d_group{1,j} = 0;
    x_nltv{1,j} = 0;
    v_nltv{1,j} = 0;
    d_nltv{1,j} = 0;
end;

%% MAD dependent lambdas:
flag_MAD = true;
if flag_MAD
    x_wavedec = cell(1,nCha);
    threshold = zeros(1:nCha);
    if flagRealAndImag
        for j=1:nCha
            x_wavedec{1,j} = wavedec2(abs(x{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wave_fine_scale = size(x_wavedec{1,j},2) - (3*waveS_l1(waveletStages+1,1)*waveS_l1(waveletStages+1,2));
            threshold(j) = mad(x_wavedec{1,j}(x_wave_fine_scale:end),1);
        end;
    else
        for j=1:nCha
            x_wavedec{1,j} = wavedec2(real(x{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wavedec{1,j+nCha} = wavedec2(imag(x{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wave_fine_scale_real = size(x_wavedec{1,j},2) - (3*waveS_l1(waveletStages+1,1)*waveS_l1(waveletStages+1,2));
            threshold(j) = mad(x_wavedec{1,j}(x_wave_fine_scale_real:end),1);
            x_wave_fine_scale_imag = size(x_wavedec{1,j},2) - (3*waveS_l1(waveletStages+1,1)*waveS_l1(waveletStages+1,2));
            threshold(j+nCha) = mad(x_wavedec{1,j}(x_wave_fine_scale_imag:end),1);
        end;
    end;
    clear x_wavedec
else
    threshold(1:nCha) = 1;
end;

if flagRealAndImag
    threshold_group = 0;
    for j = 1:nCha
        threshold_wave(j) = lambdaWave * threshold(j)/2 * 2/L;
        threshold_TV(j) = lambdaTV * threshold(j) * 2/L;
        threshold_TV(j+nCha) = lambdaTV * threshold(j) * 2/L;
        threshold_group = threshold_group + lambdaGroup * threshold(j) * 2/L;
        threshold_NLTV(j) = lambdaNLTV; % * threshold(j) * 2/L;
        threshold_NLTV(j+nCha) = lambdaNLTV;
        threshold_NLTV_h(j) = lambdaNLTV_h; %*threshold(j); % adjust carefully or NLTV won't find a solution. lambdaNLTV_h should be < 0.01
        threshold_NLTV_h(j+nCha) = lambdaNLTV_h;
    end;
    threshold_group = threshold_group /3;
else
    threshold_group_real = 0;
    threshold_group_imag = 0;
    for j = 1:nCha
        threshold_wave(j) = lambdaWave * threshold(j) * 2/L;
        threshold_wave(j+nCha) = lambdaWave * threshold(j+nCha) * 2/L;
        threshold_TV(j) = lambdaTV * threshold(j) * 2/L;
        threshold_TV(j+nCha) = lambdaTV * threshold(j+nCha) * 2/L;
        threshold_group_real = threshold_group_real + lambdaGroup * threshold(j) * 2/L;
        threshold_group_imag = threshold_group_imag + lambdaGroup * threshold(j) * 2/L;
        threshold_NLTV(j) = lambdaNLTV; % * threshold(j) * 2/L;
        threshold_NLTV(j+nCha) = lambdaNLTV;
        threshold_NLTV_h(j) = lambdaNLTV_h; %*threshold(j); % adjust carefully or NLTV won't find a solution. lambdaNLTV_h should be < 0.01
        threshold_NLTV_h(j+nCha) = lambdaNLTV_h;
    end;
    threshold_group_real = threshold_group_real /3;
    threshold_group_imag = threshold_group_imag /3;
end;

%% initialize metrics:
    itr = 0;
   	metrics.xtime(itr+1)= 0;
%     [metrics, ssim_map{1,1}] = get_metrics_itr( im_ref, im_ref_full, x, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma );
    ssim_map = [];

%% recon
% inverse for x - step:
for j = 1:nCha
    K_inv{1,j} = 1./(mue*mask + (flagWave*regularizerWeights(1) + flagTV*regularizerWeights(2) +  flagGroup*regularizerWeights(3))*ones(n1,n2)); % flagNLTV changes K_inv -> in separate
end;

dispProgress('ADMM', 0, maxitr);
for itr = 1:maxitr  % total iter counter

%% x - step
    if itr > 1 % for i = 1 already set (x{1,j} = FTb{1,j})
        for j = 1:2*nCha
            % rhs part:
            if flagWave
                x_wave{1,j} = waverec2((v_wave{1,j} - d_wave{1,j}),waveS_l1,waveletFilterName_l1); end;
            if flagTV
                x_tv{1,j} = v_tv{1,j} - d_tv{1,j}; end;
            if flagGroup
                x_g_proxA{1,j} = waverec2((v_group{1,j} - d_group{1,j}),waveS_l12proxA,waveletFilterName_l12);
                x_group{1,j} =  x_g_proxA{1,j}(1:end-proxA_extend_y,1:end-proxA_extend_x);
            end;
            if flagNLTV
                if itr == itrNLTV
                    K_inv{1,j} = 1./(mue*mask + (flagWave*regularizerWeights(1) + flagTV*regularizerWeights(2) +  flagGroup*regularizerWeights(3) + flagNLTV*regularizerWeights(4))*ones(n1,n2));
                    x_nltv{1,j} = x{1,j};
                elseif itr >= itrNLTV
                    x_nltv{1,j} = v_nltv{1,j} - d_nltv{1,j};
                end;
            end;
        end;
        for j = 1:nCha
            rhs{1,j} = mue*FTb{1,j} + ( flagWave*regularizerWeights(1).*(x_wave{1,j}+1i*x_wave{1,j+nCha}) + flagTV*regularizerWeights(2).*(x_tv{1,j}+1i*x_tv{1,j+nCha}) + flagGroup*regularizerWeights(3).*(x_group{1,j}+1i*x_group{1,j+nCha}) + flagNLTV*regularizerWeights(4).*(x_nltv{1,j}+1i*x_nltv{1,j+nCha}));
            x{1,j} = iFFT2D(K_inv{1,j}.*FFT2D(rhs{1,j}));
            x_cmplx{1,j} = real(x{1,j});
            x_cmplx{1,j+nCha} = imag(x{1,j});
        end;
    end;

%% auxiliary variables:
%% l1-Wavelet
    if flagWave
        for j = 1:2*nCha
            x_wave_helper{1,j} = wavedec2(x_cmplx{1,j},waveletStages,waveletFilterName_l1);
        end;
        if flagRealAndImag
            for j = 1:nCha
                [v_wave{1,j}, v_wave{1,j+nCha}] = groupthresh_2vec(x_wave_helper{1,j} + d_wave{1,j},x_wave_helper{1,j+nCha} + d_wave{1,j+nCha},threshold_wave(j));
            end;
        else
            for j = 1:2*nCha
                v_wave{1,j} = softthresh_real(x_wave_helper{1,j} + d_wave{1,j},threshold_wave(j));
            end;
        end;
        for j = 1:2*nCha
            d_wave{1,j} = d_wave{1,j} + x_wave_helper{1,j} - v_wave{1,j};
        end;
    end;

%% TV:
    if flagTV
        if ~flagTV_iso
            for j = 1:2*nCha
                    v_tv{1,j} = MTV_2D((x_cmplx{1,j} + d_tv{1,j}), threshold_TV(j), n1, n2);
            end;
        else
            for j = 1:2*nCha
                if (itr==1)
                    [v_tv{1,j}, P]=denoise_TV_One((x_cmplx{1,j} + d_tv{1,j}), threshold_TV(j),-inf,inf,[],parsin);
                else
                    [v_tv{1,j}, P]=denoise_TV_One((x_cmplx{1,j} + d_tv{1,j}), threshold_TV(j),-inf,inf,P,parsin);
                end;
            end;
        end;
        d_tv{1,j} = d_tv{1,j} + x_cmplx{1,j} - v_tv{1,j};
    end;

%% NLTV
    if flagNLTV
        if itr >= itrNLTV
            if flagSBNLTV
                for j = 1:2*nCha
                    v_nltv{1,j} = SB_NLTVfunc_slim_rescale( (x_cmplx{1,j} + d_nltv{1,j}),n1,n2, threshold_NLTV(j), threshold_NLTV_h(j) );
                end;
            else
                for j = 1:2*nCha
                    % if mod(itr,5) == 0 || itr == 1
                    v_nltv{1,j} = NLMF((x_cmplx{1,j} + d_nltv{1,j}),NLTV_struct);
                    v_nltv{1,j} = (L.*(x_cmplx{1,j} + d_nltv{1,j}) + 2*threshold_NLTV(j)*v_nltv{1,j})./(L+2*threshold_NLTV(j));
                    % end;
                end;
            end;
            d_nltv{1,j} = d_nltv{1,j} + x_cmplx{1,j} - v_nltv{1,j};
        else
            v_nltv{1,j} = 0;
            d_nltv{1,j} = 0;
        end;

    end;

%% l12-Wavelet
    if flagGroup
        for j = 1:2*nCha
            if flag_extendImage
                x_proxA{1,j} = extend_image(x_cmplx{1,j}, waveS_l12proxA, waveletStages, proxA_extend_y, proxA_extend_x);
                x_g_helper{1,j} = wavedec2(x_proxA{1,j},waveletStages,waveletFilterName_l12) + d_group{1,j};
            else
                x_g_helper{1,j} = wavedec2(x_cmplx{1,j},waveletStages,waveletFilterName_l12) + d_group{1,j};
            end;

            if flag_wavetree
                x_g_helper{2,j} = (G_prox*x_g_helper{1,j}')';
            else
                x_g_helper{2,j} = zeros(1,size(x_g_helper{1,j},2));
            end;
        end;
        if flagRealAndImag
            v_group(1,:) = softthresh_proxA_cha(x_g_helper,threshold_group,2*nCha,waveS_l12proxA,groupnorm_index);
        else
            v_group(1,1:nCha) = softthresh_proxA_cha(x_g_helper(:,1:nCha),threshold_group_real,nCha,waveS_l12proxA,groupnorm_index);
            v_group(1,nCha+1:2*nCha) = softthresh_proxA_cha(x_g_helper(:,1:nCha),threshold_group_imag,nCha,waveS_l12proxA,groupnorm_index);
        end;
        for j = 1:2*nCha
            d_group{1,j} = d_group{1,j} + x_g_helper{1,j} - v_group{1,j};
        end;
    end;

%% metrics of current itr:
%     disp(itr);
    dispProgress('ADMM', itr/maxitr);

   	metrics.xtime(itr+1)= toc(timer_proxA);
%     [metrics, ssim_map{1,2}] = get_metrics_itr( im_ref, im_ref_full, x, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma );

end;
dispProgress('ADMM', 'Close');

    imageCha = x;
    for j = 1:nCha
        imageCha{1,j} = turn_image( imageCha{1,j} );
    end;
%     for j = 1:nCha+1
%         ssim_map{1,1}{1,j} = turn_image( ssim_map{1,1}{1,j} );
%         ssim_map{1,2}{1,j} = turn_image( ssim_map{1,2}{1,j} );
%     end;

end


function exp_map_iteration(vt_id, transV, yawRotV, heightV, xGc, yGc, zGc, curYawHdc, curHeight)
%     NeuroSLAM System Copyright (C) 2018-2019
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com)
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013).
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
%
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
%
%     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/>.

    %% define some variables

    %%% some variables of em

    % define a variable of experiences
    global EXPERIENCES;

    % current experience id
    global CUR_EXP_ID;

    % the number of experiences
    global NUM_EXPS;

    % the min change of em between previous e with current e
    global MIN_DELTA_EM;

    % the experience delta threshold
    global DELTA_EXP_GC_HDC_THRESHOLD;

    %%% some variables of vt

    % define a variable of visual template
    global VT;

    % the previous visual template id
    global PREV_VT_ID;

    %%% some variables of 3D GC

    % The x, y, z dimension of 3D Grid Cells Model (3D CAN)
    global GC_X_DIM;
    global GC_Y_DIM;
    global GC_Z_DIM;

    %%% some variables of 3D HDC

    % The dimension of yaw in yaw_height_hdc network
    global YAW_HEIGHT_HDC_Y_DIM;

    % The dimension of height in yaw_height_hdc network
    global YAW_HEIGHT_HDC_H_DIM;

    %%% computing delta x,y,z, yaw, height

    % integrate the delta x, y, z, yaw, height
    % Rad radian  accumulative
    global ACCUM_DELTA_X;
    global ACCUM_DELTA_Y;
    global ACCUM_DELTA_Z;
    global ACCUM_DELTA_YAW;   % accum_delta_facing
%     global ACCUM_DELTA_HEIGHT;

    ACCUM_DELTA_YAW = clip_radian_180(ACCUM_DELTA_YAW + yawRotV);
%     ACCUM_DELTA_HEIGHT = mod(ACCUM_DELTA_HEIGHT + heightV, YAW_HEIGHT_HDC_H_DIM);
%
    ACCUM_DELTA_X = ACCUM_DELTA_X + transV * cos(ACCUM_DELTA_YAW);
    ACCUM_DELTA_Y = ACCUM_DELTA_Y + transV * sin(ACCUM_DELTA_YAW);
    ACCUM_DELTA_Z = ACCUM_DELTA_Z + heightV;

    % trajectory of delta of em
    global DELTA_EM;

    minDeltaX = get_min_delta(EXPERIENCES(CUR_EXP_ID).x_gc, xGc, GC_X_DIM);
    minDeltaY = get_min_delta(EXPERIENCES(CUR_EXP_ID).y_gc, yGc, GC_Y_DIM);
    minDeltaZ = get_min_delta(EXPERIENCES(CUR_EXP_ID).z_gc, zGc, GC_Z_DIM);

    minDeltaYaw = get_min_delta(EXPERIENCES(CUR_EXP_ID).yaw_hdc, curYawHdc, YAW_HEIGHT_HDC_Y_DIM);
    minDeltaHeight = get_min_delta(EXPERIENCES(CUR_EXP_ID).height_hdc, curHeight, YAW_HEIGHT_HDC_H_DIM);

    minDeltaYawReversed = get_min_delta(EXPERIENCES(CUR_EXP_ID).yaw_hdc, (YAW_HEIGHT_HDC_Y_DIM /2) - curYawHdc, YAW_HEIGHT_HDC_Y_DIM);
    minDeltaYaw = min(minDeltaYaw, minDeltaYawReversed);

%     minDeltaHeightReversed = get_min_delta(EXPERIENCES(CUR_EXP_ID).height_hdc, (YAW_HEIGHT_HDC_H_DIM /2) - curHeight, YAW_HEIGHT_HDC_H_DIM);
%     minDeltaHeight = min(minDeltaHeight, minDeltaHeightReversed);

    delta_em = sqrt((minDeltaX).^2 + (minDeltaY).^2 + (minDeltaZ).^2 + (minDeltaYaw).^2 + (minDeltaHeight).^2);
    DELTA_EM = [DELTA_EM delta_em];

    % if the visual template is new or the 3d grid cells (x,y,z) and head direction cells (yaw, height)
    % has changed enough create a new experience
    if VT(vt_id).numExp == 0 || delta_em > DELTA_EXP_GC_HDC_THRESHOLD

        NUM_EXPS = NUM_EXPS + 1;
        create_new_exp(CUR_EXP_ID, NUM_EXPS, vt_id, xGc, yGc, zGc, curYawHdc, curHeight);

        PREV_EXP_ID = CUR_EXP_ID;
        CUR_EXP_ID = NUM_EXPS;

        ACCUM_DELTA_X = 0;
        ACCUM_DELTA_Y = 0;
        ACCUM_DELTA_Z = 0;

        ACCUM_DELTA_YAW = EXPERIENCES(CUR_EXP_ID).yaw_exp_rad;
%         ACCUM_DELTA_HEIGHT = EXPERIENCES(CUR_EXP_ID).height_hdc;

        % if the visual template has changed (but isn't new) search for the matching experience
    elseif vt_id ~= PREV_VT_ID

        % find the experience associated with the current visual template and that is under the
        % threshold distance to the centre of grid cell and head direction cell activity
        % if multiple experiences are under the threshold then don't match (to reduce hash collisions)
        matched_exp_id = 0;
        matched_exp_count = 0;

        for search_id = 1:VT(vt_id).numExp

            minDeltaYaw = get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).yaw_hdc, curYawHdc, YAW_HEIGHT_HDC_Y_DIM);
%             minDeltaYawReversed = get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).yaw_hdc, (YAW_HEIGHT_HDC_Y_DIM /2) - curYawHdc, YAW_HEIGHT_HDC_Y_DIM);
%             minDeltaYaw = min(minDeltaYaw, minDeltaYawReversed);

            minDeltaHeight = get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).height_hdc, curHeight, YAW_HEIGHT_HDC_Y_DIM);
%             minDeltaHeightReversed = get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).height_hdc, (YAW_HEIGHT_HDC_Y_DIM /2) - curHeight, YAW_HEIGHT_HDC_H_DIM);
%             minDeltaHeight = min(minDeltaHeight, minDeltaHeightReversed);

            delta_em(search_id) = sqrt(get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).x_gc, xGc, GC_X_DIM)^2 ...
            + get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).y_gc, yGc, GC_Y_DIM)^2 ...
            + get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).z_gc, yGc, GC_Z_DIM)^2 ...
            + minDeltaYaw^2 + minDeltaHeight^2) ;

            if delta_em(search_id) < DELTA_EXP_GC_HDC_THRESHOLD
               matched_exp_count = matched_exp_count + 1;
            end
        end

        if matched_exp_count > 1
            % this means we aren't sure about which experience is a match due to hash table collision
            % instead of a false posivitive which may create blunder links in
            % the experience map keep the previous experience

        else
            [min_delta, min_delta_id] = min(delta_em);

            MIN_DELTA_EM = [MIN_DELTA_EM; min_delta];
            if min_delta < DELTA_EXP_GC_HDC_THRESHOLD

                matched_exp_id = VT(vt_id).EXPERIENCES(min_delta_id).id;

                % see if the previous experience already has a link to the current experience
                link_exists = 0;
                for link_id = 1 : EXPERIENCES(CUR_EXP_ID).numlinks
                    if EXPERIENCES(CUR_EXP_ID).links(link_id).exp_id == matched_exp_id
                        link_exists = 1;
                        break;
                    end
                end

                % if we didn't find a link then create the link between current
                % experience and the experience for the current visual template
                if link_exists == 0
                    EXPERIENCES(CUR_EXP_ID).numlinks = EXPERIENCES(CUR_EXP_ID).numlinks + 1;
                    EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).exp_id = matched_exp_id;
                    %  EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).d_xy = sqrt(ACCUM_DELTA_X^2 + ACCUM_DELTA_Y^2 + ACCUM_DELTA_Z^2);
                    EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).d_xy = sqrt(ACCUM_DELTA_X^2 + ACCUM_DELTA_Y^2);
                    EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).d_z = ACCUM_DELTA_Z;

                    EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).heading_yaw_exp_rad = ...
                        get_signed_delta_radian(EXPERIENCES(CUR_EXP_ID).yaw_exp_rad, atan2(ACCUM_DELTA_Y, ACCUM_DELTA_X)); % heading is the delta angle between current pose of exp and previous pose of exp

                    EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).facing_yaw_exp_rad = ... % facing is the direction of each exp
                        get_signed_delta_radian(EXPERIENCES(CUR_EXP_ID).yaw_exp_rad, ACCUM_DELTA_YAW);

                 end

            end

            % if there wasn't an experience with the current visual template and grid cell (x y z) and head direction cell (yaw, height)
            % then create a new experience
            if matched_exp_id == 0
                NUM_EXPS = NUM_EXPS + 1;
                create_new_exp(CUR_EXP_ID, NUM_EXPS, vt_id, xGc, yGc, zGc, curYawHdc, curHeight);
                matched_exp_id = NUM_EXPS;
            end

            PREV_EXP_ID = CUR_EXP_ID;
            CUR_EXP_ID = matched_exp_id;

            ACCUM_DELTA_X = 0;
            ACCUM_DELTA_Y = 0;
            ACCUM_DELTA_Z = 0;
            ACCUM_DELTA_YAW = EXPERIENCES(CUR_EXP_ID).yaw_exp_rad;
%             ACCUM_DELTA_HEIGHT = EXPERIENCES(CUR_EXP_ID).height_hdc;
        end

    end

    global EXP_CORRECTION;
    global EXP_LOOPS;

    % do the experience map correction interatively for all the links in all the experiences
    for i = 1:EXP_LOOPS

        for exp_id = 1:NUM_EXPS

            for link_id=1:EXPERIENCES(exp_id).numlinks

                % experience 0 has a link to experience 1
                e0 = exp_id;
                e1 = EXPERIENCES(exp_id).links(link_id).exp_id;

                % work out where e0 thinks e1 (x,y) should be based on the stored link information
                lx = EXPERIENCES(e0).x_exp + EXPERIENCES(e0).links(link_id).d_xy * cos(EXPERIENCES(e0).yaw_exp_rad + EXPERIENCES(e0).links(link_id).heading_yaw_exp_rad);
                ly = EXPERIENCES(e0).y_exp + EXPERIENCES(e0).links(link_id).d_xy * sin(EXPERIENCES(e0).yaw_exp_rad + EXPERIENCES(e0).links(link_id).heading_yaw_exp_rad);
                lz = EXPERIENCES(e0).z_exp + EXPERIENCES(e0).links(link_id).d_z;  %

                % correct e0 and e1 (x,y) by equal but opposite amounts
                % a 0.5 correction parameter means that e0 and e1 will be fully
                % corrected based on e0's link information
                EXPERIENCES(e0).x_exp = EXPERIENCES(e0).x_exp + (EXPERIENCES(e1).x_exp - lx) * EXP_CORRECTION;
                EXPERIENCES(e0).y_exp = EXPERIENCES(e0).y_exp + (EXPERIENCES(e1).y_exp - ly) * EXP_CORRECTION;
                EXPERIENCES(e0).z_exp = EXPERIENCES(e0).z_exp + (EXPERIENCES(e1).z_exp - lz) * EXP_CORRECTION;

                EXPERIENCES(e1).x_exp = EXPERIENCES(e1).x_exp - (EXPERIENCES(e1).x_exp - lx) * EXP_CORRECTION;
                EXPERIENCES(e1).y_exp = EXPERIENCES(e1).y_exp - (EXPERIENCES(e1).y_exp - ly) * EXP_CORRECTION;
                EXPERIENCES(e1).z_exp = EXPERIENCES(e1).z_exp - (EXPERIENCES(e1).z_exp - lz) * EXP_CORRECTION;

                % determine the angle between where e0 thinks e1's facing
                % should be based on the link information
                TempDeltaYawFacing = get_signed_delta_radian((EXPERIENCES(e0).yaw_exp_rad + EXPERIENCES(e0).links(link_id).facing_yaw_exp_rad), EXPERIENCES(e1).yaw_exp_rad);

                % correct e0 and e1 facing by equal but opposite amounts
                % a 0.5 correction parameter means that e0 and e1 will be fully
                % corrected based on e0's link information
                EXPERIENCES(e0).yaw_exp_rad = clip_radian_180(EXPERIENCES(e0).yaw_exp_rad + TempDeltaYawFacing * EXP_CORRECTION);
                EXPERIENCES(e1).yaw_exp_rad = clip_radian_180(EXPERIENCES(e1).yaw_exp_rad - TempDeltaYawFacing * EXP_CORRECTION);

            end
        end

    end

    % keep a frame by frame history of which experience was currently active
    global EXP_HISTORY;
    EXP_HISTORY = [EXP_HISTORY; CUR_EXP_ID];
end


function [samples, energies, diagn] = hmc2(f, x, opt, gradf, varargin)
%HMC2   Hybrid Monte Carlo sampling.
%
%       Description
%       SAMPLES = HMC2(F, X, OPTIONS, GRADF) uses a  hybrid Monte Carlo
%       algorithm to sample from the distribution P ~ EXP(-F), where F is the
%       first argument to HMC2. The Markov chain starts at the point X, and
%       the function GRADF is the gradient of the `energy' function F.
%
%       HMC2(F, X, OPTIONS, GRADF, P1, P2, ...) allows additional arguments to
%       be passed to F() and GRADF().
%
%       [SAMPLES, ENERGIES, DIAGN] = HMC2(F, X, OPTIONS, GRADF) also returns a
%       log of the energy values (i.e. negative log probabilities) for the
%       samples in ENERGIES and DIAGN, a structure containing diagnostic
%       information (position, momentum and acceptance threshold) for each
%       step of the chain in DIAGN.POS, DIAGN.MOM and DIAGN.ACC respectively.
%       All candidate states (including rejected ones) are stored in
%       DIAGN.POS. The DIAGN structure contains fields:
%
%       pos
%        the position vectors of the dynamic process
%       mom
%        the momentum vectors of the dynamic process
%       acc
%        the acceptance thresholds
%       rej
%        the number of rejections
%       stp
%        the step size vectors
%
%       S = HMC2('STATE') returns a state structure that contains
%       the used random stream and its state and the momentum of
%       the dynamic process. These are contained in fields stream,
%       streamstate and mom respectively. The momentum state is
%       only used for a persistent momentum update.
%
%       HMC2('STATE', S) resets the state to S. If S is an integer,
%       then it is used as a seed for the random stream and the
%       momentum variable is randomised. If S is a structure
%       returned by HMC2('STATE') then it resets the random stream
%       to exactly the same state.
%
%       See HMC2_OPT for the optional parameters in the OPTIONS structure.
%
%       See also
%       HMC2_OPT, METROP
%

%       Copyright (c) 1996-1998 Christopher M Bishop, Ian T Nabney
%       Copyright (c) 1998-2000,2010 Aki Vehtari

%  The portion of the HMC algorithm involving "windows" is derived
%  from the C code for this function included in the Software for
%  Flexible Bayesian Modeling written by Radford Neal
%  <http://www.cs.toronto.edu/~radford/fbm.software.html>.

% Global variable to store state of momentum variables: set by set_state
% Used to initialize variable if set

% 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.

global HMC_MOM

%Return state or set state to given state.
if nargin <= 2
  if ~strcmp(f, 'state')
    error('Unknown argument to hmc2');
  end
  switch nargin
   case 1
    samples = get_state(f);
    return;
   case 2
    set_state(f, x);
    return;
  end
end

% Set empty options to default values
opt=hmc2_opt(opt);
% Reference to structures is much slower, so...
opt_nsamples=opt.nsamples;
opt_window=opt.window;
opt_steps=opt.steps;
opt_display = opt.display;
opt_persistence = opt.persistence;

if opt_persistence
  alpha = opt.decay;
  salpha = sqrt(1-alpha*alpha);
end

% Stepsizes, varargin gives the opt.stepsf arguments (net, x ,y)
% where x is input data and y is a target data.
if ~isempty(opt.stepsf)
  epsilon = feval(opt.stepsf,varargin{:}).*opt.stepadj;
else
  epsilon = opt.stepadj;
end

% Force x to be a row vector
x = x(:)';
nparams = length(x);

%Set up strings for evaluating potential function and its gradient.
f = fcnchk(f, length(varargin));
gradf = fcnchk(gradf, length(varargin));

% Check the gradient evaluation.
if (opt.checkgrad)
  % Check gradients
  feval('gradcheck', x, f, gradf, varargin{:});
end

% Matrix of returned samples.
samples = zeros(opt_nsamples, nparams);
% Return energies?
if nargout >= 2
  en_save = 1;
  energies = zeros(opt_nsamples, 1);
else
  en_save = 0;
end
% Return diagnostics?
if nargout >= 3
  diagnostics = 1;
  diagn_pos = zeros(opt_nsamples, nparams);
  diagn_mom = zeros(opt_nsamples, nparams);
  diagn_acc = zeros(opt_nsamples, 1);
else
  diagnostics = 0;
end

if (~opt_persistence | isempty(HMC_MOM) | nparams ~= length(HMC_MOM))
  % Initialise momenta at random
  p = randn(1, nparams);
else
  % Initialise momenta from stored state
  p = HMC_MOM;
end

% Evaluate starting energy.
E = feval(f, x, varargin{:});

% Main loop.
nreject = 0;         %number of rejected samples
window_offset=0;     %window offset initialised to zero
k = - opt.nomit + 1;       %nomit samples are omitted, so we store
while k <= opt_nsamples    %samples from k>0

  % Store starting position and momenta
  xold = x;
  pold = p;
  % Recalculate Hamiltonian as momenta have changed
  Eold = E;
  Hold = E + 0.5*(p*p');

  % Decide on window offset, if windowed HMC is used
  if opt_window>1
    window_offset=fix(opt_window*rand(1));
  end

  have_rej = 0;
  have_acc = 0;
  n = window_offset;
  dir = -1;           %the default value for dir (=direction)
                      %assumes that windowing is used
  while (dir==-1 | n~=opt_steps)

    %if windowing is not used or we have allready taken
    %window_offset steps backwards...
    if (dir==-1 & n==0)
      % Restore, next state should be original start state.
      if window_offset > 0
	x = xold;
	p = pold;
	n = window_offset;
      end
      %set dir for forward steps
      E = Eold;
      H = Hold;
      dir = 1;
      stps = dir;
    else
      if (n*dir+1<opt_window | n>(opt_steps-opt_window))
	% State in the accept and/or reject window.
	stps = dir;
      else
	% State not in the accept and/or reject window.
	stps = opt_steps-2*(opt_window-1);
      end
      % First half-step of leapfrog.
      p = p - dir*0.5*epsilon.*feval(gradf, x, varargin{:});
      x = x + dir*epsilon.*p;
      % Full leapfrog steps.
      for m = 1:(abs(stps)-1)
	p = p - dir*epsilon.*feval(gradf, x, varargin{:});
	x = x + dir*epsilon.*p;
      end
      % Final half-step of leapfrog.
      p = p - dir*0.5*epsilon.*feval(gradf, x, varargin{:});

      E = feval(f, x, varargin{:});
      H = E + 0.5*(p*p');

      n=n+stps;
    end

    if (opt_window~=opt_steps+1 & n<opt_window)
      % Account for state in reject window.  Reject window can be
      % ignored if windows consist of the entire trajectory.
      if ~have_rej
	rej_free_energy = H;
      else
	rej_free_energy = -addlogs(-rej_free_energy, -H);
      end
      if (~have_rej | rand(1) < exp(rej_free_energy-H));
	E_rej=E;
	x_rej=x;
	p_rej=p;
        have_rej = 1;
      end
    end

    if (n>(opt_steps-opt_window))
      % Account for state in the accept window.
      if ~have_acc
	acc_free_energy = H;
      else
	acc_free_energy = -addlogs(-acc_free_energy, -H);
      end
      if (~have_acc | rand(1) < exp(acc_free_energy-H))
	E_acc=E;
	x_acc=x;
	p_acc=p;
	have_acc = 1;
      end
    end
  end

  % Acceptance threshold.
  a = exp(rej_free_energy - acc_free_energy);

  if (diagnostics & k > 0)
    diagn_pos(k,:) = x_acc;
    diagn_mom(k,:) = p_acc;
    diagn_acc(k,:) = a;
  end
  if (opt_display > 1)
    fprintf(1, 'New position is\n');
    disp(x);
  end

  % Take new state from the appropriate window.
  if a > rand(1)
    % Accept
    E=E_acc;
    x=x_acc;
    p=-p_acc; % Reverse momenta
    if (opt_display > 0)
      fprintf(1, 'Finished step %4d  Threshold: %g\n', k, a);
    end
  else
    % Reject
    if k > 0
      nreject = nreject + 1;
    end
    E=E_rej;
    x=x_rej;
    p=p_rej;
    if (opt_display > 0)
      fprintf(1, '  Sample rejected %4d.  Threshold: %g\n', k, a);
    end
  end

  if k > 0
    % Store sample
    samples(k,:) = x;
    if en_save
      % Store energy
      energies(k) = E;
    end
  end

  % Set momenta for next iteration
  if opt_persistence
    % Reverse momenta
    p = -p;
    % Adjust momenta by a small random amount
    p = alpha.*p + salpha.*randn(1, nparams);
  else
    % Replace all momenta
    p = randn(1, nparams);
  end

  k = k + 1;
end

if (opt_display > 0)
  fprintf(1, '\nFraction of samples rejected:  %g\n', ...
    nreject/(opt_nsamples));
end

% Store diagnostics
if diagnostics
  diagn.pos = diagn_pos;   %positions matrix
  diagn.mom = diagn_mom;   %momentum matrix
  diagn.acc = diagn_acc;   %acceptance treshold matrix
  diagn.rej = nreject/(opt_nsamples);   %rejection rate
  diagn.stps = epsilon;    %stepsize vector
end

% Store final momentum value in global so that it can be retrieved later
if opt_persistence
  HMC_MOM = p;
else
  HMC_MOM = [];
end
return

function state = get_state(f)
%GET_STATE           Return complete state of sampler
%                    (including momentum)

global HMC_MOM
state.stream=setrandstream();
state.streamstate = state.stream.State;
state.mom = HMC_MOM;
return

function set_state(f, x)
%SET_STATE    Set complete state of sample
%
%          Description
%          Set complete state of sampler (including momentum)
%          or just set randn and rand with integer argument.
global HMC_MOM
if isnumeric(x)
  setrandstream(x);
else
  if ~isstruct(x)
    error('Second argument to hmc must be number or state structure');
  end
  if (~isfield(x, 'stream') | ~isfield(x, 'streamstate') ...
      | ~isfield(x, 'mom'))
    error('Second argument to hmc must contain correct fields')
  end
  setrandstream(x.stream);
  x.State=x.streamstate;
  HMC_MOM = x.mom;
end
return

function c=addlogs(a,b)
%ADDLOGS(A,B)  Add numbers represented by their logarithms.
%
%        Description
%        Add numbers represented by their logarithms.
%        Computes log(exp(a)+exp(b)) in such a fashion that it
%        works even when a and b have large magnitude.
if a>b
  c = a + log(1+exp(b-a));
else
  c = b + log(1+exp(a-b));
end


function a = abs(a)
%ABS          Taylor absolute value abs(a)
%
%   c = abs(a)
%
%Result is convex hull of left and right derivative. This allows
%also input containing zero. The expansion
%  f(x) = f(xs) + f'(zeta)*(x-xs)
%for some zeta in ch(x,xs) is still valid
%

% written  05/21/09     S.M. Rump
%

  if ~isreal(a.t)
    a.t = abs(a.t);
    a.t(2:end,:) = repmat(NaN,size(a.t(2:end,:)));
    return
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  at = a.t(1,:);
  index = ( at<0 );
  a.t(:,index) = -a.t(:,index);
  if isa(at,'intval')
    index = in0(0,at(1,:));
    a.t(:,index) = infsup(-1,1)*a.t(:,index);
  end

  if rndold
    setround(rndold)
  end


function model3d_new = transform_3d_rigid(model3d,R,T)

if(model_has_normals(model3d))
    [x,y,z,nx,ny,nz] = model_to_components(model3d);
else
    [x,y,z] = model_to_components(model3d);
end

xyz = [x y z];
xyz = xyz * R';
xyz  = xyz + repmat(T',length(x),1);

if(model_has_normals(model3d))
    nxyz = [nx ny nz] * R';
    model3d_new = components_to_model(xyz(:,1),xyz(:,2),xyz(:,3),nxyz(:,1),nxyz(:,2),nxyz(:,3));
else
    model3d_new = components_to_model(xyz(:,1),xyz(:,2),xyz(:,3),[],[],[]);
end

if(model_has_colors(model3d))
    model3d_new.r = model3d.r;
    model3d_new.g = model3d.g;
    model3d_new.b = model3d.b;
end

if(model_has_faces(model3d))
    model3d_new.face = model3d.face;
end


function [P3, S_bar, V, RO, Tr, Z, sigma_sq, phi, Q, mu0, sigma0, c] =...
    em_sfm(P, MD, K, mask, imsize, bbox, tol, max_em_iter, rhandedCoords, rotP3d)

%  Non-Rigid Structure From Motion with Gaussian/LDS Deformation Model
%  Copyright (c) by Lorenzo Torresani, Stanford University
%
%  Based on the following paper:
%
%  Lorenzo Torresani, Aaron Hertzmann and Christoph Bregler,
%     Learning Non-Rigid 3D Shape from 2D Motion, NIPS 16, 2003
%  http://cs.stanford.edu/~ltorresa/projects/learning-nr-shape/
%
%  Please refer to this publication if you use this program for
%  research or for technical applications.
%
%
%  INPUT:
%
%  P           - (2*T) x J tracking matrix:          P([t t+T],:) contains the 2D projections of the J points at time t
%  MD          - T x J missing data binary matrix:   MD(t, j)=1 if no valid data is available for point j at time t, 0 otherwise
%  K           - number of deformation basis
%  use_lds     - set to 1 to model deformations using a linear dynamical system; set to 0 otherwise
%  tol         - termination tolerance (proportional change in likelihood)
%  max_em_iter - maximum number of EM iterations
%  mask        - mask.poly_x (Tx1 cell array of x coordinates of mask) and
%                mask.poly_y (Tx1 cell array of y coordinates of mask)
%  imsize      - Tx2 array with height and width of images
%  bbox        - Bboxes of instances in image (Nx4)
%  params.normdim - Normalization dimension for object bounding boxes
%
%  OUTPUT:
%
%  P3          - (3*T) x J 3D-motion matrix:                    ( P3([t t+T t+2*T],:) contains the 3D coordinates of the J points at time t )
%  S_bar       - shape average:            3 x J matrix
%  V           - deformation shapes:       (3*K) x J matrix     ( V((n-1)*3+[1:3],:) contains the n-th deformation basis )
%  RO          - rotation:                 cell array           ( RO{t} gives the rotation matrix at time t )
%  Tr          - translation:              T x 2 matrix
%  Z           - deformation weights:      T x K matrix
%  sigma_sq    - variance of the noise in feature position
%  phi         - LDS transition matrix
%  Q           - LDS state noise matrix
%  mu0         - initial state mean
%  sigma0      - initial state variance
%  c - scale coefficients                   T X 1 matrix

%% Parsing passed parameters
params = get_params();

%% Checking Dimensions
if mod(size(P,1), 1) ~= 0,
   fprintf('Error: size(P) must be (2*T)xJ\n');
   return;
end

if (size(P,1)/2 ~= size(MD,1)) || (size(P,2) ~= size(MD,2))
   fprintf('Error: Size incompatibility between P and MD\n');
   return;
end

if mod(K, 1) ~= 0,
   fprintf('Error: K must be an integer value\n');
   return;
end
%% Initialization for Tr, S_bar, R, c

[T, J] = size(MD);

P_hat = P; % if any of the points are missing, P_hat will be updated during the M-step

% uses rank 3 factorization to get a first initialization for rotation and S_bar
[R_init, Trvect, S_bar] = rigidfac(P_hat, MD); % P_hat = R_init*S_bar + Trvect

% initialize c also here !!
c = ones(T,1);
%if scaling
    for t = 1:T
       c(t) = mean([norm(R_init(t,:)),norm(R_init(t+T,:))]); % just an estimate
    end
%end

Tr(:,1) = Trvect(1:T);
Tr(:,2) = Trvect(T+1:2*T);

R = zeros(2*T, 3);

%% enforces rotation constraints for initilizations
for t = 1:T,
   Ru = R_init(t,:);
   Rv = R_init(T+t,:);
   Rz = cross(Ru,Rv); if det([Ru;Rv;Rz])<0, Rz = -Rz; end;
   RO_approx = apprRot([Ru;Rv;Rz]);
   RO{t} = RO_approx;
   R(t,:) = RO_approx(1,:);
   R(t+T,:) = RO_approx(2,:);
end

%% Initializations for deformation shapes and weights
% given the initial estimates of rotation, translation and shape average, it initializes
% deformation shapes and weights through LSQ minimization of the reprojection error

[V, Z] = init_SB(P_hat, Tr, R, c , S_bar, K); %handled c inside this function

% initializes sigma_sq
E_zz_init = cov(Z);
E_zz_init = repmat(E_zz_init, T, 1);
sigma_sq = mstep_update_noisevar(P_hat, S_bar, V, Z', E_zz_init, RO, Tr, c); %handled c inside this function

phi = [];
mu0 = [];
sigma0 = [];
Q = [];

%% EM iterations

loglik = 0;
annealing_const = 60;
max_anneal_iter = round(max_em_iter/4);

pBar =  TimedProgressBar( max_em_iter, 30, ...
    'Training NRSfM ', ', completed ', 'Training NRSfM concluded in ' );
for em_iter=1:max_em_iter,
    %mean(c)
    %std(c)
    %mean(mean(abs(S_bar)))
    %% compute the hidden variables distributions
    [E_z, E_zz] = estep_compute_Z_distr(P_hat, S_bar, V, R, c, Tr, sigma_sq);     % (Eq 17-18) %handled c inside this function
    Z = E_z';

    %% updates shape basis
    [S_bar, V] = mstep_update_shapebasis(P_hat, E_z, E_zz, R, c, Tr);   % (Eq 21)

    %% fill in missing points
    if sum(MD(:))>0,
        P_hat = mstep_update_missingdata(P_hat, MD, S_bar, V, E_z, RO, c, Tr, mask, imsize, bbox, params.nrsfm.norm_dim,(em_iter>max_em_iter/2));    % Occlusion reasoning after half iterations over
    end

    %% updates rotation
    [RO, R] = mstep_update_rotation(P_hat, S_bar, V, E_z, E_zz, RO, c, Tr);       % (Eq 24)

    %RO = rotP3d;
    %for t = 1:T
    %    R(t,:) = RO{t}(1,:);
    %    R(t+T,:) = RO{t}(2,:);
    %end

    %% update C here
    %if scaling
        c = mstep_update_c(P_hat, S_bar, V, E_z, E_zz, RO, Tr); % Eq 48 (PAMI paper)
        frac = mean(c);
        c = c/frac;
        S_bar = frac*S_bar;
        V = frac*V;
    %end

    %% updates translation
    Tr = mstep_update_transl(P_hat, S_bar, V, E_z, RO, c);                        % (Eq 23)

    %% updates noise variance
    sigma_sq = mstep_update_noisevar(P_hat, S_bar, V, E_z, E_zz, RO, Tr, c);      % (Eq 22)
    if em_iter < max_anneal_iter,
        sigma_sq = sigma_sq * (1 + annealing_const*(1 - em_iter/max_anneal_iter));
    end

    %visualize_3D_point(S_bar,params.part_names,wireframe_car(),h);figure(h);clf;
    %% computes log likelihood
    oldloglik = loglik;
    loglik = compute_log_lik(P_hat, S_bar, V, E_z, E_zz, RO, c, Tr, sigma_sq);

%     if(mod(em_iter,40)==0)
%         fprintf('Iteration %d/%d: Error %f\n', em_iter, max_em_iter, -loglik/(size(P_hat,1)/2));
%     end

    if (em_iter <= 2),
     loglikbase = loglik;
    elseif (loglik < oldloglik)
     %fprintf('Violation');
     %keyboard;
    elseif 0 && ((loglik-loglikbase)<(1 + tol)*(oldloglik-loglikbase)),
     fprintf('\n');
     break;
    end
    pBar.progress();
end
pBar.stop();
%% Check for right handed coordinate flips
vec1 = S_bar(:, rhandedCoords(1,2)) - S_bar(:, rhandedCoords(1,1));
vec2 = S_bar(:, rhandedCoords(2,2)) - S_bar(:, rhandedCoords(2,1));
vec3 = cross(vec1,vec2);
v = S_bar(:, rhandedCoords(3,2)) - S_bar(:, rhandedCoords(3,1));

badCt = 0;
for t = 1:T
   z_t = Z(t,:);
   S = S_bar;
   for kk = 1:K,
      S = S+z_t(kk)*V((kk-1)*3+[1:3],:);
   end
   S = c(t,1)*RO{t}*S;
   Zs = S(3,:);
   badZ = mean(Zs(MD(t,:)));
   goodZ = mean(Zs(~MD(t,:)));
   if(goodZ > badZ)
       badCt = badCt+1;
   end
end
thresh = badCt/T;

isFlipped = 0;
if(thresh > 0.5 || (thresh >0.2 && dot(vec3,v)<0))
    refZ = diag([1 1 -1]);
    rotZ = diag([-1 -1 1]);
    isFlipped = 1;
    S_bar = -S_bar;
    V = -V;
    for i=1:length(RO)
        RO{i} = rotZ*RO{i};
    end
end

%% Changing to PASCAL 3D
fprintf('Converting to PASCAL 3D co-ordinate frame\n');
if(~isempty(rotP3d))
    tformRot = averageRotationTransform(RO,rotP3d);
else
    fprintf('PASCAL 3D rotations not found. Might be a problem with evaluation\n');
    tformRot = averageRotationTransform(RO,RO);
end

%tformRot = eye(3);
S_bar = tformRot'*S_bar;
for t = 1:T
    RO{t}  = RO{t}*tformRot;
end
for kk = 1:K
  V((kk-1)*3+[1:3],:) = tformRot'*V((kk-1)*3+[1:3],:);
end

%% compute the final answer
P3 = zeros(3*T, J);
for t = 1:T,
   z_t = Z(t,:);
   S = S_bar;
   for kk = 1:K,
      S = S+z_t(kk)*V((kk-1)*3+[1:3],:);
   end;
   S = c(t,1)*RO{t}*S; %% changed code here !

   P3([t t+T t+2*T], :) = S + [Tr(t, [1 2]) -mean(S(3,:))]'*ones(1,J);
end


function edgeFaces = trimeshEdgeFaces(faces, varargin)
%TRIMESHEDGEFACES Compute index of faces adjacent to each edge of a triangular mesh.
%
%   EF = trimeshEdgeFaces(FACES)
%   EF = trimeshEdgeFaces(VERTICES, FACES)
%   EF = trimeshEdgeFaces(VERTICES, EDGES, FACES)
%   Compute index array of faces adjacent to each edge of a mesh.
%   FACES is a NF-by-3 array containing vertex indices of each face. The
%   result EF is a NE-by-2 array containing the indices of the two faces
%   incident to each edge. If an edge belongs to only one face, the other
%   face index is ZERO.
%
%   The list of edges (as array of source and target vertex indices) can be
%   obtained from the function 'meshEdges'.
%
%   Note: faces are listed in increasing order for each edge, and no
%   information is kept about relative orientation of edge and face.
%
%   Example
%     % compute incidence list of each edge of an octahedron. For example,
%     % first edge is incident to faces 1 and 5. Second edge is incident to
%     % faces 4 and 8, and so on.
%     [v, f] = createOctahedron;
%     ef = trimeshEdgeFaces(v, f)
%     ef =
%          1     5
%          4     8
%          4     1
%          5     8
%          2     6
%          1     2
%          6     5
%          3     7
%          2     3
%          7     6
%          3     4
%          7     8
%
%   See also
%   meshes3d, meshEdgeFaces, trimeshMeanBreadth, meshEdges

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Created: 2015-08-19,    using Matlab 8.5.0.197613 (R2015a)
% Copyright 2015 INRA - Cepia Software Platform.

if nargin == 2
    faces = varargin{1};
elseif nargin == 3
    faces = varargin{2};
end

% compute vertex indices of each edge (in increasing index order)
edges = sort([faces(:,[1 2]) ; faces(:,[2 3]) ; faces(:,[3 1])], 2);

% create an array to keep indices of faces "creating" each edge
nFaces = size(faces, 1);
edgeFaceInds = repmat( (1:nFaces)', 3, 1);

% sort edges, keeping indices
[edges, ia, ib] = unique(edges, 'rows'); %#ok<ASGLU>
nEdges = size(edges, 1);

% allocate memory for result
edgeFaces = zeros(nEdges, 2);

% iterate over edges, to identify incident faces
for iEdge = 1:nEdges
    inds = find(ib == iEdge);
    edgeFaces(iEdge, 1:length(inds)) = edgeFaceInds(inds);
end


%{
 * Copyright (C) 2013-2020, The Regents of The University of Michigan.
 * All rights reserved.
 * This software was developed in the Biped Lab (https://www.biped.solutions/)
 * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
 * be available under alternative licensing terms; contact the address above.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * The views and conclusions contained in the software and documentation are those
 * of the authors and should not be interpreted as representing official policies,
 * either expressed or implied, of the Regents of The University of Michigan.
 *
 * AUTHOR: Bruce JK Huang (bjhuang[at]umich.edu)
 * WEBSITE: https://www.brucerobot.com/
%}

function total_cost = computeConstraintCustomizedCost(X, theta_x, theta_y, theta_z, T, target_size, box_width)
    cost_x = 0;
    cost_y = 0;
    cost_z = 0;
    R = rotx(theta_x) * roty(theta_y) * rotz(theta_z);
    x_prime = R(1, :) * X(1:3, :) + T(1);
    y_prime = R(2, :) * X(1:3, :) + T(2);
    z_prime = R(3, :) * X(1:3, :) + T(3);
    for i = 1:size(X, 2)
        cost_z = cost_z + checkCost(z_prime(i), -target_size/2, target_size/2);
        cost_y = cost_y + checkCost(y_prime(i), -target_size/2, target_size/2);
        cost_x = cost_x + checkCost(x_prime(i), -box_width, box_width);
    end
%     total_cost = sqrt(cost_x + cost_y + cost_z);
    total_cost = cost_x + cost_y + cost_z;
end


%%*****************************************************************************
%% HSDsqlp: solve an semidefinite-quadratic-linear program
%%   by infeasible path-following method on the homogeneous self-dual model.
%%
%%  [obj,X,y,Z,info,runhist] =
%%      HSDsqlp(blk,At,C,b,OPTIONS,X0,y0,Z0);
%%
%%  Input: blk: a cell array describing the block diagonal structure of SQL data.
%%          At: a cell array with At{p} = [svec(Ap1) ... svec(Apm)]
%%         b,C: data for the SQL instance.
%%     OPTIONS: a structure that specifies parameters required in HSDsqlp.m,
%%              (if it is not given, the default in sqlparameters.m is used).
%%
%%  (X0,y0,Z0): an initial iterate (if it is not given, the default is used).
%%
%%  Output: obj  = [<C,X> <b,y>].
%%          (X,y,Z): an approximately optimal solution or a primal or dual
%%                   infeasibility certificate.
%%          info.termcode = termination-code
%%          info.iter     = number of iterations
%%          info.obj      = [primal-obj, dual-obj]
%%          info.cputime  = total-time
%%          info.gap      = gap
%%          info.pinfeas  = primal_infeas
%%          info.dinfeas  = dual_infeas
%%          runhist.pobj    = history of primal objective value.
%%          runhist.dobj    = history of dual   objective value.
%%          runhist.gap     = history of <X,Z>.
%%          runhist.pinfeas = history of primal infeasibility.
%%          runhist.dinfeas = history of dual   infeasibility.
%%          runhist.cputime = history of cputime spent.
%%-----------------------------------------------------------------
%%  The OPTIONS structure specifies the required parameters:
%%      vers  gam  predcorr  expon  gaptol  inftol  steptol
%%      maxit  printlevel  ...
%%      (all have default values set in sqlparameters.m).
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

  function [obj,X,y,Z,info,runhist] = ...
            HSDsqlp(blk,At,C,b,OPTIONS,X0,y0,Z0,kap0,tau0,theta0);

    if (nargin < 5); OPTIONS = []; end

    isemptyAtb = 0;
    if isempty(At) & isempty(b);
       %% Add redundant constraint: <-I,X> <= 0
       b = 0;
       At = ops(ops(blk,'identity'),'*',-1);
       numblk = size(blk,1);
       blk{numblk+1,1} = 'l'; blk{numblk+1,2} = 1;
       At{numblk+1,1} = 1; C{numblk+1,1} = 0;
       isemptyAtb = 1;
    end
%%
%%-----------------------------------------
%% get parameters from the OPTIONS structure.
%%-----------------------------------------
%%
   warning off;

   matlabversion = sscanf(version,'%f');
   if strcmp(computer,'PCWIN64') | strcmp(computer,'GLNXA64')
      par.computer = 64;
   else
      par.computer = 32;
   end
   par.matlabversion = matlabversion(1);
   par.vers        = 0;
   par.predcorr    = 1;
   par.gam         = 0;
   par.expon       = 1;
   par.gaptol      = 1e-8;
   par.inftol      = 1e-8;
   par.steptol     = 1e-6;
   par.maxit       = 100;
   par.printlevel  = 3;
   par.stoplevel   = 1;
   par.scale_data  = 0;
   par.spdensity   = 0.4;
   par.rmdepconstr = 0;
   par.smallblkdim = 50;
   par.schurfun     = cell(size(blk,1),1);
   par.schurfun_par = cell(size(blk,1),1);
%%
   parbarrier  = cell(size(blk,1),1);
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      if strcmp(pblk{1},'s') | strcmp(pblk{1},'q')
         parbarrier{p} = zeros(1,length(pblk{2}));
      elseif strcmp(pblk{1},'l') | strcmp(pblk{1},'u' )
         parbarrier{p} = zeros(1,sum(pblk{2}));
      end
   end
   parbarrier_0 = parbarrier;
%%
   if exist('OPTIONS')
      if isfield(OPTIONS,'vers');        par.vers     = OPTIONS.vers; end
      if isfield(OPTIONS,'predcorr');    par.predcorr = OPTIONS.predcorr; end
      if isfield(OPTIONS,'gam');         par.gam      = OPTIONS.gam; end
      if isfield(OPTIONS,'expon');       par.expon    = OPTIONS.expon; end
      if isfield(OPTIONS,'gaptol');      par.gaptol   = OPTIONS.gaptol; end
      if isfield(OPTIONS,'inftol');      par.inftol   = OPTIONS.inftol; end
      if isfield(OPTIONS,'steptol');     par.steptol  = OPTIONS.steptol; end
      if isfield(OPTIONS,'maxit');       par.maxit    = OPTIONS.maxit; end
      if isfield(OPTIONS,'printlevel');  par.printlevel  = OPTIONS.printlevel; end
      if isfield(OPTIONS,'stoplevel');   par.stoplevel   = OPTIONS.stoplevel; end
      if isfield(OPTIONS,'scale_data');  par.scale_data  = OPTIONS.scale_data; end
      if isfield(OPTIONS,'spdensity');   par.spdensity   = OPTIONS.spdensity; end
      if isfield(OPTIONS,'rmdepconstr'); par.rmdepconstr = OPTIONS.rmdepconstr; end
      if isfield(OPTIONS,'smallblkdim'); par.smallblkdim = OPTIONS.smallblkdim; end
      if isfield(OPTIONS,'parbarrier');
         parbarrier = OPTIONS.parbarrier;
         if isempty(parbarrier); parbarrier = parbarrier_0; end
         if ~iscell(parbarrier);
            tmp = parbarrier; clear parbarrier; parbarrier{1} = tmp;
         end
         if (length(parbarrier) < size(blk,1))
            len = length(parbarrier);
            parbarrier(len+1:size(blk,1)) = parbarrier_0(len+1:size(blk,1));
         end
      end
      if isfield(OPTIONS,'schurfun');
         par.schurfun = OPTIONS.schurfun;
         if ~isempty(par.schurfun); par.scale_data = 0; end
      end
      if isfield(OPTIONS,'schurfun_par'); par.schurfun_par = OPTIONS.schurfun_par; end
      if isempty(par.schurfun);     par.schurfun = cell(size(blk,1),1); end
      if isempty(par.schurfun_par); par.schurfun_par = cell(size(blk,1),1); end
   end
   if (size(blk,2) > 2); par.smallblkdim = 0; end
%%
%%-----------------------------------------
%% convert matrices to cell arrays.
%%-----------------------------------------
%%
   if ~iscell(At); At = {At}; end;
   if ~iscell(C);  C = {C}; end;
   if all(size(At) == [size(blk,1), length(b)]);
      convertyes = zeros(size(blk,1),1);
      for p = 1:size(blk,1)
         if strcmp(blk{p,1},'s') & all(size(At{p,1}) == sum(blk{p,2}))
            convertyes(p) = 1;
         end
      end
      if any(convertyes)
         if (par.printlevel);
            fprintf('\n sqlp: converting At into required format');
         end
         At = svec(blk,At,ones(size(blk,1),1));
      end
   end
%%
%%-----------------------------------------
%% validate SQLP data.
%%-----------------------------------------
%%
   tstart = cputime;
   [blk,At,C,b,blkdim,numblk] = validate(blk,At,C,b,par);
   [blk,At,C,b,iscmp] = convertcmpsdp(blk,At,C,b);
   if (iscmp) & (par.printlevel>=2)
      fprintf('\n SQLP has complex data');
   end
   if (nargin <= 5) | (isempty(X0) | isempty(y0) | isempty(Z0));
      if (max([ops(At,'norm'),ops(C,'norm'),norm(b)]) > 1e2)
         [X0,y0,Z0] = infeaspt(blk,At,C,b,1);
      else
         [X0,y0,Z0] = infeaspt(blk,At,C,b,2,1);
      end
   end
   if ~iscell(X0);  X0 = {X0}; end;
   if ~iscell(Z0);  Z0 = {Z0}; end;
   if (length(y0) ~= length(b))
      error('HSDsqlp: length of b and y0 not compatible');
   end
   [X0,Z0] = validate_startpoint(blk,X0,Z0,par.spdensity);
%%
   if (nargin <= 8) | (isempty(kap0) | isempty(tau0) | isempty(theta0))
      if (max([ops(At,'norm'),ops(C,'norm'),norm(b)]) > 1e6)
         kap0 = 10*blktrace(blk,X0,Z0);
      else
         kap0 = blktrace(blk,X0,Z0);
      end
      tau0 = 1; theta0 = 1;
   end
%%
   if (par.printlevel>=2)
      fprintf('\n num. of constraints = %2.0d',length(b));
      if blkdim(1);
         fprintf('\n dim. of sdp    var  = %2.0d,',blkdim(1));
         fprintf('   num. of sdp  blk  = %2.0d',numblk(1));
      end
      if blkdim(2);
         fprintf('\n dim. of socp   var  = %2.0d,',blkdim(2));
         fprintf('   num. of socp blk  = %2.0d',numblk(2));
      end
      if blkdim(3); fprintf('\n dim. of linear var  = %2.0d',blkdim(3)); end
      if blkdim(4); fprintf('\n dim. of free   var  = %2.0d',blkdim(4)); end
   end
%%
%%-----------------------------------------
%% detect unrestricted blocks in linear blocks
%%-----------------------------------------
%%
   user_supplied_schurfun = 0;
   for p = 1:size(blk,1)
      if ~isempty(par.schurfun{p}); user_supplied_schurfun = 1; end
   end
   if (user_supplied_schurfun == 0)
      [blk2,At2,C2,ublkinfo,parbarrier2,X02,Z02] = ...
      detect_ublk(blk,At,C,parbarrier,X0,Z0,par.printlevel);
   else
      blk2 = blk; At2 = At; C2 = C;
      parbarrier2 = parbarrier; X02 = X0; Z02 = Z0;
      ublkinfo = cell(size(blk2,1),1);
   end
   ublksize = blkdim(4);
   for p = 1:size(ublkinfo,1)
      ublksize = ublksize + length(ublkinfo{p});
   end
%%
%%-----------------------------------------
%% detect diagonal blocks in semidefinite blocks
%%-----------------------------------------
%%
   if (user_supplied_schurfun==0)
      [blk3,At3,C3,diagblkinfo,diagblkchange,parbarrier3,X03,Z03] = ...
      detect_lblk(blk2,At2,C2,b,parbarrier2,X02,Z02,par.printlevel);
   else
      blk3 = blk2; At3 = At2; C3 = C2;
      parbarrier3 = parbarrier2; X03 = X02; Z03 = Z02;
      diagblkchange = 0;
      diagblkinfo = cell(size(blk3,1),1);
   end
%%
%%-----------------------------------------
%% main solver
%%-----------------------------------------
%%
   exist_analytic_term = 0;
   for p = 1:size(blk3,1);
      idx = find(parbarrier3{p} > 0);
      if ~isempty(idx); exist_analytic_term = 1; end
   end
%%
   if (par.vers == 0);
      if blkdim(1); par.vers = 1; else; par.vers = 2; end
   end
   par.blkdim = blkdim;
   par.ublksize = ublksize;
   [obj,X3,y,Z3,info,runhist] = ...
        HSDsqlpmain(blk3,At3,C3,b,par,X03,y0,Z03,kap0,tau0,theta0);
%%
%%-----------------------------------------
%% recover semidefinite blocks from linear blocks
%%-----------------------------------------
%%
   if any(diagblkchange)
      X2 = cell(size(blk2,1),1); Z2 = cell(size(blk2,1),1);
      count = 0;
      for p = 1:size(blk2,1)
         pblk = blk2(p,:);
         n = sum(pblk{2});
         blkno      = diagblkinfo{p,1};
         idxdiag    = diagblkinfo{p,2};
         idxnondiag = diagblkinfo{p,3};
         if ~isempty(idxdiag)
            len = length(idxdiag);
            Xtmp = [idxdiag,idxdiag,X3{end}(count+[1:len]); n, n, 0];
            Ztmp = [idxdiag,idxdiag,Z3{end}(count+[1:len]); n, n, 0];
            if ~isempty(idxnondiag)
               [ii,jj,vv] = find(X3{blkno});
               Xtmp = [Xtmp; idxnondiag(ii),idxnondiag(jj),vv];
               [ii,jj,vv] = find(Z3{blkno});
               Ztmp = [Ztmp; idxnondiag(ii),idxnondiag(jj),vv];
            end
            X2{p} = spconvert(Xtmp);
            Z2{p} = spconvert(Ztmp);
	    count = count + len;
         else
	    X2(p) = X3(blkno); Z2(p) = Z3(blkno);
         end
      end
   else
      X2 = X3; Z2 = Z3;
   end
%%
%%-----------------------------------------
%% recover linear block from unrestricted block
%%-----------------------------------------
%%
   numblk = size(blk,1);
   numblknew = numblk;
   X = cell(numblk,1); Z = cell(numblk,1);
   for p = 1:numblk
      n = blk{p,2};
      if isempty(ublkinfo{p,1})
         X{p} = X2{p}; Z{p} = Z2{p};
      else
	 Xtmp = zeros(n,1); Ztmp = zeros(n,1);
         Xtmp(ublkinfo{p,1}) = max(0,X2{p});
         Xtmp(ublkinfo{p,2}) = max(0,-X2{p});
         Ztmp(ublkinfo{p,1}) = max(0,Z2{p});
         Ztmp(ublkinfo{p,2}) = max(0,-Z2{p});
         if ~isempty(ublkinfo{p,3})
            numblknew = numblknew + 1;
            Xtmp(ublkinfo{p,3}) = X2{numblknew};
            Ztmp(ublkinfo{p,3}) = Z2{numblknew};
         end
	 X{p} = Xtmp; Z{p} = Ztmp;
      end
   end
%%
%%-----------------------------------------
%% recover complex solution
%%-----------------------------------------
%%
   if (iscmp)
      for p = 1:numblk
         pblk = blk(p,:);
         n = sum(pblk{2})/2;
         if strcmp(pblk{1},'s');
            X{p} = X{p}(1:n,1:n) + sqrt(-1)*X{p}(n+[1:n],1:n);
            Z{p} = Z{p}(1:n,1:n) + sqrt(-1)*Z{p}(n+[1:n],1:n);
            X{p} = 0.5*(X{p}+X{p}');
            Z{p} = 0.5*(Z{p}+Z{p}');
         end
      end
   end
   if (isemptyAtb)
      X = X(1:end-1); Z = Z(1:end-1);
   end
%%*****************************************************************************


function imdb = Extract_PPG_feautures(PPG,HRVparams,SigName)

% function imdb = PPG_feat_ext(PPG,HRVparams)
% This function is a modified version of CRC_features_calculation used in
% CV_SleepStaging designed to dela with PPG signals insted ECG
%
% Inputs:
%        PPG : vector containing the PPG signals
%  HRVparams : structur with
%    SigName : name use to save derived features
%
% Output:
%       imdb : structur containing the extracted feature to be used for
%              classifictaion
%
% This function is a part of the PPG_sleepstagin project
%         https://github.com/cliffordlab/PPG-sleepstaging
%
% A list of dependencies is given in the readme file
%
% Author : Giulia Da Poian (giulia.dap@gmail.com), 07-Dec-2018
% Part of this code was originally included in the CRC_features_ectract
% in the CV_SleepStaging code, and has been modified to work with PPG
% signals
%

imdb=[];
imdb.images.data=[];
imdb.meta.features=[];

SavePath = HRVparams.writedata;
Fs_resamp = 4;
NFFT = 2000;

% 512 points for 2 Hz (Fs_resamp/2)
very_low_band = 1:3;  % <0.01
low_band = 4:25;      % 0.01-0.1
high_band = 26:103;   % 0.1-0.4

win_inc = HRVparams.increment; % sliding window length is 30s
win_len = HRVparams.windowlength; % segment length 300s (5 min)

% try

   % Preprocess the PPG signal and extract IBI
   if ~exist([SavePath filesep 'FiltSigs' filesep SigName '_filt.mat'],'file')
       ppg_filt = get_filtered_PPG(PPG,HRVparams,1,SigName);
   else
       load([SavePath filesep 'FiltSigs' filesep SigName '_filt.mat'],'ppg_filt');
   end

    % check if annotations are already presents
    if exist([SavePath filesep 'Annotation' filesep SigName '.sqippg'],'file')
        [onsets,~,sqinum] = read_ann([SavePath filesep 'Annotation' filesep SigName],'sqippg');
%%%%%        sqinum = sqinum(2:end); % convert sqi to range 0-100
    else
        % PPG Detection - qppg
        onsets = qppg_fast(ppg_filt,HRVparams.Fs);
        % PPG SQI
        [sqinum, ~, ppgsqi] = calculate_ppgsqi(onsets,ppg_filt,HRVparams.Fs);
        % Write PPG  annotations
        AnnotationFolder = [SavePath filesep 'Annotation'];
        if ~exist(AnnotationFolder,'dir'); mkdir(AnnotationFolder); end
        write_ann([AnnotationFolder filesep SigName],HRVparams,'sqippg',onsets(1:length(sqinum)),char(ppgsqi),sqinum);
    end

    RIAV = Extract_Respiratory_Components(ppg_filt,onsets,HRVparams.Fs); % respiratory induced  amplitude  variation
%%%%% adding sqi_resamp processing
    [NN_resamp, RIAV_resamp, sqi_resamp] = clean_resample_timeseries(onsets,RIAV,sqinum,HRVparams.Fs,Fs_resamp);

    % segmnets of 5-min-length, sliding forwrard every 30s
    N_win = floor(length(RIAV_resamp)/Fs_resamp-win_len)/win_inc+1;

    for idx = 1:N_win
        % image data number

        try
            rr_in_win = NN_resamp((idx-1)*win_inc*Fs_resamp+1:min(((idx-1)*win_inc+win_len)*Fs_resamp,length(NN_resamp)));
            RIAV_in_win = RIAV_resamp((idx-1)*win_inc*Fs_resamp+1:min(((idx-1)*win_inc+win_len)*Fs_resamp,length(RIAV_resamp)));

            rr = rr_in_win;
            rrs = (1/Fs_resamp):(1/Fs_resamp):length(rr_in_win)/Fs_resamp;

            %%% HRV parameters from NN intervals
            SDNN=std(rr);

            % Using HRV_toolbox functions
            [AC,DC] = prsa(rr, rrs, HRVparams, [], 1);
            % SampEn
            sampEn_10 = EvalEntropyMetrics(rr, cumsum(rr),2,0.10, HRVparams, 1, []);
            sampEn_15 = EvalEntropyMetrics(rr, cumsum(rr),2,0.15, HRVparams, 1, []);
            sampEn_20 = EvalEntropyMetrics(rr, cumsum(rr),2,0.20, HRVparams, 1, []);
            % LF/HF
            [PSD,F] = CalcLomb(rrs',detrend(rr'), 1/NFFT:1/NFFT:(Fs_resamp/2) ,[],0);
            LF_HF_ratio=sum(PSD(F>0.04&F<=0.15))/sum(PSD(F>0.15&F<=0.4));

            x = detrend(rr_in_win);
            y = detrend(RIAV_in_win);
            Pxy_amp = abs(csd_amp(x,y,2^10,Fs_resamp,hanning(512),256));
            Pxy_phase = abs(csd_phase(x,y,2^10,Fs_resamp,hanning(512),256));

            % get max amplitude and normalize
            max_amp = max(Pxy_amp);
            max_phase = max(Pxy_phase);
            z1 = (Pxy_amp./max_amp).^2;
            z2 = (Pxy_phase./max_phase).^2;
            Cxy = z1.*z2;

            % call crc % from Robert Joseph Thomas, An Electrocardiogram-Based
            % Technique to Assess Cardiopulmonary Coupling During Sleep

            cross_sp=cpsd(x,y,hanning(512),[],1024);
            co=coherence(x(1:1024),y(1:1024),hanning(512),1024);
            crc = abs(cross_sp).^2 .* co';
            crc=crc./max(crc);

            [low_peaks1, ~]=findpeaks(Cxy(low_band),'SORTSTR','descend');
            [high_peaks1, ~]=findpeaks(Cxy(high_band),'SORTSTR','descend');
            if ~isempty(low_peaks1) && ~isempty(low_peaks1)
                Lo_Hi_ratio1=sum(low_peaks1(1:min(2,length(low_peaks1))))/sum(high_peaks1(1:min(2,length(low_peaks1))));
            else
                if isempty(low_peaks1)
                    Lo_Hi_ratio1=0;
                end
                if isempty(high_peaks1)
                    Lo_Hi_ratio1=NaN;
                end
            end
            [low_peaks2, ~] = findpeaks(crc(low_band), 'SORTSTR' ,'descend');
            [high_peaks2, ~] = findpeaks(crc(high_band), 'SORTSTR' ,'descend');
            if ~isempty(low_peaks2) && ~isempty(low_peaks2)
                Lo_Hi_ratio2=sum(low_peaks2(1:min(2,length(low_peaks2))))/sum(high_peaks2(1:min(2,length(low_peaks2))));
            else
                if isempty(low_peaks2)
                    Lo_Hi_ratio2=0;
                end
                if isempty(high_peaks2)
                    Lo_Hi_ratio2=NaN;
                end
            end

            % area ratio
            sum_very_low=sum(crc(very_low_band));
            sum_low=sum(crc(low_band));
            sum_high=sum(crc(high_band));
            area_ratio_v_l=sum_very_low/sum_low;
            area_ratio_v_h=sum_very_low/sum_high;
            area_ratio_l_h=sum_low/sum_high;

            % set each image by a 512 window and slide forward every 10 seconds
            moving_window=512;
            sliding=10*Fs_resamp;
            for k=1:18
                x=rr_in_win((k-1)*sliding+1:(k-1)*sliding+moving_window);
                y=RIAV_in_win((k-1)*sliding+1:(k-1)*sliding+moving_window);
                x=detrend(x);
                y=detrend(y);

                % call crc % from Robert Joseph Thomas, An Electrocardiogram-Based
                % Technique to Assess Cardiopulmonary Coupling During Sleep

                cross_sp=cpsd(x,y,hanning(256),[],NFFT);
                co=coherence(x,y,hanning(256),NFFT);
                crc = abs(cross_sp).^2 .* co';
                crc = crc./max(crc);
                imagedata = crc(1:250);
                % resample crc data to 50 points
                tmpImg = [imagedata(1:25); resample(imagedata(26:end),25,225)];
                imdb.images.data(:,k,1,idx)= tmpImg;
            end

            % set class of image
            % SQI_i = sqinum((idx-1)*win_inc+1:min(((idx-1)*win_inc+win_len),length(sqinum)));
            SQI_i = nanmean(sqi_resamp((idx-1)*win_inc*Fs_resamp+1:min(((idx-1)*win_inc+win_len)*Fs_resamp,length(sqi_resamp))));

            imdb.meta.sqi(:,idx) = SQI_i;
            imdb.meta.features(idx,1) = AC;
            imdb.meta.features(idx,2) = DC;
            imdb.meta.features(idx,3) = sampEn_10;
            imdb.meta.features(idx,4) = sampEn_15;
            imdb.meta.features(idx,5) = sampEn_20;
            imdb.meta.features(idx,6) = NaN;
            imdb.meta.features(idx,7) = SDNN;
            imdb.meta.features(idx,8) = LF_HF_ratio;
            imdb.meta.features(idx,9) = Lo_Hi_ratio1;
            imdb.meta.features(idx,10) = Lo_Hi_ratio2;
            imdb.meta.features(idx,11) = area_ratio_v_l;
            imdb.meta.features(idx,12) = area_ratio_v_h;
            imdb.meta.features(idx,13) = area_ratio_l_h;
            imdb.meta.rr{idx} = rr;
        catch

            error=1
            imdb.meta.sqi(:,idx) = NaN;
            imdb.meta.features(idx,1) = NaN;
            imdb.meta.features(idx,2) = NaN;
            imdb.meta.features(idx,3) = NaN;
            imdb.meta.features(idx,4) = NaN;
            imdb.meta.features(idx,5) = NaN;
            imdb.meta.features(idx,6) = NaN;
            imdb.meta.features(idx,7) = NaN;
            imdb.meta.features(idx,8) = NaN;
            imdb.meta.features(idx,9) = NaN;
            imdb.meta.features(idx,10) = NaN;
            imdb.meta.features(idx,11) = NaN;
            imdb.meta.features(idx,12) = NaN;
            imdb.meta.features(idx,13) = NaN;
%             imagedata = zeros(1,250);
            imdb.meta.rr{idx} = zeros(1,1200)*NaN;%[imagedata(1:25); resample(imagedata(26:end),25,225)];
            continue;
        end
    end

% catch
% end
imdb.images.data=single(imdb.images.data);

end