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

%Coordinates generation
R = acos(-D(:,:,3)) ./ (pi * 2 * sqrt(D(:,:,1).^2 + D(:,:,2).^2));

X1 = (0.5 + R .* D(:,:,1)) * c;
Y1 = (0.5 - R .* D(:,:,2)) * r;

X1 = RemoveSpecials(X1);
Y1 = RemoveSpecials(Y1);

end


function CrowdDis = ConvergenceScore(front, p)

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

% This function is written by Annibale Panichella

[m,~] = size(front);
CrowdDis = zeros(1,m);

for i=1:m
    CrowdDis(i) = -norm(front(i,:),p);
end
end


function linplus_test42 ( )

%*****************************************************************************80
%
%% TEST42 tests R8GE_NP_TRF, R8GE_NP_TRM, R8GE_NP_TRS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  m = 10;
  n = m;
  nrhs = 1;

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST42\n' );
  fprintf ( 1, '  For a matrix in general storage,\n' );
  fprintf ( 1, '  R8GE_NP_TRF factors without pivoting,\n' );
  fprintf ( 1, '  R8GE_NP_TRS solves factored systems.\n' );
  fprintf ( 1, '  R8GE_NP_TRM computes A*X for factored A.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix rows M =    %d\n', m );
  fprintf ( 1, '  Matrix columns N = %d\n', n );
%
%  Set the matrix.
%
  [ a, seed ] = r8ge_random ( m, n, seed );
%
%  Set the desired solution.
%
  x(1:n) = 1.0E+00;
%
%  Compute the corresponding right hand side.
%
  b = r8ge_mxv ( m, n, a, x );
%
%  Factor the matrix.
%
  [ a_lu, info ] = r8ge_np_trf ( m, n, a );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST42 - Fatal error!\n' );
    fprintf ( 1, '  R8GE_NP_TRF declares the matrix is singular!\n' );
    fprintf ( 1, '  The value of INFO is %d\n', info );
    return
  end
%
%  Solve the linear system.
%
  b_mat = r8vec_to_r8ge ( n, nrhs, b );
  [ x_mat, info ] = r8ge_np_trs ( n, nrhs, 'N', a_lu, b_mat );
  x = r8ge_to_r8vec ( n, nrhs, x_mat );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST42 - Fatal error!\n' );
    fprintf ( 1, '  R8GE_TRS returned an error condition!\n' );
    fprintf ( 1, '  The value of INFO is %d\n', info );
    return
  end

  r8vec_print ( n, x, '  Solution:' );
%
%  Set the desired solution.
%
  x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
  job = 0;
  b = r8ge_np_trm ( m, n, a_lu, x, job );
%
%  Solve the system
%
  b_mat = r8vec_to_r8ge ( n, nrhs, b );
  [ x_mat, info ] = r8ge_np_trs ( n, nrhs, 'N', a_lu, b_mat );
  x = r8ge_to_r8vec ( n, nrhs, x_mat );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST42 - Fatal error!\n' );
    fprintf ( 1, '  R8GE_TRS returned an error condition!\n' );
    fprintf ( 1, '  The value of INFO is %d\n', info );
    return
  end

  r8vec_print ( n, x, '  Solution:' );
%
%  Set the desired solution.
%
  x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
  job = 1;
  b = r8ge_np_trm ( m, n, a_lu, x, job );
%
%  Solve the system.
%
  b_mat = r8vec_to_r8ge ( n, nrhs, b );
  [ x_mat, info ] = r8ge_np_trs ( n, nrhs, 'T', a_lu, b_mat );
  x = r8ge_to_r8vec ( n, nrhs, x_mat );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST42 - Fatal error!\n' );
    fprintf ( 1, '  R8GE_TRS returned an error condition!\n' );
    fprintf ( 1, '  The value of INFO is %d\n', info );
    return
  end

  r8vec_print ( n, x, '  Solution of transposed system:' );

  return
end


function linplus_test24 ( )

%*****************************************************************************80
%
%% TEST24 tests R8GB_ML.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  m = 10;
  n = m;
  ml = 1;
  mu = 2;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST24\n' );
  fprintf ( 1, '  For a general banded matrix,\n' );
  fprintf ( 1, '  R8GB_ML computes A*x or A''*X\n' );
  fprintf ( 1, '    where A has been factored by R8GB_FA.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix rows M              = %d\n', m );
  fprintf ( 1, '  Matrix columns N           = %d\n', n );
  fprintf ( 1, '  Lower bandwidth ML         = %d\n', ml );
  fprintf ( 1, '  Upper bandwidth MU         = %d\n', mu );

  for job = 0 : 1
%
%  Set the matrix.
%
    [ a, seed ] = r8gb_random ( m, n, ml, mu, seed );
%
%  Set the desired solution.
%
    x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
    if ( job == 0 )
      b = r8gb_mxv ( m, n, ml, mu, a, x );
    else
      b = r8gb_vxm ( m, n, ml, mu, a, x );
    end
%
%  Factor the matrix.
%
    [ a_lu, pivot, info ] = r8gb_fa ( n, ml, mu, a );

    if ( info ~= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'TEST24 - Fatal error!\n' );
      fprintf ( 1, '  R8GB_FA declares the matrix is singular!\n' );
      fprintf ( 1, '  The value of INFO is %d\n', info );
      return
    end
%
%  Now multiply factored matrix times solution to get right hand side again.
%
    b2 = r8gb_ml ( n, ml, mu, a_lu, pivot, x, job );

    if ( job == 0 )
      r8vec2_print_some ( n, b, b2, 10, '  A*x and PLU*x' );
    else
      r8vec2_print_some ( n, b, b2, 10, '  A''*x and (PLU)''*x' );
    end

  end

  return
end


function T = GenPencil(im, P, J)
% ==============================================
%   Compute the pencil map 'T'
%
%   Paras:
%   @im        : input image ranging value from 0 to 1.
%   @P         : the pencil texture.
%   @J         : the tone map.
%

    %% Parameters
    theta = 0.2;

    [H, W, ~] = size(im);

    %% Initialization
    P = imresize(P, [H, W]);
    P = reshape(P, H*W, 1);
    logP = log(P);
    logP = spdiags(logP, 0, H*W, H*W);

    J = imresize(J, [H, W]);
    J = reshape(J, H*W, 1);
    logJ = log(J);

    e = ones(H*W, 1);
    Dx = spdiags([-e, e], [0, H], H*W, H*W);
    Dy = spdiags([-e, e], [0, 1], H*W, H*W);

    %% Compute matrix A and b
    A = theta * (Dx * Dx' + Dy * Dy') + (logP)' * logP;
    b = (logP)' * logJ;

    %% Conjugate gradient
    beta = pcg(A, b, 1e-6, 60);

    %% Compute the result
    beta = reshape(beta, H, W);

    P = reshape(P, H, W);

    T = P .^ beta;
end


function GaussianHighpass
a=imread('cameraman.tif');
figure(1)
imshow(a)
[m n]=size(a);
f_transform=fft2(a);
f_shift=fftshift(f_transform);
p=m/2;
q=n/2;
d0=70;
for i=1:m
for j=1:n
distance=sqrt((i-p)^2+(j-q)^2);
low_filter(i,j)=1-exp(-(distance)^2/(2*(d0^2)));
end
end
filter_apply=f_shift.*low_filter;
image_orignal=ifftshift(filter_apply);
image_filter_apply=abs(ifft2(image_orignal));
figure(2)
imshow(image_filter_apply,[])


% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function sensor = sensorfield(x, y)

    xc = 60; yc = 90;

    sensor = 200 ./ ((x-xc).^2 + (y-yc).^2 + 200);


% Replace zeros f0 values by linear interpolation
%
% Copyright (c) 2012 University of Crete - Computer Science Department (UOC-CSD)
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file 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 Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function f0sout = fillf0(f0sin)

    f0sout = f0sin;

    idx = find(f0sin(:,2)~=0);
    if length(idx)==1
        f0sout(:,2) = f0sin(idx,2);
    elseif length(idx)>1
        f0sout(:,2) = 2.^(interp1(f0sin(idx,1), log2(f0sin(idx,2)), f0sin(:,1), 'linear', NaN));
    end

    f0sout(:,2) = interp1_fixnan(f0sout(:,2));

return


function y = lorentzian(x, sigma, type)
%LORENTZIAN   Lorentzian robust function.
%   LORENTZIAN(X, SIGMA, TYPE) evaluates the Lorentzian robust function
%   with sigma SIGMA at point(s) X.
%   TYPE selects the evaluation type:
%    - 0: function value
%    - 1: first derivative
%    - 2: second derivative
%
%   This is a private member function of the class 'robust_function'.
%
%   Author:  Stefan Roth, Department of Computer Science, TU Darmstadt
%   Contact: sroth@cs.tu-darmstadt.de
%   $Date:  $
%   $Revision: $

% Copyright 2004-2007, Brown University, Providence, RI. USA
% Copyright 2007-2010 TU Darmstadt, Darmstadt, Germany.
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

  switch (type)
   case 0
    y = log(1 + x.^2 / (2 * sigma^2));
   case 1
    y = 2 * x ./ (2 * sigma^2 + x.^2);
   case 2
    y = 2 ./ (2 * sigma^2 + x.^2);      % not second order, but first order/x  (Deqing Sun 24 Nov 2007)
  end


function table2 = i4mat_border_cut ( m, n, table )

%*****************************************************************************80
%
%% I4MAT_BORDER_CUT cuts the "border" of an I4MAT.
%
%  Discussion:
%
%    We suppose the input data gives values of a quantity on nodes
%    on a 2D grid, and we wish to create a new table corresponding only
%    to those nodes in the interior of the 2D grid.
%
%      0 0 0 0 0 0
%      0 * * * * 0    * * * *
%      0 * * * * 0 -> * * * *
%      0 * * * * 0    * * * *
%      0 0 0 0 0 0
%
%    The illustration suggests the situation in which a 5 by 6 array
%    is input, and a 3 by 4 array is to be output.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, integer TABLE(M,N), the table data.
%
%    Output, integer TABLE2(M-2,N-2), the new table data.
%
  if ( m <= 2 || n <= 2 )
    table2 = [];
    return
  end

  table2(1:m-2,1:n-2) = round ( table(2:m-1,2:n-1) );

  return
end


function M = stiefelgeneralizedfactory(n, p, B)
% Returns a manifold structure of "scaled" orthonormal matrices.
%
% function M = stiefelgeneralizedfactory(n, p)
% function M = stiefelgeneralizedfactory(n, p, B)
%
% The generalized Stiefel manifold is the set of "scaled" orthonormal
% nxp matrices X such that X'*B*X is identity. B must be positive definite.
% If B is identity, then this is the standard Stiefel manifold.
%
% The generalized Stiefel manifold is endowed with a scaled metric
% by making it a Riemannian submanifold of the Euclidean space,
% again endowed with the scaled inner product.
%
% Some notions (not all) are from Section 4.5 of the paper
% "The geometry of algorithms with orthogonality constraints",
% A. Edelman, T. A. Arias, S. T. Smith, SIMAX, 1998.
%
% Paper link: http://arxiv.org/abs/physics/9806030.
%
% Note: egrad2rgrad and ehess2rhess involve solving linear systems in B. If
% this is a bottleneck for a specific application, then a way forward is to
% create a modified version of this file which preprocesses B to speed this
% up (typically, by computing a Cholesky factorization of it, then calling
% an appropriate solver).
%
% See also: stiefelfactory  grassmannfactory  grassmanngeneralizedfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, June 30, 2015.
% Contributors:
%
% Change log:
%

    if ~exist('B', 'var') || isempty(B)
        B = speye(n); % Standard Stiefel manifold.
    end

    M.name = @() sprintf('Generalized Stiefel manifold St(%d, %d)', n, p);

    M.dim = @() (n*p - .5*p*(p+1));

    M.inner = @(X, eta, zeta) trace(eta'*(B*zeta)); % Scaled metric.

    M.norm = @(X, eta) sqrt(M.inner(X, eta, eta));

    M.dist = @(X, Y) error('stiefelgeneralizedfactory.dist not implemented yet.');

    M.typicaldist = @() sqrt(p);

    % Orthogonal projection of an ambient vector U to the tangent space
    % at X.
    M.proj = @projection;
    function Up = projection(X, U)
        BX = B*X;

        % Projection onto the tangent space
        Up = U - X*symm(BX'*U);
    end

    M.tangent = M.proj;

    M.egrad2rgrad = @egrad2rgrad;
    function rgrad = egrad2rgrad(X, egrad)

        % First, scale egrad according the to the scaled metric in the
        % Euclidean space.
        egrad_scaled = B\egrad;

        % Second, project onto the tangent space.
        % rgrad = egrad_scaled - X*symm((B*X)'*egrad_scaled);
        %
        % Verify that symm(BX'*egrad_scaled) = symm(X'*egrad).

        rgrad = egrad_scaled - X*symm(X'*egrad);
    end

    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(X, egrad, ehess, H)
        egraddot = ehess;
        Xdot = H;

        % Directional derivative of the Riemannian gradient.
        egrad_scaleddot = B\egraddot;
        rgraddot = egrad_scaleddot - Xdot*symm(X'*egrad)...
            - X*symm(Xdot'*egrad)...
            - X*symm(X'*egraddot);

        % Project onto the tangent space.
        rhess = M.proj(X, rgraddot);
    end

    M.retr = @retraction;
    function Y = retraction(X, U, t)
        if nargin < 3
            t = 1.0;
        end
        Y = guf(X + t*U); % Ensure that Y'*B*Y is identity.
    end

    M.exp = @exponential;
    function Y = exponential(X, Z, t)
        if nargin < 3
            t = 1.0;
        end
        Y = retraction(X, Z, t);
        warning('manopt:stiefelgeneralizedfactory:exp', ...
               ['Exponential for generalized Stiefel manifold ' ...
                'manifold not implemented yet. Used retraction instead.']);
    end

    M.hash = @(X) ['z' hashmd5(X(:))];

    M.rand = @random;
    function X = random()
        X = guf(randn(n, p)); % Ensure that X'*B*X is identity;
    end

    M.randvec = @randomvec;
    function U = randomvec(X)
        U = projection(X, randn(n, p));
        U = U / norm(U(:));
    end

    M.lincomb = @matrixlincomb;

    M.zerovec = @(X) zeros(n, p);

    % This transport is compatible with the generalized polar retraction.
    M.transp = @(X1, X2, d) projection(X2, d);

    M.vec = @(X, u_mat) u_mat(:);
    M.mat = @(X, u_vec) reshape(u_vec, [n, p]);
    M.vecmatareisometries = @() false;

    % Some auxiliary functions
    symm = @(D) (D + D')/2;

    function X = guf(Y)
        % Generalized polar decomposition of an n-by-p matrix Y.
        % X'*B*X is identity.

        % Method 1
        [u, ~, v] = svd(Y, 0);

        % Instead of the following three steps, an equivalent, but an
        % expensive way is to do X = u*(sqrtm(u'*(B*u))\(v')).
        [q, ssquare] = eig(u'*(B*u));
        qsinv = q/sparse(diag(sqrt(diag(ssquare))));
        X = u*((qsinv*q')*v'); % X'*B*X is identity.

        % Another computation using restricted_svd
        % [u, ~, v] = restricted_svd(Y);
        % X = u*v'; % X'*B*X is identity.

    end

    function [u, s, v] = restricted_svd(Y)
        % We compute a thin svd-like decomposition of an n-by-p matrix Y
        % into matrices u, s, and v such that u is an n-by-p matrix
        % with u'*B*u being identity, s is a p-by-p diagonal matrix
        % with positive entries, and v is a p-by-p orthogonal matrix.
        % Y = u*s*v'.
        [v, ssquare] = eig(symm(Y'*(B*Y))); % Y*B*Y is positive definite
        ssquarevec = diag(ssquare);

        s = sparse(diag(abs(sqrt(ssquarevec))));
        u = Y*(v/s); % u'*B*u is identity.
    end

end


function [zPred,PzPred,otherInfo]=reducedStateMeasPred(xPred,PPred,H)
%%REDUCEDSTATEMEASPRED Perform the measurement prediction part of the
%           measurement update step of the reduced state estimator. The
%           function reducedStateMeasUpdateWithPred can be used to complete
%           the measurement update. Separating the measurement prediction
%           step from the rest of the update step can make the creation of
%           multiple measurement association hypotheses from a single
%           target prediction more efficient. The full measurement update
%           function is reducedStateUpdate.
%
%INPUTS: xPred The xDimXnumComp predicted target states.
%        PPred The xDimXxDimXnumComp predicted state covariance matrices.
%            H The zDimXxDim measurement matrix. The measurement is modeled
%              as z=H*x+noise.
%
%OUTPUTS: zPred The zDimXnumComp measurement predictions from the filter.
%        PzPred The zDimXzDimXnumComp covariance matrix associated with the
%               values in zPred.
%     otherInfo A structure containing members of intermediate results of
%               this function that can be passed to KalmanUpdateWithPred
%               when updating with a measurement.
%
%The measurement prediction step in the measurement update step of the
%reduced state estimator is the same as that in the Kalman filter. Thus,
%this function just calls KalmanMeasPred.
%
%The filter is taken from [1]. See the comments to reducedStateUpdate for
%more information.
%
%EXAMPLE:
%With this example, we demonstrate that one gets the same result using
%reducedStateUpdate versus calling reducedStateMeasPred and then
%reducedStateMeasUpdateWithPred.
% xPred=[1e3;-2e3;100;200];
% MPred=[28,   3.5,    6,  8.5;
%      3.5,    23,  8.5,   11;
%        6,   8.5,   18, 13.5;
%      8.5,    11, 13.5,   13];
% DPred=[8, 0;
%        0, 8;
%        8, 0;
%        0, 8];
% PPred=MPred+(1/2)*(DPred*DPred');
% z=1e3*[-5.498856156296510;
%        1.199241491470584];
% R=eye(2);
% H=[0, 4, 9, 8;
%    6, 3, 0, 6];
% %The update in one step.
% [xUpdate,MUpdate,DUpdate,innov,Pzz,W]=reducedStateUpdate(xPred,PPred,MPred,DPred,z,R,H);
% %The update in two steps.
% [zPred,PzPred,otherInfo]=reducedStateMeasPred(xPred,PPred,H);
% [xUpdate1,MUpdate1,DUpdate1,innov1,Pzz1,W1]=reducedStateMeasUpdateWithPred(z,R,zPred,PzPred,otherInfo,MPred,DPred);
% %One will see that the one and two step updates agree.
% max(abs([xUpdate1-xUpdate;MUpdate1(:)-MUpdate(:);DUpdate1(:)-DUpdate(:);innov1(:)-innov;Pzz1(:)-Pzz(:);W1(:)-W(:)]))
%
%REFERENCES:
%[1] P. Mookerjee and F. Reifler, "Reduced state estimator for systems with
%    parametric inputs," IEEE Transactions on Aerospace and Electronic
%    Systems, vol. 40, no. 2, pp. 446-461, Apr. 2004.
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[zPred,PzPred,otherInfo]=KalmanMeasPred(xPred,PPred,H);

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [I,k] = verifyquad(funfcn,a,b,tol,see,mmax,varargin)
%QUAD         Verified quadrature using Romberg's scheme - rudimentary implementation
%
%The input function must be real and at least 4 times differentiable. The quadrature
%  does NOT work if f or its derivatives have poles in or near the interval [a,b].
%  Not much optimization of the parameters is done; if the routine works well, there
%  is at least a verified inclusion of the integral available.
%
%   [ I , k ] = verifyquad(f,a,b,tol,see,mmax,varargin)
%
%The univariate real function f is integrated from a to b; must at least be 4 times differentiable.
%
% optional input    tol     anticipated (relative) tolerance (default 1e-6)
%                   see     see intermediate results
%                   mmax    maximal order of Romberg scheme <=16; note that derivatives
%                             up to 2*mmax+2 are computed (default mmax=4)
%                   P1,...  extra parameters for function evaluation
%                           f(x,P1,P2,...)
% optional output   k       number of subintervals
%
%A simple example:
%
%   Q = verifyquad('exp(x*sin(x))',0,10)
%
%Routine verifyquad is by no means optimized, just written straightforwardly. But sometimes
%it is even faster (and much more accurate) than the Matlab built-in function quad:
% f = @(x)(sinh(exp(x))), a = 0; b = 5;
%   tic, Approx = quad(f,a,b), toc,
%   tic, Incl = verifyquad(f,a,b), toc
%produces
% f =
%     @(x)(sinh(exp(x)))
% Warning: Maximum function count exceeded; singularity likely.
% > In quad at 100
% Approx =
%   3.3124e+032
% Elapsed time is 0.281967 seconds.
% intval Incl =
%   1.0e+061 *
%     9.6709
% Elapsed time is 0.161995 seconds.
%
%Note that verifyquad is almost twice as fast, and the approximate value is off
%  by several orders of magnitude. But a warning is given.
%It may also happen that no warning is given:
% f = @(x)(sin(x+exp(x))), a = 0; b = 8;
%   tic, Approx = quad(f,a,b), toc,
%   tic, Incl = verifyquad(f,a,b), toc
% f =
%     @(x)(sin(x+exp(x)))
% Approx =
%     0.2511
% Elapsed time is 0.178671 seconds.
% intval Incl =
%     0.3474
% Elapsed time is 1.125084 seconds.
%
%Note, however, that the approximate value is only correct to one figure, and no warning is given.
%
%Based on Romberg's rule with error term by
%   F. L. Bauer, H. Rutishauser, and E. Stiefel: New Aspects in Numerical Quadrature,
%   Proc. Symp. Appl. Math. Vol. XV, AMS 1963, pp. 198-218.
%

% written  05/29/09     S.M. Rump
% modified 11/30/09     S.M. Rump  function check and vectorize
%

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

  % store standard function exception mode
  RealStdFctsExcptnMode = intvalinit('RealStdFctsExcptn',0);
  intvalinit('RealStdFctsExcptnNaN',0);

  % Convert to inline function as needed
  try
    if isa(funfcn,'function_handle')
      strfun = fcnchk(eval(vectorize(funfcn)),length(varargin));
    else
      strfun = fcnchk(vectorize(funfcn),length(varargin));
    end
  catch
    strfun = fcnchk(funfcn,length(varargin));
  end

  if ( nargin<4 ) | isempty(tol)
    tol = 1e-6;                         % default (absolute) tolerance 1e-6
  end
  tol = max(tol,1e-16);                 % avoid tiny tolerances
  D = intval(b)-a;                      % inclusion of b-a
  if ( nargin<5 ) | isempty(see)
    see = 0;
  end
  if ( nargin<6 ) | isempty(mmax)
    mmax = 4;                           % up to 2*mmax+2 derivatives
  end
  if mmax>16
    warning('maximum order of Romberg scheme is mmax=16.')
    mmax = 16;
  end

  % store warning mode
  wng = warning;
  warning off

  I = infsup(-inf,inf);
  relerrold = inf;
  kmin = max(5,mmax);                   % start with at least 2^kmin subintervals

  % bounds for taylor^(2*m+2) for m=1..mmax
  xi = linspace(a,b,17);
  TF_ = feval(strfun,taylorinit(infsup(xi(1:end-1),xi(2:end)),2*mmax+2),varargin{:});
  if any(isnan(TF_.t(:))) | any(isinf(TF_.t(:)))  % second try with many subintervals
    xi = linspace(a,b,1025);
    TF_ = feval(strfun,taylorinit(infsup(xi(1:end-1),xi(2:end)),2*mmax+2),varargin{:});
  end
  if any(isnan(TF_.t(:))) | any(isinf(TF_.t(:)))  % derivative calculation failed
    X = feval(strfun,xi,varargin{:});
    if any(isnan(X(:))) | any(isinf(X(:)))
      error('function evaluation failed over interval [a,b]')
    else
      error('evaluation of higher derivatives failed, try smaller maximum order')
    end
  end
  for m=1:mmax
    TF{m} = infsup(min(TF_{2*m+2}.inf),max(TF_{2*m+2}.sup));
  end
  if see
    disp(['Inclusions of ' int2str(mmax) '-th derivative'])
    TF{mmax}
  end

  % first T{kmin..kmin-mmax+1}(m) for m=1
  k = kmin+1;
  h = 2^(-k)*D;                         % inclusion of (b-a)/2^k
  Xi = a + ( (0:2^k)*(2^(-k)) ) * D;    % inclusion of grid points a+i*(b-a)/2^k
  w = [ 1 repmat([4 2],1,2^(k-1)) ];    % no rounding errors
  w(end) = 1;
  y = feval(strfun,Xi,varargin{:})/3;
  T{k-1}(1) = h * sum(w.*y);
  for j=1:m-1
    w = w(1:(2^(k-j)+1));
    w(end) = 1;
    h = 2*h;                            % inclusion of (b-a)/2^k
    T{k-j-1}(1) = h * sum(w.*y(1:2^j:end));
  end

  % create tableaux up to mmax
  for m=2:mmax
    for k=kmin-mmax+1:kmin-m+1
      T{k}(m) = ( 4^m*T{k+1}(m-1) - T{k}(m-1) ) / ( 4^m-1 );
    end
  end

  % first error term for m=1    [ B(m) = +/- Bernoulli(2*m+2) ]
  B = intval([1 1 1 5 691 7 3617 43867 174611 854513 236364091 8553103 ...
                 23749461029 8615841276005 7709321041217 2577687858367]) ./ ...
             [30 42 30 66 2730 6 510 798 330 138 2730 6 870 14322 510 6];
  m = mmax;
  k = kmin-m+1;
  E = 2^(-2*k*(m+1)-m*(m+1)) * TF{m}*D^(2*m+3);
  err = B(m)*E;
  I = T{k}(m) - err;
  if see
    disp('First inclusion')
    I
  end

  % initialize Romberg iteration
  if in(0,I)
    relerrI = diam(I);
  else
    relerrI = relerr(I);
  end
  j = 0;

  while ( relerrI>tol ) & ( relerrI<relerrold ) & ( kmin+j<20 )
    j = j+1;
    Iold = I;
    relerrold = relerrI;
    k = kmin+j+1;
    % create entry T{kmin+j}(1)
    h = 2^(-k)*D;                         % inclusion of (b-a)/2^k
    Xi = a + ( (0:2^k)*(2^(-k)) ) * D;    % inclusion of grid points a+i*(b-a)/2^k
    w = [ 1 repmat([4 2],1,2^(k-1)) ];    % no rounding errors
    w(end) = 1;
    y = feval(strfun,Xi,varargin{:})/3;
    k = k-1;
    T{k}(1) = h * sum(w.*y);
    % update last row of Romberg table
    for m=2:mmax
      T{k-m+1}(m) = ( 4^m*T{k-m+2}(m-1) - T{k-m+1}(m-1) ) / ( 4^m-1 );
    end
    % new inclusion
    m = mmax;
    k = k-m+1;
    E = 2^(-2*k*(m+1)-m*(m+1)) * TF{m}*D^(2*m+3);
    err = B(m)*E;
    I = T{k}(m) - err;
    if see
      disp('New inclusion')
      I
    end
    if in(0,I)
      relerrI = diam(I);
    else
      relerrI = relerr(I);
    end
  end

  if relerrI>relerrold
    I = Iold;
    k = k-1;
  end

  % restore warning and exception mode
  warning(wng)
  % restore out-of-range exception mode
  intvalinit(RealStdFctsExcptnMode,0);

  if rndold
    setround(rndold)
  end


function [newIndex] = cycle(index, cycleLength)
%function [newIndex] = cycle(index, cycleLength)
%function to facilitate indexing on cyclical structures.
%The new index if the index cycled around on a structure of length cycleLength.
%e.g.: cycle(11, 10) = 1, cycle(12, 10) = 2, cycle(9, 10) = 9, etc.
%'index' can be a vector.
%JOD, Feb 02.
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

newIndex = zeros(size(index));

for i = 1 : length(index)
  if index(i) > cycleLength
    newIndex(i) = index(i) - cycleLength;
  elseif index(i) < 1
    newIndex(i) = cycleLength + index(i);
  else
    newIndex(i) = index(i);
  end
end


function fe2d_d_fast_test ( )

%*****************************************************************************80
%
%% FE2D_D_FAST_TEST tests the FE2D_D_FAST code.
%
%  Discussion:
%
%    This function sets all parameter values and initial condition information
%    necessary to execute the "fast" version of the fe2d_d algorithm.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    28 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Reference:
%
%    Marcus R Garvie, John Burkardt, Jeff Morgan,
%    Simple Finite Element Methods for Approximating Predator-Prey Dynamics
%    in Two Dimensions using MATLAB,
%    Submitted to Bulletin of Mathematical Biology, 2014.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FE2D_D_FAST_TEST:\n' );
  fprintf ( 1, '  Test the FE2D_D_FAST function\n' );
  fprintf ( 1, '  which applies Dirichlet boundary conditions as it\n' );
  fprintf ( 1, '  approximates a solution to a predator-prey system.\n' );
%
%  Set the parameters.
%
  alpha = 0.4;
  beta = 2.0;
  gamma = 0.6;
  delta = 1.0;
%
%  Use T=150.0 for normal run.
%  Use T=0.50 for a "quick" run that might take 15 minutes of computing.
%
% T = 150.0;
  T = 0.50;
  delt = 1.0 / 384.0;

  t = tic;
  fe2d_d_fast ( alpha, beta, gamma, delta, T, delt, @u0f, @v0f, @guf, @gvf );
  t = toc ( t );

  fprintf ( 1, '  Execution took %10.2g minutes \n', t / 60.0 );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FE2D_D_FAST_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end

function value = u0f ( x, y )

%*****************************************************************************80
%
%% U0F evaluates the initial condition for U.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    26 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location in the region.
%
%    Output, real VALUE, the initial condition for U at (X,Y).
%
  value = 6.0 / 35.0 - 2.0E-07 * ( x - 0.1 * y - 225.0 ) * ( x - 0.1 * y - 675.0 );

  return
end

function value = v0f ( x, y )

%*****************************************************************************80
%
%% V0F evaluates the initial condition for V.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    26 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location in the region.
%
%    Output, real VALUE, the initial condition for V at (X,Y).
%
  value = 116.0 / 245.0 - 3.0E-05 * ( x - 450.0 ) - 1.2E-04 * ( y - 150.0 );

  return
end

function value = guf ( x, y, t )

%*****************************************************************************80
%
%% GUF evaluates the Dirichlet boundary condition for U.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    28 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location on the boundary.
%
%    Input, real T, the time.
%
%    Output, real VALUE, the prescribed value for U at (X,Y,T).
%
  value = 0.0;

  return
end
function value = gvf ( x, y, t )

%*****************************************************************************80
%
%% GVF evaluates the Dirichlet boundary condition for V.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    28 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location on the boundary.
%
%    Input, real T, the time.
%
%    Output, real VALUE, the prescribed value for V at (X,Y,T).
%
  value = 0.0;

  return
end


%% RATE OF CONVERGENCE OF BILINEAR FINITE ELEMENT METHOD
%
% This example is to show the rate of convergence of bilinear finite
% element approximation of the Poisson equation on the unit square with the
% following boundary conditions:
%
% - Non-empty Dirichlet boundary condition.
% - Pure Neumann boundary condition.
% - Robin boundary condition.
%
% The basis, data structure and numerical test is summarized in <a
% href="matlab:ifem PoissonQ1femrate">PoissonQ1femrate</a>.
%
% See also Poissonfemrate, Poissonafemrate
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

clear variable
%% Setting
[node,elem] = squarequadmesh([0,1,0,1],1/2^3);
mesh  = struct('node',node,'elem',elem);
option.L0 = 2;
option.maxIt = 4;
option.printlevel = 1;
option.plotflag = 0;
option.elemType = 'Q1';

%% Non-empty Dirichlet boundary condition.
pde = sincosdata;
mesh.bdFlag = setboundary(node,elem,'Dirichlet','~(x==0)','Neumann','x==0');
femPoisson(mesh,pde,option);

%% Pure Neumann boundary condition.
pde = sincosNeumanndata;
mesh.bdFlag = setboundary(node,elem,'Neumann');
femPoisson(mesh,pde,option);

%% Pure Robin boundary condition.
option.plotflag = 0;
pde = sincosRobindata;
mesh.bdFlag = setboundary(node,elem,'Robin');
femPoisson(mesh,pde,option);

%% Conclusion
% To do:
% 1. Fix the getH1errorQ1. The H1 error is computed wrong.
% 2. For pure Neuman boundary condition, the rate is not quite right.


function varargout = cumprod(varargin)
%CUMPROD  Indefinite product integral of a CHEBFUN2.
%   G = CUMPROD(F) returns the CHEBFUN2 G = exp( cumsum(log(F)) )
%
%   G = CUMPROD(F, DIM) returns the CHEBFUN2 G = exp( cumsum(log(F), DIM) )
%
% See also CUMSUM, SUM, PROD.

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

[varargout{1:nargout}] = cumprod@separableApprox(varargin{:});

end


function out = CO_PartialAutoCorr(y,maxTau,whatMethod)
% CO_PartialAutoCorr   Compute the partial autocorrelation of an input time series
%
%---INPUTS:
% y, a scalar time series column vector.
%
% maxTau, the maximum time-delay. Returns for lags up to this maximum.
%
% whatMethod, the method used to compute: 'ols' or 'yule_walker'
%
%---OUTPUT: the partial autocorrelations across the set of time lags.
%

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
%% Check inputs and set defaults:
% ------------------------------------------------------------------------------
if nargin < 2
    % Use a maximum lag of 10 by default
    maxTau = 10;
end

if nargin < 3 || isempty(whatMethod)
    % ordinary least square by default
    whatMethod = 'ols';
end

%-------------------------------------------------------------------------------
%% Initial checks on maxTau
%-------------------------------------------------------------------------------
N = length(y); % time-series length

assert(maxTau > 0)

if maxTau < 0
    error('Negative time lags not applicable')
end

% ------------------------------------------------------------------------------
%% Do the computation
% ------------------------------------------------------------------------------

pacf = parcorr(y,'NumLags',maxTau,'Method',whatMethod);

% Zero lag is the first entry in the PACF (and should always be 1)

for i = 1:maxTau
    out.(sprintf('pac_%u',i)) = pacf(i+1);
end

end


function [operator_pt]=pt(operator, partition, dimensions)
%returns the partial transpose of the operator 'operator' with respect to
%the particles indicated by a one in the binary vector 'partition'.
%the optional parameter 'dimensions' includes the dimensions of the systems
%in array form. per default, it is assumed that each system is a qubit.

%otherwise, number of systems is given by the length of 'dimensions'
n=size(partition);
n=n(2);

if (nargin == 2)
    %if 'dimensions' are not given, assume qubits, i.e. always dimension 2
    dimensions = 2*ones(1,n);
else
    %throw an error if 'partion' and 'dimensions' have different length
    if (max(size(partition) ~= size(dimensions))==1)
        error('partition array and dimensions have different length');
    end
end

%throw an error if 'operator' is not a square matrix
opdims=size(operator);
if (opdims(1) ~= opdims(2))
    error('first argument is no square matrix');
end

%throw an error if 'operator' is not hermitian (within a certain precision)
if (max(max(abs(ctranspose(operator)-operator))) > 1e-12)
    error('first argument is not hermitian');
end

%throw an error if any value in 'dimensions' is smaller than two
if (min(dimensions)<2)
    error('dimensions must be larger than 1');
end

if (max(dimensions)==min(dimensions))
    %if all particles have the same dimension, we can simply use another
    %numerical system, e.g. the binary system. this speeds things up when
    %compared to the case in which the systems have different dimensions

    %obtain the dimensionality of each system (equals the first system's
    %dimensionsality, since all are the same)
    dim = dimensions(1);

    %throw an error if operator dimensions do not match the length of
    %'partition'
    if (opdims(1) ~= dim^n)
        error('operator dimensions do not match the partition array');
    end

    %******************* this is the main part *******************
    %start with the zero matrix to build up the partially transposed
    %operator
    oppt = zeros(dim^n,dim^n);

    %define the identity on the space of 'operator'
    id=eye(size(operator));

    for rowind=1:opdims(1) %loop through rows ...
        for colind=(rowind+1):opdims(2) %... and columns of the operator
                               %due to hermiticity and invariance of trace
                               %under partial transpose, only loop through
                               %upper right half
            col=dec2base(colind-1,dim,n); %determine current row and ...
            row=dec2base(rowind-1,dim,n); %column index in the base given by
                                      %'dimensions'

            %determine new row and column index by transposing the systems
            %indicated by 'partition'

            %for the new column index, take the ith digit from col if
            %partition(i) is zero. if partition(i) is one, take the ith
            %digit of row ...
            newcol=transpose((1-partition(:)).*str2num(col(:))+partition(:).*str2num(row(:)));
            %... and vice versa for the new row index
            newrow=transpose((1-partition(:)).*str2num(row(:))+partition(:).*str2num(col(:)));

            %note that newrow and newcol are row vectors. therefore,
            %convert them into strings and drop the white spaces
            newcol=strrep(num2str(newcol),' ','');
            newrow=strrep(num2str(newrow),' ','');

            %convert row and column index back into the decimal system
            newcolind=base2dec(newcol,dim)+1;
            newrowind=base2dec(newrow,dim)+1;

            %add the matrix element on its new place to oppt
            oppt=operator(rowind,colind)*(id(:,newrowind)*id(newcolind,:))+oppt;
        end
    end

    %add elements due to hermiticity
    oppt=oppt+ctranspose(oppt);
    %add diagonal elements (which did not change through the partial
    %transposition)
    oppt = oppt + diag(diag(operator));

else
    %if the system has different dimensions, the program is a bit more complex
    %and slower

    %throw an error if operator dimensions do not match the length of
    %'partition'
    if (opdims(1) ~= prod(dimensions))
        error('operator dimensions do not match the partition array');
    end

    %******************* this is the other main part *******************

    %start with the zero matrix to build up the partially transposed
    %operator
    oppt = zeros(size(operator));

    %define the identity on the space of 'operator'
    id=eye(size(operator));
    %define the n x n - identity
    idnxn=eye(n,n);

    %to loop through all matrix elements of 'operator', we need to create
    %the indices strings of the basis vectors in the usual notation. now,
    %however, the different digits run from zero to the corresponding
    %system's dimension (minus one), which differs from system to system.
    indices=zeros(n,1); %first index has only zeros

    for k=1:opdims(1) %loop through whole matrix 'operator'
        inddims=size(indices);
        last = indices(:,inddims(2)); %get the last index string

        for l=n:-1:1 %loop through digits of last index string
            if (last(l) < dimensions(l)-1) %the first digit from the right
                                           %which is still smaller than the
                                           %dimension (minus one) ...
                newvec=last+idnxn(:,l); %... must be increased by one ...
                newvec=newvec.*(vertcat(ones(l,1),zeros(n-l,1))); % ... and
                                       %all digits to the right set to zero
                indices=horzcat(indices,newvec); %append new index string
                break;
            end
        end
    end

    for rowind=1:opdims(1) %loop through rows ...
        for colind=(rowind+1):opdims(2) %... and columns of the operator
                               %due to hermiticity and invariance of trace
                               %under partial transpose, only loop through
                               %upper right half
            col=indices(:,colind); %write current row and ...
            row=indices(:,rowind); %column index as index string.
                                   %this time, row and col are column
                                   %vectors

            %determine new row and column index by transposing the systems
            %indicated by 'partition'

            %for the new column index, take the ith digit from col if
            %partition(i) is zero. if partition(i) is one, take the ith
            %digit of row ...
            newcol=transpose((1-partition(:)).*col(:)+partition(:).*row(:));
            %... and vice versa for the new row index
            newrow=transpose((1-partition(:)).*row(:)+partition(:).*col(:));

            %since newrow and newcol are row vectors denoting an index
            %string, we need to convert them back to a decimal number that
            %denotes the element's new position
            newcolind=find(ismember(transpose(indices),newcol,'rows'));
            newrowind=find(ismember(transpose(indices),newrow,'rows'));

            %add the matrix element on its new place to oppt
            oppt=operator(rowind,colind)*(id(:,newrowind)*id(newcolind,:))+oppt;
        end
    end

    %build the partial transpose of 'operator'
    %elements that used to be in the upper right half
    %oppt=full(sparse(colarray,rowarray,valarray,dim^n,dim^n,((dim^n)^2)/2-(dim^n)/2));
    %add elements due to hermiticity
    oppt=oppt+ctranspose(oppt);
    %add diagonal elements (which did not change through the partial
    %transposition)
    oppt = oppt + diag(diag(operator));
end

operator_pt=oppt;


function p=protate(p,phi)

%   Copyright (C) 2004-2006 Per-Olof Persson. See COPYRIGHT.TXT for details.

A=[cos(phi),-sin(phi);sin(phi),cos(phi)];
p=p*A;


%[2006]-"Ant Colony Optimization"

% (9/12/2020)

function ACS = jAntColonySystem(feat,label,opts)
% Parameters
tau   = 1;      % pheromone value
eta   = 1;      % heuristic desirability
alpha = 1;      % control pheromone
beta  = 1;      % control heuristic
rho   = 0.2;    % pheromone trail decay coefficient
phi   = 0.5;    % pheromena coefficient

if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'tau'), tau = opts.tau; end
if isfield(opts,'alpha'), alpha = opts.alpha; end
if isfield(opts,'beta'), beta = opts.beta; end
if isfield(opts,'rho'), rho = opts.rho; end
if isfield(opts,'eta'), eta = opts.eta; end
if isfield(opts,'phi'), phi = opts.phi; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial Tau & Eta
tau = tau * ones(dim,dim);
eta = eta * ones(dim,dim);
% Pre
fitG = inf;
fit  = zeros(1,N);
tau0 = tau;

curve = inf;
t = 1;
% Iterations
while t <= max_Iter
	% Reset ant
	X = zeros(N,dim);
	for i=1:N
    % Set number of features
    num_feat = randi([1,dim]);
    % Ant start with random position
    X(i,1)   = randi([1,dim]);
    k        = [];
    if num_feat > 1
      for d = 2:num_feat
        % Start with previous tour
        k      = [k(1:end), X(i, d-1)];
        % Edge / Probability Selection (4)
        P      = (tau(k(end),:) .^ alpha) .* (eta(k(end),:) .^ beta);
        % Set selected position = 0 probability (4)
        P(k)   = 0;
        % Convert probability (4)
        prob   = P ./ sum(P(:));
        % Roulette Wheel selection
        route  = jRouletteWheelSelection(prob);
        % Store selected position to be next tour
        X(i,d) = route;
      end
    end
  end
  % Binary
  X_bin = zeros(N,dim);
  for i = 1:N
    % Binary form
    ind           = X(i,:);
    ind(ind == 0) = [];
    X_bin(i, ind) = 1;
  end
  % Binary version
  for i = 1:N
    % Fitness
    fit(i) = fun(feat,label,X_bin(i,:),opts);
    % Global update
    if fit(i) < fitG
      Xgb  = X(i,:);
      fitG = fit(i);
    end
  end
  % Tau update
  tour            = Xgb;
  tour(tour == 0) = [];
  tour            = [tour(1:end), tour(1)];
  for d = 1 : length(tour) - 1
    % Feature selected
    x = tour(d);
    y = tour(d + 1);
    % Delta tau
    Dtau = 1 / fitG;
    % Update tau (10)
    tau(x,y) = (1 - phi) * tau(x,y) + phi * Dtau;
  end
  % Evaporate pheromone (9)
  tau = (1 - rho) * tau + rho * tau0;
  % Save
  curve(t) = fitG;
  fprintf('\nIteration %d Best (ACS)= %f',t,curve(t))
  t = t + 1;
end
% Select features based on selected index
Sf = unique(Xgb);
Sf(Sf == 0) = [];
sFeat = feat(:,Sf);
% Store results
ACS.sf = Sf;
ACS.ff = sFeat;
ACS.nf = length(Sf);
ACS.c  = curve;
ACS.f  = feat;
ACS.l  = label;
end

%// Roulette Wheel Selection //
function Index = jRouletteWheelSelection(prob)
% Cummulative summation
C = cumsum(prob);
% Random one value, most probability value [0~1]
P = rand();
% Route wheel
for i = 1:length(C)
	if C(i) > P
    Index = i;
    break;
  end
end
end


%% Weighted optimization for CP tensor decomposition with incomplete data
% We explain how to use |cp_wopt| with the POBLANO toolbox.

%% Create an example problem with missing data.
% Here we have 25% missing data and 10% noise.
R = 2;
info = create_problem('Size', [15 10 5], 'Num_Factors', R, ...
    'M', 0.25, 'Noise', 0.10);
X = info.Data;
P = info.Pattern;
M_true= info.Soln;

%% Create initial guess using 'nvecs'
M_init = create_guess('Data', X, 'Num_Factors', R, ...
    'Factor_Generator', 'nvecs');

%% Set up the optimization parameters
% It's genearlly a good idea to consider the parameters of the optimization
% method. The default options may be either too stringent or not stringent
% enough. The most important options to consider are detailed here.

% Get the defaults
ncg_opts = ncg('defaults');
% Tighten the stop tolerance (norm of gradient). This is often too large.
ncg_opts.StopTol = 1.0e-6;
% Tighten relative change in function value tolearnce. This is often too large.
ncg_opts.RelFuncTol = 1.0e-20;
% Increase the number of iterations.
ncg_opts.MaxIters = 10^4;
% Only display every 10th iteration
ncg_opts.DisplayIters = 10;
% Display the final set of options
ncg_opts

%% Call the |cp_wopt| method
% Here is an example call to the cp_opt method. By default, each iteration
% prints the least squares fit function value (being minimized) and the
% norm of the gradient. The meaning of any line search warnings
% can be checked via <matlab:doc('cvsrch') doc cvsrch>.
[M,~,output] = cp_wopt(X, P, R, 'init', M_init, ...
    'alg', 'ncg', 'alg_options', ncg_opts);

%% Check the output
% It's important to check the output of the optimization method. In
% particular, it's worthwhile to check the exit flag.
% A zero (0) indicates successful termination with the gradient smaller
% than the specified StopTol, and a three (3) indicates a successful
% termination where the change in function value is less than RelFuncTol.
% The meaning of any other flags can be checked via
% <matlab:doc('poblano_params') doc poblano_params>.
exitflag = output.ExitFlag

%% Evaluate the output
% We can "score" the similarity of the model computed by CP and compare
% that with the truth. The |score| function on ktensor's gives a score in
% [0,1]  with 1 indicating a perfect match. Because we have noise, we do
% not expect the fit to be perfect. See <matlab:doc('ktensor/score') doc
% score> for more details.
scr = score(M,M_true)

%% Create a SPARSE example problem with missing data.
% Here we have 95% missing data and 10% noise.
R = 2;
info = create_problem('Size', [150 100 50], 'Num_Factors', R, ...
    'M', 0.95, 'Sparse_M', true, 'Noise', 0.10);
X = info.Data;
P = info.Pattern;
M_true= info.Soln;

%% Create initial guess using 'nvecs'
M_init = create_guess('Data', X, 'Num_Factors', R, ...
    'Factor_Generator', 'nvecs');

%% Set up the optimization parameters
% It's genearlly a good idea to consider the parameters of the optimization
% method. The default options may be either too stringent or not stringent
% enough. The most important options to consider are detailed here.

% Get the defaults
ncg_opts = ncg('defaults');
% Tighten the stop tolerance (norm of gradient). This is often too large.
ncg_opts.StopTol = 1.0e-6;
% Tighten relative change in function value tolearnce. This is often too large.
ncg_opts.RelFuncTol = 1.0e-20;
% Increase the number of iterations.
ncg_opts.MaxIters = 10^4;
% Only display every 10th iteration
ncg_opts.DisplayIters = 10;
% Display the final set of options
ncg_opts

%% Call the |cp_wopt| method
% Here is an example call to the cp_opt method. By default, each iteration
% prints the least squares fit function value (being minimized) and the
% norm of the gradient. The meaning of any line search warnings
% can be checked via <matlab:doc('cvsrch') doc cvsrch>.
[M,~,output] = cp_wopt(X, P, R, 'init', M_init, ...
    'alg', 'ncg', 'alg_options', ncg_opts);

%% Check the output
% It's important to check the output of the optimization method. In
% particular, it's worthwhile to check the exit flag.
% A zero (0) indicates successful termination with the gradient smaller
% than the specified StopTol, and a three (3) indicates a successful
% termination where the change in function value is less than RelFuncTol.
% The meaning of any other flags can be checked via
% <matlab:doc('poblano_params') doc poblano_params>.
exitflag = output.ExitFlag

%% Evaluate the output
% We can "score" the similarity of the model computed by CP and compare
% that with the truth. The |score| function on ktensor's gives a score in
% [0,1]  with 1 indicating a perfect match. Because we have noise, we do
% not expect the fit to be perfect. See <matlab:doc('ktensor/score') doc
% score> for more details.
scr = score(M,M_true)


function [feat_vector] = SPC_extract_featuresAR(data)
no_range = 0;
if(range(data)<0.01)
    no_range = 1;
    data = randn(size(data));
end

dat1=data(1:floor(end/2));
dat2=data(ceil(end/2)+1:end);
fit = ar_prediction_error(dat1,dat2,9);
AR1 = fit(1);
AR2 = fit(2);
AR3 = fit(3);
AR4 = fit(4);
AR5 = fit(5);
AR6 = fit(6);
AR7 = fit(7);
AR8 = fit(8);
AR9 = fit(9);

feat_vector = [AR1;AR2;AR3;AR4;AR5;AR6;AR7;AR8;AR9];

if(no_range == 1)
    feat_vector = NaN(size(feat_vector));
end


function demo_excludeData ()
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [posterior, out] = demo_excludeData ()
% Demo of simulation and inversion of a dynamical system that is sampled on
% an irregular grid (not all timepoints are observed)
%
% More generally, this demo shows how to deal with data exclusion
%
% /////////////////////////////////////////////////////////////////////////

% number of simulated points
N = 257;

%% Specify the model
% =========================================================================
% For the sake of the demonstration, we use a simple linear dynamical
% system $ dx/dt = Ax + phi' u $ that is directly observed $ y = x + noise $

% evolution function
% -------------------------------------------------------------------------
options.inF.A = [- 4, - 16; 4, - 4];
options.inF.dt = 1e-1;

function fx = f_evolution(x,P,u,in)
    xdot = in.A * x + diag(P) * u;
    fx = x + in.dt * xdot;
end

f_fname = @f_evolution;

% observation function
% -------------------------------------------------------------------------
g_fname = @g_Id;

%% Simulate data
% =========================================================================

% inputs
u = randn(2, N);

% parameters
x0 = [0; 0]; % initial state
theta = [1; 2]; % effect of inputs
phi = []; % no observation parameters
alpha = Inf; % deterministic
sigma = 1; % observation noise

% Build full time series of hidden states and observations
[y,x,x0,eta,e] = VBA_simulate (N,f_fname,g_fname,theta,phi,u,alpha,sigma,options,x0);

% display full time series of hidden states and observations
displaySimulations(y,x,eta,e);

%% Decimation
% =========================================================================
% here we will simulate a sampling of data on an irregular grid  by
% 'removing' data from the full simulation

% sampling grid (exponential timestps)
timepoints = 2 .^ (1 : floor (log (N) ./ log (2)));

% exclude all data but timepoints:
% if options.isYout = 1, the datapoint is ignored
options.isYout = ones (size (y));
options.isYout(:, timepoints) = 0;

%% Inversion
% =========================================================================

% dimensions of the problem
dim.n_theta = 2;
dim.n_phi = 0;
dim.n = 2;
dim.p = 2;

% Invert deterministic model
[posterior,out] = VBA_NLStateSpaceModel(y,u,f_fname,g_fname,dim,options);

% display
displayResults(posterior,out,y-e,x,x0,theta,phi,alpha,sigma);

end


classdef NNOP < Algorithm
    %NNOP Neural Network with Ordered Partitions (NNOP). This model
    % considers the OrderedPartitions coding scheme for the labels and a
    % rule for decisions based on the first node whose output is higher
    % than a predefined threshold (T=0.5, in our experiments). The
    % model has one hidden layer with hiddenN neurons and one outputlayer
    % with as many neurons as the number of classes minus one. The learning
    % is based on iRProp+ algorithm and the implementation provided by
    % Roberto Calandra in his toolbox Rprop Toolbox for {MATLAB}:
    % http://www.ias.informatik.tu-darmstadt.de/Research/RpropToolbox
    % The model is adjusted by minimizing mean squared error. A regularization
    % parameter "lambda" is included based on L2, and the number of
    % iterations is specified by the "iter" parameter.
    %   NNPOM methods:
    %      fitpredict               - runs the corresponding algorithm,
    %                                   fitting the model and testing it
    %                                   in a dataset.
    %      fit                        - Fits a model from training data
    %      predict                    - Performs label prediction
    %
    %   NNPOM properties:
    %      epsilonInit                - Range for initializing the weights.
    %      parameters.hiddenN         - Number of hidden neurons of the
    %                                   model.
    %      parameters.iter            - Number of iterations for iRProp+
    %                                   algorithm.
    %      parameters.lambda          - Regularization parameter.
    %
    %   References:
    %     [1] J. Cheng, Z. Wang, and G. Pollastri, "A neural network
    %         approach to ordinal regression," in Proc. IEEE Int. Joint
    %         Conf. Neural Netw. (IEEE World Congr. Comput. Intell.), 2008,
    %         pp. 1279-1284.
    %     [2] P.A. Gutiérrez, M. Pérez-Ortiz, J. Sánchez-Monedero,
    %         F. Fernández-Navarro and C. Hervás-Martínez
    %         Ordinal regression methods: survey and experimental study
    %         IEEE Transactions on Knowledge and Data Engineering, Vol. 28.
    %         Issue 1, 2016
    %         http://dx.doi.org/10.1109/TKDE.2015.2457911
    %
    %   This file is part of ORCA: https://github.com/ayrna/orca
    %   Original authors: Pedro Antonio Gutiérrez, María Pérez Ortiz, Javier Sánchez Monedero
    %   Citation: If you use this code, please cite the associated paper http://www.uco.es/grupos/ayrna/orreview
    %   Copyright:
    %       This software is released under the The GNU General Public License v3.0 licence
    %       available at http://www.gnu.org/licenses/gpl-3.0.html

    properties
        description = 'Neural Network with Ordered Partitions';
        % Weights range
        epsilonInit = 0.5;
        parameters = struct('iter', 500,'hiddenN', 50,'lambda', 0.01);
    end

    methods

        function obj = NNOP(varargin)
            %NNOP constructs an object of the class NNOP and sets its default
            %   characteristics
            %   obj = NNOP('epsilonInit', 0.5) sets initialization of
            %   epsilon to 0.5
            obj.parseArgs(varargin);
        end

        function obj = set.epsilonInit(obj,e)
            if strcmp(class(obj.epsilonInit), class(e))
                obj.epsilonInit= e;
            else
                error('epsilonInit type is ''%s'' and ''%s'' was provided', class(obj.epsilonInit), class(e))
            end
        end

        function [projectedTrain, predictedTrain] = privfit( obj, train, parameters)
            %PRIVFIT trains the model for the NNOP method with TRAIN data and
            %vector of parameters PARAM.

            % Aux variables
            X = train.patterns;
            y = train.targets;
            input_layer_size  = size(X,2);
            hidden_layer_size = parameters.hiddenN;
            num_labels = numel(unique(y));
            m = size(X,1);

            % Recode y to Y using ordered partitions
            Y = repmat(y,1,num_labels) <= repmat((1:num_labels),m,1);

            % Hidden layer weigths (with bias)
            initial_Theta1 = obj.randInitializeWeights(input_layer_size+1, hidden_layer_size);
            % Output layer weigths (without bias, the biases will be the
            %                       Thresholds)
            initial_Theta2 = obj.randInitializeWeights(hidden_layer_size+1, num_labels-1);

            % Pack parameters
            initial_nn_params = [initial_Theta1(:) ; initial_Theta2(:)];

            % Set regularization parameter
            lambda = parameters.lambda;

            % Create "short hand" for the cost function to be minimized
            costFunction = @(p) obj.nnOPCostFunction(p, ...
                input_layer_size, ...
                hidden_layer_size, ...
                num_labels, X, Y, lambda);

            % RProp options
            p.verbosity = 0;                    % Increase indent
            p.MaxIter   = parameters.iter;     	% Maximum number of iterations
            p.d_Obj     = -1;                   % Objective cost
            p.method    = 'IRprop+';            % Use IRprop- algorithm
            p.display   = 0;

            % Running RProp
            [nn_params,cost,exitflag,stats1] = rprop(costFunction,initial_nn_params,p);

            %             options = optimoptions('fminunc','Algorithm','quasi-newton','SpecifyObjectiveGradient',true,'Diagnostics','on','Display','iter-detailed','UseParallel',true,'MaxIter', 1000,'CheckGradients',true);
            %             [nn_params, cost, exitflag, output] = fminunc(costFunction, initial_nn_params, options);

            % Unpack the parameters
            [Theta1, Theta2] = obj.unpackParameters(nn_params,input_layer_size,hidden_layer_size,num_labels);
            model.Theta1=Theta1;
            model.Theta2=Theta2;
            model.num_labels=num_labels;
            model.m = m;
            model.parameters = parameters;
            obj.model = model;
            [projectedTrain, predictedTrain] = obj.predict(train.patterns);
        end

        function [projected, predicted]= predict(obj,test)
            %PREDICT predicts labels of TEST patterns labels. The object needs to be fitted to the data first.
            m = size(test,1);
            a1 = [ones(m, 1) test];
            z2 = [ones(m, 1) a1*obj.model.Theta1'];
            a2 =  1.0 ./ (1.0 + exp(-z2));
            projected=a2*obj.model.Theta2';
            projected=1.0 ./ (1.0 + exp(-projected));

            a3 = ([projected ones(m,1)] > 0.5).*repmat(1:obj.model.num_labels,m,1);
            a3(a3==0)=obj.model.num_labels+1;

            predicted = min(a3,[],2);
        end

    end

    methods(Access = private)

        function [Theta1, Theta2] = unpackParameters(obj,nn_params,input_layer_size,hidden_layer_size,num_labels)
            % UNPACKPARAMETERS obtains Theta1 and Theta2
            % back from the whole array nn_params
            nTheta1 = hidden_layer_size * (input_layer_size + 1);
            Theta1 = reshape(nn_params(1:nTheta1), ...
                hidden_layer_size, (input_layer_size + 1));
            Theta2 = reshape(nn_params((1+nTheta1):end), ...
                num_labels-1, (hidden_layer_size+1));
        end

        function W = randInitializeWeights(obj, L_in, L_out)
            %RANDINITIALIZEWEIGHTS randomly initializes the weights of a layer with L_in
            %incoming connections and L_out outgoing connections
            W = rand(L_out, L_in)*2*obj.epsilonInit - obj.epsilonInit;
        end

        function [J,grad] = nnOPCostFunction(obj, nn_params, ...
                input_layer_size, ...
                hidden_layer_size, ...
                num_labels, ...
                X, Y, lambda)
            %NNPOMCOSTFUNCTION implements the cost function and obtains the
            %corresponding derivatives.

            % Unroll all the parameters
            [Theta1, Theta2] = unpackParameters(obj,...
                nn_params,input_layer_size,hidden_layer_size,num_labels);

            % Setup some useful variables
            m = size(X, 1);

            % Neural Network model
            a1 = [ones(m, 1) X];
            z2 = a1*Theta1';
            a2 =  [ones(m, 1) (1.0 ./ (1.0 + exp(-z2)))];
            z3=a2*Theta2';
            h =  [1.0 ./ (1.0 + exp(-z3)) ones(m, 1)];

            % Final output
            out = h;

            % calculte penalty (regularización L2)
            p = sum(sum(Theta1(:, 2:end).^2, 2))+sum(sum(Theta2(:, 2:end).^2, 2));

            % MSE
            J = sum(sum((out-Y).^2, 2))/(2*m) + lambda*p/(2*m);
            % Cross entropy
            %J = sum(-log(out(Y==1)), 1)/m + lambda*p/(2*m);
            if nargout > 1
                % Cross entropy
                %out(out<0.00001)=0.00001;
                %errorDer = zeros(size(Y));
                %errorDer(Y~=0) = (-Y(Y~=0)./out(Y~=0));

                % MSE
                errorDer=(out-Y);

                % Calculate sigmas
                sigma3 = errorDer.*h.*(1-h);
                sigma3 = sigma3(:,1:(end-1));
                sigma2 = (sigma3*Theta2).*a2.*(1-a2);
                sigma2 = sigma2(:, 2:end);

                % Accumulate gradients
                delta_1 = (sigma2'*a1);
                delta_2 = (sigma3'*a2);

                % calculate regularized gradient
                p1 = (lambda/m)*[zeros(size(Theta1, 1), 1) Theta1(:, 2:end)];
                p2 = (lambda/m)*[zeros(size(Theta2, 1), 1) Theta2(:, 2:end)];
                Theta1_grad = delta_1./m + p1;
                Theta2_grad = delta_2./m + p2;

                % Unroll gradients
                grad = [Theta1_grad(:) ; Theta2_grad(:)];
            end

        end
    end
end


function [D,X,Err] = simplestNMF(Y, opts)
    [n,T] = size(Y);

    if nargin<2
        opts = struct();
    end
    opts = initialize_opts(Y, opts);
    D = opts.D0;
    X = opts.X0;

    iter = 0;
    Err = zeros(opts.max_iter, 1);
    lambda_factor = repmat(opts.lambda.*ones(opts.m,1), 1, T);

    %% ----- Preprocess [start] -----
    nmflib_in_options.metric_type = opts.metric_type;
    nmflib_in_options.verbose = opts.verbose;
    nmflib_in_options.x_init.W = D;
    nmflib_in_options.x_init.H = X;
    nmflib_in_options.d_beta = opts.d_beta;
    [D, X, ~, nmflib_infos, nmflib_options] = pre_process('simplestNMF', Y, nmflib_in_options);
    % other necessary processes should be inserted here.
    %% ----- Preprocess [end] -----

    while iter < opts.max_iter
        iter = iter + 1;

        %update D
        if ~isempty(opts.updateD)
            %DX = D(:, opts.updateD)*X(opts.updateD, :);
            DX = D*X;
            if opts.d_beta<2
                DX(DX==0) = eps;
            end
            pgradD = (DX.^(opts.d_beta-1))*X(opts.updateD, :)';
            pgradD(pgradD==0)=eps;
            ngradD = ((DX.^(opts.d_beta-2)).*Y)*X(opts.updateD, :)';
            D(:, opts.updateD) = D(:, opts.updateD) .* ngradD ./ pgradD;
        end

        %update X
        DX = D*X;
        if opts.d_beta<2
            DX(DX==0) = eps;
        end
        pgradX = D'*(DX.^(opts.d_beta-1));
        ngradX = D'*((DX.^(opts.d_beta-2)).*Y);
        X = X .* ngradX ./ (pgradX + lambda_factor);

        DX = D*X;
        DX(DX==0) = eps;
        Err(iter) = beta_divergence(Y, DX, opts.d_beta);
        %sparsy = sum(sum(lambda_factor.*X));
        delta = inf;
        if iter>1
            delta = (Err(iter-1)-Err(iter))/Err(iter-1);
        end

        %% ----- Innerprocess [start] -----
        nmflib_infos = inner_process('simplestNMF', Y, D, X, [], nmflib_options, nmflib_infos, iter);
        %% ----- Innerprocess [end] -----

        %fprintf('iter = %d, Err = %f, delta = %f, sparsy = %f\n', iter, Err(iter), delta, sparsy);
        %if delta < opts.conv_value
        %    break;
        %end
    end
    Err(iter+1:end) = [];
end

function opts = initialize_opts(Y, opts)
    [n, T] = size(Y);
    if ~isfield(opts, 'm') opts.m = 1; end
    if ~isfield(opts, 'conv_value') opts.conv_value = 1e-3; end
    if ~isfield(opts, 'max_iter') opts.max_iter = 1000; end
    if ~isfield(opts, 'lambda') opts.lambda = eps; end
    if ~isfield(opts, 'beta') opts.d_beta = 2; end
    if ~isfield(opts, 'updateD') opts.updateD = 1:opts.m; end
    if ~isfield(opts, 'D0') opts.D0 = rand(n, opts.m); end
    if ~isfield(opts, 'X0') opts.X0 = rand(opts.m, T); end
end

function r = beta_divergence(A, B, beta)
    switch beta
        case 0
            r = sum(sum (A./B - log(A./B+eps) - 1));
        case 1
            r = sum(sum(A.*log(A./B+eps) - A + B));
        case 2
            r = .5 * norm(A - B, 'fro').^2;
    end
end


function [output binarizedContour] = CORF(I, sigma, t)
% CORF Contour detection based on a computational model of a simple cell.
%
% VERSION 22/04/2012
% CREATED BY: George Azzopardi and Nicolai Petkov, University of Groningen,
%             Johann Bernoulli Institute for Mathematics and Computer Science, Intelligent Systems
%
%If you use this script please cite the following paper:
%   George Azzopardi and Nicolai Petkov, "A CORF computational model of a
%     simple cell that relies on LGN input outperforms the Gabor function
%     model", 2012, DOI: 10.1007/s00422-012-0486-6
%
%   CORF achieves orientation selectivity by combining the output - at certain
%   positions with respect to the center of the CORF operator - of center-on
%   and center-off difference of Gaussians (DoG) functions by a weighted geometric mean.
%
%   CORF takes as input:
%      I -> intensity image
%      sigma -> the standard deviation of the outer Gaussian function of the
%               DoG operator
%      t -> high threshold used for hysteresis thresholding
%
%   CORF returns:
%      output -> maximum superposition of CORF responses that correspond to 12 orientations
%      binarizedContour -> A contour map that is obtaiend by first thinning
%                          output and then performs hysteresis thresholding
%                          with a high threshold t and a low threshold that
%                          is a fraction 0.5 of the given t.
%
%   Example: [o bc] = CORF(imread('rino.pgm'),2.5,0.3);
%
%   The image rino.pgm is taken from the RuG data set of 40 images of
%   natural scenes, which can be downloaded from:
%   http://www.cs.rug.nl/~imaging/databases/contour_database/contour_database.html

% configure CORF operator
operator = configureCORF(sigma,0.5);

% Set number of orientations
noriens = 12;
orienslist = 0:180/noriens:359;

% Preprocessing
inputImage = double(I);
if ndims(inputImage) == 3
    inputImage = double(rgb2gray(inputImage));
end
if max(inputImage(:)) > 1
    inputImage = inputImage ./ 255;
end

% Apply difference of Gaussians function.
padwidth = ceil(max(operator.params.rho));
DoGresponse(:,:,1) = applyDoG(inputImage, 0, padwidth, operator.params.sigma, operator.params.sigmaRatio, 0);
DoGresponse(:,:,2) = -DoGresponse(:,:,1);
DoGresponse(:,:,1) = DoGresponse(:,:,1) .* (DoGresponse(:,:,1) > 0);
DoGresponse(:,:,2) = DoGresponse(:,:,2) .* (DoGresponse(:,:,2) > 0);

% Apply the CORF oeprator at different orientations
data = [];
output = zeros(size(DoGresponse,1),size(DoGresponse,2),noriens);
for rot = 1:length(orienslist)
    rotatedOperator = operator;
    rotatedOperator.tuple(4,:) = rotatedOperator.tuple(4,:) + orienslist(rot)*pi/180;
    [output(:,:,rot) data] = getCORFresponse(DoGresponse,rotatedOperator,data);
end
output = output(padwidth+1:end-padwidth,padwidth+1:end-padwidth,:);

% Merge output of all orientations by using maximum superposition
[output, oriensMatrix] = calc_viewimage(output,1:size(output,3), orienslist*pi/180);

% Perform thinning
thinningOutput = thinning(output, oriensMatrix, 2);

%Perform hysteresis thresholding
t = t * max(thinningOutput(:));
binarizedContour = hysthresh(thinningOutput, t, 0.5*t);

function [output data] = getCORFresponse(DoGresponse,operator,data)

sz = [size(DoGresponse,1),size(DoGresponse,2)];

if isempty(data)
    data.params = [];
    data.tupleOutput = cell(0);
    data.location = [];
    data.paramsindex = 1;
    data.current.tupleOutput = zeros(sz(1),sz(2),size(operator.tuple,2));
end

% The weight vector is requried for the computation of the weighted
% geometric mean at the bottom of the function.
weightVector = zeros(1,size(operator.tuple,2));
sgm = max(operator.tuple(3,:)) / 3;

for i = 1:size(operator.tuple,2)
    polarity = operator.tuple(1,i);
    rho = operator.tuple(3,i);
    phi = operator.tuple(4,i);

    weightVector(i) = (exp(-rho^2/(2*sgm*sgm)));

    mem = find(ismember(data.params,[polarity rho],'rows'),1);
    if ~isempty(mem)
        % This tuple output is obtained by appropriate shifting of another
        % tuple output that was computed for the same values of polarity,
        % sigma and rho.
        [col row] = pol2cart(phi,rho);
        shiftrow = -(data.location{mem}(1)-fix(row));
        shiftcol = data.location{mem}(2)-fix(col);
        data.current.tupleOutput(:,:,i) = circshift(data.tupleOutput{mem},[shiftrow,shiftcol]);
    else
        DoG = DoGresponse(:,:,polarity+1);
        data.params(data.paramsindex,:) = [polarity rho];
        [col row] = pol2cart(phi,rho);

        r = (operator.params.d0 + operator.params.alpha*rho)/2;
        if r > 0
            smoothfilter = fspecial('gaussian',round([2*r+1,2*r+1]),r/3);
            data.current.tupleOutput(:,:,i) = conv2(DoG,smoothfilter,'same');
            data.current.tupleOutput(:,:,i) = circshift(data.current.tupleOutput(:,:,i),[fix(row),fix(-col)]);
        else
            data.current.tupleOutput(:,:,i) = DoG;
        end
        data.current.tupleOutput(:,:,i) = data.current.tupleOutput(:,:,i) .^ weightVector(i);

        % We use the following variables to reuse the computations obtained
        % for the same values of parameters: polarity, sigma and rho.
        data.tupleOutput{data.paramsindex} = data.current.tupleOutput(:,:,i);
        data.location{data.paramsindex} = [round(row) round(col)];
        data.paramsindex = data.paramsindex + 1;
    end
end

% compute the weighted geometric mean
output = prod(data.current.tupleOutput,3).^(1/sum(weightVector));

function operator = configureCORF(sigma, sigmaRatio)
% configureCORF: configure a CORF operator for the given parameters.

% The parameters alpha and d0 are set as reported in the above mentioned paper
operator.params.alpha = 0.9;
operator.params.d0 = 2;

operator.params.sigma = sigma;
operator.params.sigmaRatio = sigmaRatio;

% The following are the rho values that were used in the experiments
% reported in the above mentioned paper.
if sigma >= 1 && sigma < 2.5
    operator.params.rho = [14.38 6.9796 3.0310 1.4135];
elseif sigma >= 2.5 && sigma < 4
    operator.params.rho = [3.0515 6.1992 12.6488 24.62];
elseif sigma >= 4 && sigma <= 5
    operator.params.rho = [3.3021 4.7877 9.2467 18.08 34.43];
else
    error('The value of the parameter sigma is out of bounds');
end

maxRadius = ceil(max(operator.params.rho))+1;
stimulus = zeros((2*maxRadius));
stimulus(:,1:maxRadius) = 1;
center = [maxRadius maxRadius];

%Obtain the output of DoG function for the synthetic edge stimulus
DoGresponse(:,:,1) = applyDoG(stimulus, 0, maxRadius, sigma, sigmaRatio, 1);
DoGresponse(:,:,2) = -DoGresponse(:,:,1);
DoGresponse(:,:,1) = DoGresponse(:,:,1) .* (DoGresponse(:,:,1) > 0);
DoGresponse(:,:,2) = DoGresponse(:,:,2) .* (DoGresponse(:,:,2) > 0);

operator.tuple = [];
for r = 1:length(operator.params.rho)
    [polarity rho phi] = getTuple(DoGresponse,operator.params.rho(r),center);
    operator.tuple = [operator.tuple [polarity; repmat(sigma,1,length(polarity)); rho; phi]];
end

function [polarity rho phi] = getTuple(DoGresponse,radius,fp)
% Determine the values of the polar coordinates (rho,phi)

if radius <= 0
    error('Parameter radius must be greater than 0');
end

phi = []; polarity = [];
x = 1:360;

for pol = 1:size(DoGresponse,3)
    DoG = DoGresponse(:,:,pol);
    y = DoG(sub2ind(size(DoG),round(fp(1) + radius*cos(pi/2+x*pi/180)),round(fp(2) + radius*sin(pi/2+x*pi/180))));

    % Threshold low values
    y(y < 0.1*max(DoGresponse(:))) = 0;
    y = round(y*1000)/1000;

    BW     = bwlabel(imregionalmax(y));
    npeaks = max(BW(:));

    for i = 1:npeaks
        phi(end+1) = mean(x(BW == i)) * pi/180;
        polarity(end+1) = pol - 1;
    end
end
rho = repmat(radius,1,length(phi));

function output = applyDoG(inputImage,polarity,padwidth,sigma,sigmaRatio,crop)

% pad input image
paddedInputImage = padarray(inputImage,[padwidth padwidth],'both','symmetric');

% create DoG operator
sz = size(inputImage) + padwidth + padwidth;
g1 = fspecial('gaussian',sz,sigma);
g2 = fspecial('gaussian',sz,sigma*sigmaRatio);
if polarity == 1
    DoG = g2 - g1;
elseif polarity == 0
    DoG = g1 - g2;
else
    error('Polarity must be either 0 (on) or 1 (off)');
end

% compute DoG
output = fftshift(ifft2(fft2(DoG,sz(1),sz(2)) .* fft2(paddedInputImage)));

if crop == 1
    output = output(padwidth+1:end-padwidth,padwidth+1:end-padwidth);
end

function [result, oriensMatrix] = calc_viewimage(matrices, dispcomb, theta)
% VERSION 14/05/04
% CREATED BY: M.B. Wieling and N. Petkov, Groningen University,
%             Department of Computer Science, Intelligent Systems
%
% CALC_VIEWIMAGE: calculates the maximum-superposition of all the matrices stored
% in MATRICES (according to the L-infinity norm). It uses only the matrices for
% which the index is entered in DISPCOMB, e.g. if DISPCOMB contains the values
% 1,2,4: only the first, second and fourth matrix contained in MATRICES are used
% for the superposition. This method also calculates the orientationmatrix (ORIENSMATRIX)
% which stores the maximum orientation response of each point in the resulting matrix
% (RESULT) - for use in CALC_THINNING. A progressbar of the calculations is also shown.
%   CALC_VIEWIMAGE(MATRICES, DISPCOMB, THETA)
%   calculates the single viewing image according to the following parameters
%     MATRICES - the matrices which hold all the convolutions for each orientation
%     DISPCOMB  - the indexes of each matrix (in MATRICES) which should be used for the
%                 superposition
%     THETA - a list of all the orientations - to create ORIENSMATRIX

% initialize values
oriensMatrix = 0;
tmpMaxConv = -Inf;
result = -Inf;
cnt1 = 1;

if (size(dispcomb,2) == 1)
  result = matrices(:,:,dispcomb(1));
else

  % calculate the superposition (L-infinity norm)
  while (cnt1 <= size(dispcomb,2))
    % calculate the maximum orientation-response in each point (based on the absolute values)
    oriensMatrixtmp1 = (abs(matrices(:,:,dispcomb(cnt1))) > tmpMaxConv) .* theta(dispcomb(cnt1));
    oriensMatrixtmp2 = (abs(matrices(:,:,dispcomb(cnt1))) <= tmpMaxConv) .* oriensMatrix;
    oriensMatrix = oriensMatrixtmp1 + oriensMatrixtmp2;
    tmpMaxConv = max(abs(matrices(:,:,dispcomb(cnt1))), tmpMaxConv);

    % calculate the superposition
    result = max(result,abs(matrices(:,:,dispcomb(cnt1))));
    cnt1 = cnt1 + 1;
  end
end

function result = thinning(matrix, oriensMatrix, method)
% VERSION 27/02/05
% CREATED BY: M.B. Wieling and N. Petkov, Groningen University,
%             Department of Computer Science, Intelligent Systems
%
% THINNING: reduces the edges to a width of 1 pixel. This is done
%           by looking at the two pixels next to the current pixel.
%           the neighbourpixels are determined by the orientation of
%           the current point (this is stored in the exact same location
%           as in the matrix ORIENSMATRIX). A progressbar of the
%           calculations is also shown.
%   THINNING(MATRIX, ORIENSMATRIX, METHOD) thins the edge
%      MATRIX - the matrix which should be thinned
%      ORIENSMATRIX - the matrix which holds for every pixel of MATRIX the
%                     orientation (in the same position)
%      METHOD - the method of thinning: METHOD == 1: simple method, just
%               take the value of the nearest pixels to compare with.
%               e.g. if the orientation = 20 degrees, the points S & N are chosen
%               to compare to, if the orientation = 25 degrees the points SW & NE
%               are chosen (see below, P = current point)
%                             NW  N  NE
%                             W   P   E
%                             SW  S  SE
%               METHOD == 2: the values to compare with are calculated using
%               interpolation based on the surrounding pixels (NW, N, NE, E, SE, S, SW, W)
%
% corrected an error: <pi := <=pi

% create the coordinate-system
h = size(matrix,1); % height of image
w = size(matrix,2); % width of image
mx = max(h,w);
[xcoords, ycoords] = meshgrid(1:mx);
xcoords = xcoords(1:h, 1:w);
ycoords = ycoords(1:h, 1:w);

% set every value between 0 and pi
oriensMatrix = mod(oriensMatrix, pi);

% add a border of zeros to 'matrix' and 'oriensMatrix' to ease calculations.
matrixB(h+2,w+2) = 0;
matrixB(2:h+1,2:w+1) = matrix(:,:);
oriensMatrixB(h+2,w+2) = 0;
oriensMatrixB(2:h+1,2:w+1) = oriensMatrix(:,:);

if (method == 1) % simple thinning
  result = 0;
  for I=2:h+1 % the rows
    for J=2:w+1 % the columns
      orien = oriensMatrixB(I,J);

      % calculate dx and dy - this is correct as can be seen
      % in a picture
      dx = (orien < (3/8)*pi) - (orien > (5/8)*pi);
      dy = ((orien > (1/8)*pi) & (orien <= (1/2)*pi)) - ((orien > (1/2)*pi) & (orien < (7/8)*pi));

      % normally the same pixels would be checked if (dy and dx > 0) and (dy and dx < 0). because
      % different pixels should be checked with a gabor-orientation of 45 and 135 this difference should
      % be checked
      if (dy < 0) & (dx < 0)
       result(I,J) = ((matrixB(I,J) >= matrixB(I+dy, J+dx)) & (matrixB(I,J) >= matrixB(I-dy, J-dx))) * matrixB(I,J);
      else
       result(I,J) = ((matrixB(I,J) >= matrixB(I-dy, J+dx)) & (matrixB(I,J) >= matrixB(I+dy, J-dx))) * matrixB(I,J);
      end
    end
  end
else % linear thinning
  %hb = waitbar(0,'Applying linear thinning, please wait ... (Step 6/7)'); % display a progressbar
  result = 0;
  for I=2:h+1 % the rows
    for J=2:w+1 % the columns
      orien = oriensMatrixB(I,J);
      % get the values of the surrounding pixels
      north = matrixB(I-1, J);
      northeast = matrixB(I-1, J+1);
      east = matrixB(I, J+1);
      southeast = matrixB(I+1, J+1);
      south = matrixB(I+1, J);
      southwest = matrixB(I+1, J-1);
      west = matrixB(I, J-1);
      northwest = matrixB(I-1, J-1);

      % calculate the value of the points in one line (using interpolation)
      if (orien <= (1/4)*pi)
          fraction = orien/((1/4)*pi);
          pnt1 = (1-fraction) * east + (fraction) * northeast;
          pnt2 = (1-fraction) * west + (fraction) * southwest;
      elseif (orien <= (1/2)*pi)
          fraction = (orien-(1/4)*pi)/((1/4)*pi);
          pnt1 = (1-fraction) * northeast + (fraction) * north;
          pnt2 = (1-fraction) * southwest + (fraction) * south;
      elseif (orien <= (3/4)*pi)
          fraction = (orien-(1/2)*pi)/((1/4)*pi);
          pnt1 = (1-fraction) * north + (fraction) * northwest;
          pnt2 = (1-fraction) * south + (fraction) * southeast;
      elseif (orien <= pi)
          fraction = (orien-(3/4)*pi)/((1/4)*pi);
          pnt1 = (1-fraction) * northwest + (fraction) * west;
          pnt2 = (1-fraction) * southeast + (fraction) * east;
      else
        orien
      end
      result(I,J) = ( (matrixB(I,J) >= pnt1) & (matrixB(I,J) >= pnt2) ) * matrixB(I,J);
    end
    %waitbar(I/h); % update the progressbar
  end
  %close(hb);
end

% removing the borders
result = result(2:h+1, 2:w+1);

function bw = hysthresh(im, T1, T2)
% HYSTHRESH - Hysteresis thresholding
%
% Usage: bw = hysthresh(im, T1, T2)
%
% Arguments:
%             im  - image to be thresholded (assumed to be non-negative)
%             T1  - upper threshold value
%             T2  - lower threshold value
%
% Returns:
%             bw  - the thresholded image (containing values 0 or 1)
%
% Function performs hysteresis thresholding of an image.
% All pixels with values above threshold T1 are marked as edges
% All pixels that are adjacent to points that have been marked as edges
% and with values above threshold T2 are also marked as edges. Eight
% connectivity is used.
%
% It is assumed that the input image is non-negative
%
% Author: Peter Kovesi
% School of Computer Science & Software Engineering
% The University of Western Australia
% pk @ csse uwa edu au   http://www.csse.uwa.edu.au/~pk
%
% December 1996  - Original version
% March    2001  - Speed improvements made (~4x)

%
% A stack (implemented as an array) is used to keep track of all the
% indices of pixels that need to be checked.
% Note: For speed the number of conditional tests have been minimised
% This results in the top and bottom edges of the image being considered to
% be connected.  This may cause some stray edges to be propagated further than
% they should be from the top or bottom.
%

if (T2 > T1 | T2 < 0 | T1 < 0)  % Check thesholds are sensible
  error('T1 must be >= T2 and both must be >= 0 ');
end

[rows, cols] = size(im);    % Precompute some values for speed and convenience.
rc = rows*cols;
rcmr = rc - rows;
rp1 = rows+1;

bw = im(:);                 % Make image into a column vector
pix = find(bw > T1);        % Find indices of all pixels with value > T1
npix = size(pix,1);         % Find the number of pixels with value > T1

stack = zeros(rows*cols,1); % Create a stack array (that should never
                            % overflow!)

stack(1:npix) = pix;        % Put all the edge points on the stack
stp = npix;                 % set stack pointer
for k = 1:npix
    bw(pix(k)) = -1;        % mark points as edges
end

% Precompute an array, O, of index offset values that correspond to the eight
% surrounding pixels of any point. Note that the image was transformed into
% a column vector, so if we reshape the image back to a square the indices
% surrounding a pixel with index, n, will be:
%              n-rows-1   n-1   n+rows-1
%
%               n-rows     n     n+rows
%
%              n-rows+1   n+1   n+rows+1

O = [-1, 1, -rows-1, -rows, -rows+1, rows-1, rows, rows+1];

while stp ~= 0            % While the stack is not empty
    v = stack(stp);         % Pop next index off the stack
    stp = stp - 1;

    if v > rp1 & v < rcmr   % Prevent us from generating illegal indices
			    % Now look at surrounding pixels to see if they
                            % should be pushed onto the stack to be
                            % processed as well.
       index = O+v;	    % Calculate indices of points around this pixel.
       for l = 1:8
	   ind = index(l);
	   if bw(ind) > T2   % if value > T2,
	       stp = stp+1;  % push index onto the stack.
	       stack(stp) = ind;
	       bw(ind) = -1; % mark this as an edge point
	   end
       end
    end
end

bw = (bw == -1);            % Finally zero out anything that was not an edge
bw = reshape(bw,rows,cols); % and reshape the image


%% Phase Calculation
% Generates the phase signal for Kalman Filtering
% Inputs
% peaks             QRS peak locations
% length            Length of data for phase generation
%
%
% Fetal Extraction Toolbox, version 1.0, February 2014
% Released under the GNU General Public License
%
% Copyright (C) 2014  Fernando Andreotti
% Dresden University of Technology, Institute of Biomedical Engineering
% fernando.andreotti@mailbox.tu-dresden.de
%
% Last updated : 24-07-2014
%
% Based on: Synthetic ECG model error
% Open Source ECG Toolbox, version 2.0, March 2008
% Released under the GNU General Public License
% Copyright (C) 2008  Reza Sameni
% Sharif University of Technology, Tehran, Iran -- LIS-INPG, Grenoble, France
% reza.sameni@gmail.com

% This program is free software; you can redistribute it and/or modify it
% under the terms of the GNU General Public License as published by the
% Free Software Foundation; either version 2 of the License, or (at your
% option) any later version.
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
% Public License for more details.

% This program is free software; you can redistribute it and/or modify it
% under the terms of the GNU General Public License as published by the
% Free Software Foundation; either version 2 of the License, or (at your
% option) any later version.
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
% Public License for more details.
%
function phase = FECGx_kf_PhaseCalc(peaks,lengthx)
% Based on Sameni's method for phase calculation
phase = zeros(1,lengthx);
m = diff(peaks);            % gets distance between peaks
% first interval
% dealing with borders (first and last peaks may not be full waves)
% uses second interval as reference
L = peaks(1);   %length of first interval
if isempty(m) % only ONE peak was detected
    phase(1:lengthx) = linspace(-2*pi,2*pi,lengthx);
else
    phase(1:L) = linspace(2*pi-L*2*pi/m(1),2*pi,L);
    % beats in the middle
    for i = 1:length(peaks)-1;  % generate phases between 0 and 2pi for almos all peaks
        phase(peaks(i):peaks(i+1)) = linspace(0,2*pi,m(i)+1);
    end                                         % 2pi is overlapped by 0 on every loop
    % last interval
    % uses second last interval as reference
    L = length(phase)-peaks(end);   %length of last interval
    phase(peaks(end):end) = linspace(0,L*2*pi/m(end),L+1);
end
phase = mod(phase,2*pi);
phase(find(phase>pi)) = phase(find(phase>pi))- 2*pi;


function [] = test_elasticnet()

    clc;
    clear;
    close all;

    %% Set algorithms
    if 0
        algorithms = gd_solver_list('ALL');
    else
        algorithms = {'PG-BKT', 'PG-TFOCS-BKT', 'APG-BKT', 'CD-EasticNet', 'FISTA'};
    end

    %% prepare dataset
    if 1
        % generate synthtic data
        n = 500;
        d = 100;
        A = randn(d,n);
        b = randn(d,1);
        lambda1 = 5;
        lambda2 = 1;
    else
    end

    %% define problem definitions
    problem = elastic_net(A, b, lambda1, lambda2);

    %% initialize
    w_init = rand(n,1);
    w_list = cell(length(algorithms),1);
    info_list = cell(length(algorithms),1);

    %% perform algorithms
    for alg_idx=1:length(algorithms)
        fprintf('\n\n### [%02d] %s ###\n\n', alg_idx, algorithms{alg_idx});

        clear options;
        % general options for optimization algorithms
        options.w_init = w_init;
        options.tol_gnorm = 1e-10;
        options.max_iter = 300;
        options.verbose = true;
        options.f_opt = 0;
        options.store_w = false;

        switch algorithms{alg_idx}
            case {'PG-BKT'}

                options.step_alg = 'backtracking';
                options.step_init_alg = 'bb_init';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'PG-TFOCS-BKT'}

                options.step_alg = 'tfocs_backtracking';
                options.step_init_alg = 'bb_init';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'APG-BKT'}

                options.step_alg = 'backtracking';
                options.step_init_alg = 'bb_init';
                [w_list{alg_idx}, info_list{alg_idx}] = ag(problem, options);

            case {'APG-TFOCS-BKT'}

                options.step_alg = 'tfocs_backtracking';
                options.step_init_alg = 'bb_init';
                [w_list{alg_idx}, info_list{alg_idx}] = ag(problem, options);

            case {'FISTA'}

                options.sub_mode  = 'FISTA';
                [w_list{alg_idx}, info_list{alg_idx}] = ista(problem, options);

            case {'ADMM-LASSO'}

                options.rho = 0.1;
                [w_list{alg_idx}, info_list{alg_idx}] = admm_lasso(problem, options);

            case {'CD-EasticNet'}

                options.sub_mode = 'elasticnet';
                [w_list{alg_idx}, info_list{alg_idx}] = cd_lasso_elasticnet(problem, options);

            case {'L-BFGS-BKT'}

                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'L-BFGS-WOLFE'}

                options.step_alg = 'strong_wolfe';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'P-Newton-CHOLESKY'}

                options.sub_mode = 'CHOLESKY';
                options.step_init_alg = 'bb_init';
                options.step_alg = 'tfocs_backtracking';
                %options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = newton(problem, options);

            otherwise
                warn_str = [algorithms{alg_idx}, ' is not supported.'];
                warning(warn_str);
                w_list{alg_idx} = '';
                info_list{alg_idx} = '';
        end

    end

    fprintf('\n\n');

    %% plot all
    close all;

    % display iter vs cost/gnorm
    display_graph('iter','cost', algorithms, w_list, info_list);
    % display iter vs. l1 norm, i.e. the toral number of non-zero elements
    display_graph('iter','reg', algorithms, w_list, info_list);

end


function L = computeL(xK,K)
% COMPUTEL computes L-function from ripleys K function (L=sqrt(K/pi)-xK)
% L = computeL(xK,K)
% xK - position where K function was estimated
% K - K-function
L=bsxfun(@minus,sqrt(K/pi),xK);


%  Figure 10.78      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
% fig10_78.m is a script for the LQR(LQG) design for the disk
% drive case study

clf;
wm= 5*pi;
z=.05;
GM = 4;
a=0.1;
numG= [1/(50*pi) 1];
denG=[1/(25*pi^2) 1/(50*pi) 1  0 0];
sysG=tf(numG,denG);
% form the system with y + a*ydot output
numGv=[0 numG]+.09*[numG 0];
sysGv=tf(numGv,denG);
% convert the design system to state form
[f,g,h,j]=ssdata(sysGv);
sysGvs=ss(f,g,h,j);
K=lqry(sysGvs,10000,1)
% form the state feedback
fc=f-g*K;
sysR=ss(fc,g,h,j);
[num,den]=tfdata(sysR,'v');
% remove the zero used to weight ydot
sysCLR=tf(numG,den);
[ac,b,c,d]=ssdata(sysCLR);
% normalize the gain for unity dc
k=c*inv(-ac)*b
b1=b/k;
sysCLR=ss(ac,b1,c,d);
t=0:.01:1.5;
[y,t]=step(sysCLR,t);
plot(t,y);
xlabel('Time (msec)');
ylabel('Amplitude');
grid;
title('Fig. 10.61 Step response of the LQR design for the disk');
%pause;
pc=eig(sysCLR);
pe=10*pc;
[f,g,h,j]=ssdata(sysG);
L=place(f',h',pe)';
nbar=1
a=[f-g*K-L*h];
b=[L -L];
c=-K;
d=[0 0];
sysF=ss(a,b,c,d);
sysOL=sysF*sysG;
sysH=tf(1,1);
sysCL=feedback(sysOL,sysH,2,1);
%
%grid
nicegrid


function S = matsignt(T)
%MATSIGNT    Matrix sign function of a triangular matrix.
%            S = MATSIGN(T) computes the matrix sign function S of the
%            upper triangular matrix T using a recurrence.

%            Called by SIGNM.

if ~isequal(T,triu(T)), error('Matrix must be upper triangular.'), end

n = length(T);

S = diag( sign( diag(real(T)) ) );
for p = 1:n-1
   for i = 1:n-p

      j = i+p;
      d = T(j,j) - T(i,i);

      if S(i,i) ~= -S(j,j)  % Solve via S^2 = I if we can.

         % Get S(i,j) from S^2 = I.
         k = i+1:j-1;
         S(i,j) = -S(i,k)*S(k,j) / (S(i,i)+S(j,j));

      else

         % Get S(i,j) from S*T = T*S.
         s = T(i,j)*(S(j,j)-S(i,i));
         if p > 1
            k = i+1:j-1;
            s = s + T(i,k)*S(k,j) - S(i,k)*T(k,j);
         end
         S(i,j) = s/d;

      end

   end
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%

% problem 6 - convolution of x(t) and h(t)

t1=0:.1:2;
t2=2.1:.1:4;
t3=4.1:.1:10;
x1=t1;
x2=4-t2;
x3=zeros(size(t3));
x=[x1 x2 x3];
t=0:.1:10;
h=t.*exp(-t);
y=conv(x,h)*0.1;
plot(0:.1:20,y);
title('System response y(t)')