function err = getHdiverrorBDM1(node,elem,divSigma,sigmah,markedElem)
%% GETHDIVERRORBDM1 Hdiv norm of the approximation error for BDM1.
%
% The input divSigma is a function bundle, and sigma_h is a vector array
% whose component including two parts, the first part (i.e., sigmah(1:NE))
% is the line integral of flux in norm direction, i.e.
% sigmah_i = \int_{e_i} sigma\cdot n.
% the second part (i.e., sigmah(NE+1:2*NE)) is the dual basis written as
% sigmah_i = 3\int_{e_i} (\lambda1-\lambda2) sigma\cdot n  (ei = < 1 , 2 >)
%
% err = getHdiverrorBDM1(node,elem,divSigma,sigmah)
%
% Example
%
%     maxIt = 5;
%     node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%     elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%     bdEdge = setboundary(node,elem,'Dirichlet');
%     pde = mixBCdata;
%     err = zeros(maxIt,1); N = zeros(maxIt,1);
%     for i =1:maxIt
%         [node,elem,bdEdge] = uniformrefine(node,elem,bdEdge);
%         [u,sigma,NULL] = PoissonBDM1(node,elem,pde,bdEdge);
%         err(i) = getHdiverrorBDM1(node,elem,pde.f,-sigma,[]);
%         N(i) = size(u,1);
%     end
%     r1 = showrate(N,err,2);
%     legend('||\sigma - \sigma_h||_{H(div)}',['N^{' num2str(r1) '}'],...
%            'LOCATION','Best');
%
% See also getHdiverror3RT0, getL2errorRT0.
%
% Created by Ming Wang at Jan 17, 2011, M-lint modified at May 15, 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Construct Data Structure
[elem2dof,~,elem2edgeSign] = dofedge(elem);
NT = size(elem,1);
% compute div phi
[Dlambda,area] = gradbasis(node,elem);
% div phi = 2*(Dlambda_i,Rot_j);
rotMat = [0 -1; 1 0]; % rotation matrix for computing rotLambda.
divPhi(:,3) = 2*dot(Dlambda(:,:,1),Dlambda(:,:,2)*rotMat,2);
divPhi(:,1) = 2*dot(Dlambda(:,:,2),Dlambda(:,:,3)*rotMat,2);
divPhi(:,2) = 2*dot(Dlambda(:,:,3),Dlambda(:,:,1)*rotMat,2);
divSigmahp = zeros(NT,1);
for k = 1:3
    divSigmahp = divSigmahp + elem2edgeSign(:,k).*sigmah(elem2dof(:,k)).*divPhi(:,k);
end

%% compute Hdiv error element-wise
[lambda,w] = quadpts(2);
nQuad = size(lambda,1);
err = zeros(NT,1);
for p = 1:nQuad
    % quadrature points in the x-y-z coordinate
    pxy = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:);
    divSigmap = divSigma(pxy);
    % compute divSigmahp at quadrature points
    err = err + w(p)*sum((divSigmap - divSigmahp).^2,2);
end
err = err.*area;
% modify the error
err(isnan(err)) = 0; % remove the singular part
if (nargin == 5) && ~isempty(markedElem)
    err = err(markedElem); % error on some marked region
end
err = sqrt(sum(err));


%DTRANSFORM Distance transform
%
% DT = DTRANSFORM(IM, OPTIONS) is the distance transform of the
% binary image IM. The value of each output pixel is the distance (pixels)
% to the closest set pixel.
%
% Options::
% 'Euclidean'   use Euclidean distance (default)
% 'cityblock'   use cityblock (Manhattan) distance
% 'show',T      display the evolving distance transform, with a delay of T
%               seconds between frames
%
% See also IMORPH, DISTANCEXFORM, DXform.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB 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.
%
% MVTB 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 MVTB.  If not, see <http://www.gnu.org/licenses/>.

function d = dtransform(world, varargin)

    opt.metric = {'Euclidean', 'cityblock'};
    opt.show = [];

    [opt,args] = tb_optparse(opt, varargin);
    if length(args) > 0 && isnumeric(args{1})
        opt.show = args{1};
    end

    if strcmpi(opt.metric, 'cityblock')
        m = ones(3,3);
        m(2,2) = 0;
    elseif strcmpi(opt.metric, 'Euclidean')
        r2 = sqrt(2);
        m = [r2 1 r2; 1 0 1; r2 1 r2];
    end

    world(world==0) = Inf;
    world(world==1) = 0;

    count = 0;
    while 1
        world = imorph(world, m, 'plusmin');
        count = count+1;
        if opt.show
            cmap = gray(256);
            cmap = [1 0 0; cmap];
            colormap(cmap)
            image(world+1, 'CDataMapping', 'direct');
            set(gca, 'Ydir', 'normal');
            xlabel('x');
            ylabel('y');
            pause(opt.show);
        end

        if length(find(world(:)==Inf)) == 0
            break;
        end
    end

    if opt.show
        fprintf('%d iterations\n', count);
    end

    d = world;


function m = hlp_minor(X, row, col)
%
% calculate the ijth minor of matrix X by returning the determinant of X
% after removing the ith row and jth column
%
% Input:
%
%   X:      2- or 3-dimensional matrix
%   row,col to remove
%
% output:
%
%   m:      if X is 2-D, m = minor(X)
%           if X is 3-D, m(i) = minor(X(:,:,i))
%
%
% References:
%
% [1] Mullen T (2010) The Source Information Flow Toolbox (SIFT):
%   Theoretical Handbook and User Manual.
%   Available at: http://www.sccn.ucsd.edu/wiki/Sift/
%
%
% Author: Tim Mullen Dec 1st, 2010, SCCN/INC, UCSD.
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

nd = ndims(X);
if nd < 2 || nd > 3
    error('X must have 2 or 3 dimensions');
end

X(row,:,:) = [];
X(:,col,:) = [];

if nd==2
    m = det(X);
else
    m=zeros(size(X,3),1);
    for k=1:size(X,3)
        m(k) = det(X(:,:,k));
    end
end


function y = sigmoid(y, fac)
% SIGMOID nonlinear funcion for cochlear model
%	y = sigmoid(y, fac);
%	fac: non-linear factor
%	 -- fac > 0, transister-like function
%	 -- fac = 0, hard-limiter
%	 -- fac = -1, half-wave rectifier
%	 -- else, no operation, i.e., linear
%
%	SIGMOID is a monotonic increasing function which simulates
%	hair cell nonlinearity.
%	See also: WAV2AUD, AUD2WAV

% Auther: Powen Ru (powen@isr.umd.edu), NSL, UMD
% v1.00: 01-Jun-97

if fac > 0,
	%y = exp(y/fac); y = 1/(1+.1)-1./(1+.1*y);
	y = exp(-y/fac); y = 1./(1+y);
elseif fac == 0,
	y = (y > 0);	% hard-limiter
elseif fac == -1,
	y = max(y, 0);	% half-wave rectifier
elseif fac == -3,
	y = halfregu(y);
end;


%  INTERNAL FUNCTION: Prediction step for Kalman filter
%
%  ::
%
%    [a,P,Tt,Rt,Record]=prediction_step(T,R,att,Ptt)
%    [a,P,Tt,Rt,Record]=prediction_step(T,R,att,Ptt,MUt,OMGt,DPHI,DT,Record,ExpandedFlag)
%
%  Args:
%
%     - **T** [matrix] : m x m state matrix (autoregressive part)
%     - **R** [matrix] : m x n state matrix (shock impacts)
%     - **att** [vector] : m x 1 state update
%     - **Ptt** [matrix] : m x m covariance matrix of state update
%     - **MUt** [matrix] : k x l matrix with forward information to match
%     - **OMGt** [matrix|{[]}] : kl x kl covariance matrix of forward
%       information
%     - **DPHI** [matrix] : Matrix representing the restrictions on future
%       shocks
%     - **DT** [matrix] : Convoluted impact of initial conditions
%     - **Record** [] : Holder of invariant information
%     - **ExpandedFlag** [true|false] : if true, returns the expanded state
%       vector including the future shocks. If false, returns only the
%       endogenous variables
%
%  Returns:
%     :
%
%     - **a** [vector] : m x 1 or mm x 1 vector of predictions
%     - **P** [matrix] : m x m or mm x mm covariance of predictions
%     - **Tt** [matrix] : m x m or mm x mm time-varying matrix, modifying the
%       impact of lagged endogenous (T)
%     - **Rt** [] : m x n or mm x nn matrix of impact of shocks
%     - **Record** [] : Holder of invariant information
%
%


function [ basis, filt_sig, param ] = gsp_eigenspace_estimation( G, k, param )
%GSP_EIGENSPACE_ESTIMATION Estimation of first eigenvectors of any graph Laplacian
%   Usage:  basis = gsp_eigenspace_estimation(G,k);
%           [basis, approx_U, param] = gsp_eigenspace_estimation(G,k,param);
%
%   Input parameters :
%         G          : Graph structure.
%         k          : Dimension of the subspace.
%         param      : Optional parameters
%   Output parameters:
%         basis      : Approximated basis of k first eigenvectors
%         filt_sig   : Filtered random signal
%         param      : Optional parameters (with new entries)
%
%   'gsp_eigenspace_estimation(G,k)' computes an estimation of the first
%   $k$ eigenvectors of the Laplacian of G using Gaussian random signal
%   filtering, following the FEARS method described in paratte2017fast.
%
%
%   Example:::
%
%         G = gsp_sensor(256);
%         G = gsp_estimate_lmax(G);
%         k = 8;
%         param.order = 100;
%         Uk_est = gsp_eigenspace_estimation(G, k, param);
%         G = gsp_compute_fourier_basis(G);
%         proj_energy = norm(Uk_est' * G.U(:, 1:k), 'fro');
%
%
%   Additional parameters
%   ---------------------
%
%   * *param.filter*  : Select the filter to be used for the computation.
%     * 'lp-ch'   : Chebyshev polynomial approximation
%     * 'lp-jch'  : Jackson-Chebyshev polynomial approximation
%     * 'expwin'  : Exponentially decreasing polynomial approximation Default: 'lp-jch'
%   * *param.order* : Degree of the polynomial approximation (default=50).
%   * *param.lk_est_method* : Select the version of lk estimation.
%     * 'fast'      : Accelerated method using local uniformity assumption
%     * 'std'  : Usual method using dichotomy all the time Default: 'fast'
%   * *param.R* : Random matrix to use (of size N > d, d >= k)
%     (default: Gaussian(0, 1/k) of size Nxk)
%   * *param.pcoef* : Polynomial coefficients if already known.
%   * *param.lk* : Estimated value of lambda_k if already known.
%   * *param.verbose* : Verbosity level (0 no log - 1 display warnings) (default 1).
%
%
%   References: paratte2016fast
%

% Author: Johan Paratte, Lionel Martin
% Date: 3 November 2016

if nargin < 3, param = struct; end
if ~isfield(param, 'filter'), param.filter = 'lp-jch'; end
if ~isfield(param, 'order'), param.order = 50; end
if ~isfield(param, 'lk_est_method'), param.lk_est_method = 'fast'; end
if ~isfield(param, 'R'), param.R = randn(G.N, k)/sqrt(k); end
if ~isfield(param, 'verbose'), param.verbose = 1; end

assert(size(param.R, 1) == G.N && size(param.R, 2) >= k, 'The optional parameter R has wrong size.');

if ~isfield(param, 'pcoefs')
    if param.verbose, disp('Polynomial filtering required. Computing polynomial coefficients...'); end

    if ~isfield(G, 'lmax')
        G = gsp_estimate_lmax(G);
        warning(['GSP_EIGENSPACE_ESTIMATION: The variable lmax is not available.', ...
            'The function will compute it for you. However, if you apply ', ...
            'many time this function, you should precompute it using the ', ...
            'function: gsp_estimate_lmax.']);
    end

    if ~isfield(param, 'lk')
        if param.verbose, fprintf('Estimation of lambda_k');
        end

        tic;
        switch param.lk_est_method
            case 'fast'
                [param.lk, info] = gsp_fast_estimate_lk(G, k, param);
                if param.verbose, disp('using our accelerated method.'); end

            case 'std'
                [~, param.lk, ~, ~, nb_iter_lk, k_est_lk] = gsp_estimate_lk(G, k, param);
                if param.verbose, disp('using the standard method.'); end

            otherwise
                error('Unknown method for lk_est_method.');
        end
        t = toc;

        if param.verbose
            fprintf(['* Estimated lk: %d\n', ...
            '* Time to estimate lk: %f sec\n', ...
            '* in %d iterations with k_est=%d (target=%d)\n'], ...
            param.lk, t, mean(info.calls), mean(info.k_est), k);
        end
    else
        warning('lambda_k was provided to the method from param.');
    end

    tic;
    switch param.filter
        case 'lp-ch'
            [param.pcoefs, ~] = jackson_cheby_poly_coefficients(0, param.lk, [0, G.lmax], param.order);

        case 'lp-jch'
            [~, param.pcoefs] = jackson_cheby_poly_coefficients(0, param.lk, [0, G.lmax], param.order);

        case 'expwin'
            ew = gsp_design_expwin(G, param.lk/G.lmax);
            param.pcoefs = gsp_cheby_coeff(G, ew, param.order);

        otherwise
            error('Unknown filter type!');
    end

    t = toc;
    if param.verbose, fprintf('* Time to compute polynomial coefficients: %f sec.\n', t); end

else
    if param.verbose, warning('pcoef was provided to the method from param.'); end
end

if param.verbose, disp('Filtering random signals...'); end
tic;
filt_sig = gsp_cheby_op(G, param.pcoefs, param.R);
t = toc;
if param.verbose, fprintf('* Time to filter random signals: %f sec.\n', t); end

if param.verbose, disp('Computing the SVD for eigenspace recovery...'); end
tic;
[basis, ~, ~] = svd(filt_sig, 'econ');
t = toc;
if param.verbose, fprintf('* Time to compute SVD: %f sec.\n', t); end

end


function [G, cliques, fill_ins] = triangulate(G, order)
% TRIANGULATE Ensure G is triangulated (chordal), i.e., every cycle of length > 3 has a chord.
% [G, cliques, fill_ins, cliques_containing_node] = triangulate(G, order)
%
% cliques{i} is the i'th maximal complete subgraph of the triangulated graph.
% fill_ins(i,j) = 1 iff we add a fill-in arc between i and j.
%
% To find the maximal cliques, we save each induced cluster (created by adding connecting
% neighbors) that is not a subset of any previously saved cluster. (A cluster is a complete,
% but not necessarily maximal, set of nodes.)

MG = G;
n = length(G);
eliminated = zeros(1,n);
cliques = {};
for i=1:n
  u = order(i);
  U = find(~eliminated); % uneliminated
  nodes = myintersect(neighbors(G,u), U); % look up neighbors in the partially filled-in graph
  nodes = myunion(nodes, u); % the clique will always contain at least u
  G(nodes,nodes) = 1; % make them all connected to each other
  G = setdiag(G,0);
  eliminated(u) = 1;

  exclude = 0;
  for c=1:length(cliques)
    if mysubset(nodes,cliques{c}) % not maximal
      exclude = 1;
      break;
    end
  end
  if ~exclude
    cnum = length(cliques)+1;
    cliques{cnum} = nodes;
  end
end

fill_ins = sparse(triu(max(0, G - MG), 1));

%assert(check_triangulated(G)); % takes 72% of the time!


function r = bisect_characteristic ( x0, theta, characteristic )

%*****************************************************************************80
%
%% BISECT_CHARACTERISTIC: characteristic function transition surface.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X0(M,1), the coordinates of the base point inside the surface.
%
%    Input, real THETA, the coordinates of a direction from the base point.
%
%    Input, real y = CHARACTERISTIC ( m, n, x ), the handle for a function which
%    evaluates the characteristic for the object at N M-dimensional points X,
%    returning a 1 for points inside the object, and 0 for points outside.
%
%    Output, real R, the distance from X0 to the surface in direction THETA.
%

  x0 = x0(:);

  m = length ( x0 );
  n = 1;
%
%  Initially, X1 = X0, and so Y1 should be 1.
%
  x1(1:m,1) = x0(1:m,1);
  y1 = characteristic ( m, n, x1 );
%
%  Seek an exterior point.
%
  x2 = exterior_point_characteristic ( m, x0, theta, characteristic );
%
%  Carry out the bisection search in the interval [X1,X2].
%
  it_num = 0;

  while ( 1.0E-3 < sqrt ( norm ( x2 - x1 ) ) )

    x3 = ( x1 + x2 ) / 2.0;
    y3 = characteristic ( m, n, x3 );

    if ( y3 == 0 )
      x2 = x3;
    else
      x1 = x3;
    end

    it_num = it_num + 1;

    if ( 1000 < it_num )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'BISECT_CHARACTERISTIC - Fatal error!\n' );
      fprintf ( 1, '  Too many iterations.\n' );
      error ( 'BISECT_CHARACTERISTIC - Fatal error!' )
    end

  end
%
%  Measure the distance from X0 to the transition.
%  We estimate the transition to occur at the average of X1 and X2.
%
  x3 = ( x1 + x2 ) / 2.0;
  r = norm ( x3 - x0 );

  return
end


% funSimLogNormProbCov returns SINR-based k-coverage probability
% under log-normal shadowing based on repeated simulations of
% of model outlined in [1]
%
% simPCovk=funSimLogNormProbCov(tValues,betaConst,K,lambda,sigma,W,diskRadius,simNumb,k)
% simPCovk is the k-coverage probability
% tValues are the SINR threshold values. tValues can be a vector
% betaConst is the pathloss exponent.
% K = path-loss constant
% lambda = density of base station/nodes of cellular network
% sigma = standard deviation of log-normal shadowing (not in dB)
% W = noise constant
% diskRadius = radius of simulation region (Warning: if too small, edge
% effects will not be sufficiently included and disagreement with analytic
% results may occurr)
% simNumb = number of simulations
% k = coverage number (often set to one)
% All input values (except tValues) are scalars
%
% Author: H.P. Keeler, Inria Paris/ENS, 2013
%
% References
% [1] H.P. Keeler, B. Błaszczyszyn and M. Karray,
% 'SINR-based k-coverage probability in cellular networks with arbitrary
% shadowing', accepted at ISIT, 2013

function simPCovk=funSimLogNormProbCov(tValues,betaConst,K,lambda,sigma,W,diskRadius,simNumb,k)

if nargin==8
    k=1;
end

tNumb=length(tValues);

%%% Simulation Section %%%
%(uniformly) randomly places nodes on a disk of radius diskRadius
diskArea=pi*diskRadius^2;
coveredNumbk=zeros(size(tValues));
ESTwoBeta=exp(sigma^2*(2-betaConst)/betaConst^2);
%rescale lambda - see foot note 5 in [1]
lambdaSim=lambda*ESTwoBeta;
for i=1:simNumb
    randNumb=poissrnd(lambdaSim*diskArea);
    %shadowing distribution can be constant if lambda is rescaled - see [1]
    shadowRand=ones(randNumb,1);
    %random distances from the typical node
    rRand=diskRadius*sqrt(rand(randNumb,1)); %uniform in cartesion, not polar coordinates

    signalRand=shadowRand.*(K*rRand).^(-betaConst);
    interferTotal=sum(signalRand); %total inteference in network
    SINR=signalRand./((interferTotal-signalRand)+W); %calculate SINR for each node in the network

    for j=1:tNumb
        T=tValues(j);
        %counts how many nodes are exactly k or more connected/covered
        if sum(SINR>=T)>=k
            coveredNumbk(j)=coveredNumbk(j)+1;
        end
    end

end

simPCovk=coveredNumbk/simNumb;


function H = higuchi(sequence,isplot)
%
% 'higuchi' estimate the hurst parameter of a given sequence with
%     higuchi's method.
%
% Inputs:
%     sequence: the input sequence for estimate
%     isplot: whether display the plot. without a plot if isplot equal to 0
% Outputs:
%     H: the estimated hurst coeffeient of the input sequence

%  Author: Chu Chen
%  Version 1.0,  03/10/2008
%  chen-chu@163.com
%

if nargin == 1
    isplot = 0;
end

sequence = cumsum(sequence);
N = length(sequence);
mlarge = floor(N/5);
M = [floor(logspace(0,log10(mlarge),50))];
M = unique(M(M>1));
n = length(M);
cut_min = ceil(n/10);
cut_max = floor(6*n/10);

curve_length = zeros(1,n);
for h = 1:n
    m = M(h);
    k = floor((N-m)/m);
    temp_length = zeros(m,k);

    for i = 1:m
        for j = 1:k
            temp_length(i,j) = abs(sequence(i+j*m)-sequence(i+(j-1)*m));
        end
    end

    curve_length(h) = sum(mean(temp_length,2)) * ((N-1)/m^3);
end

x = log(M);
y = log(curve_length);
X = x(cut_min:cut_max);
Y = y(cut_min:cut_max);
p1 = polyfit(X,Y,1);
Yfit = polyval(p1,X);
yfit = polyval(p1,x);
H = 2 + (Yfit(end)-Yfit(1))/(X(end)-X(1));

if isplot ~= 0
    figure,hold on;
    plot(x,y,'b*');
    plot(X,Yfit,'r-','LineWidth',2);
    plot(x(1:cut_min),yfit(1:cut_min),'r:','LineWidth',2);
    plot(x(cut_max:end),yfit(cut_max:end),'r:','LineWidth',2);
    xlabel('Log10(Aggregate Level)'),ylabel('Log10(Curve Legnth)'),title('Higuchi Method');
end


function [g,a]=ar1nv(x)
% AR1NV - Estimate the parameters for an AR(1) model
% Syntax: [g,a]=ar1nv(x);
%
% Input: x - a time series.
%
% Output: g - estimate of the lag-one autocorrelation.
%         a - estimate of the noise variance.

% (c) Eric Breitenberger

x=x(:);
N=length(x);
m=mean(x);
x=x-m;

% Lag zero and one covariance estimates:
c0=x'*x/N;
c1=x(1:N-1)'*x(2:N)/(N-1);

g=c1/c0;
a=sqrt((1-g^2)*c0);


function [prices, alphas] = Heston1993KahlJaeckelLordRev3(PC, S,K,T,t,r,q,v0,theta,rho,kappa,sigma, alphas)
% Heston pricing function based on the implementation suggested by
% Roger Lord and Chrisitan Kahl in "Optimal Fourier inversion in
% semi-analytical option pricing"
%
%
% Input: (PC till q can be vectorized)
%       PC: 1 for Calls, 2 for Puts
%       S: Spot
%       K: Strike
%       T: Maturity
%       t: start date
%       r: interest rate
%       q: dividend
%       v0: initial variance
%       theta: long run mean variance
%       kappa: mean reversion speed of  volatility
%       sigma: volatility of volatility
%       rho: correlation between returns volatility
%       alpha: alpha can be a vector supplied by the user, otherwise the
%       function attempts to find a payoff-dependent optimal alpha
%
%   Output: Price for each option, optionally generated alphas
%
%   Usage: Heston1993KahlJaeckelLordRev3(1, 100, 100, 20,0, 0.05, 0.0,
%           0.00003, 0.00003,-0.3, 0.5, 0.0008)
%
%   Author: Jonathan Frei, 2015
%

    % force column vector
    PC=PC(:);
    S=S(:);
    K=K(:);
    T=T(:);
    t=t(:);
    r=r(:);
    q=q(:);

    nos = numel(S);
    prices=NaN(nos,1);
    tau=T-t;
    mu=(r-q);
    F = S.*exp(mu.*tau);

    if(~exist('alphas','var'))
        alphas = NaN(numel(S),1);
    elseif(numel(alphas)==1)
        alphas = repmat(alphas,numel(S),1);
    end

    alpha0=0.75;

    for(ind=1:nos)
        if(isnan(alphas(ind)))
            try
                % using fzero here instead of fminsearch
               alphas(ind) = fzero( @(a) psi(a,K(ind), F(ind), kappa, theta, rho, sigma, tau(ind), v0), alpha0);
            catch
               alphas(ind) = alpha0;
            end
        end
        prices(ind) =  Ralpha(F(ind), K(ind), alphas(ind))+1/pi*integral(@(x) phi(x, K(ind), alphas(ind), F(ind), kappa, theta, rho, sigma, tau(ind), v0) , 0, Inf);
        if (PC(ind)==2)
            prices(ind) = prices(ind) + K(ind)*exp(-r(ind)*tau(ind))-S(ind)*exp(-q(ind)*tau(ind));
        end
    end

end

function p = psi(alpha, K, F, kappa, theta, rho, sigma, tau, v0)
    k = log(K);
    p = -alpha*k+0.5*log(phi(-(alpha+1)*1i, K, alpha, F, kappa, theta, rho, sigma, tau, v0)^2);
end

function r = Ralpha(F, K, alpha)
    r = F*(alpha<=0)-K*(alpha<=-1)-0.5*(F*(alpha==0)-K*(alpha==-1));
end

function y = phi(v, K, alpha, F, kappa, theta, rho, sigma, tau, v0)
    k = log(K);
    y = real(exp(-1i*(v-1i*alpha)*k).*( cf(v-1i*(alpha+1), F, kappa, theta, rho, sigma, tau, v0)./(-(v-1i*(alpha+1)).*(v-1i*alpha))));
end

function c = cf(u, F, kappa, theta, rho, sigma, tau, v0)
    f = log(F);
    c = exp(1i*u*f+ A(u, kappa, theta, rho, sigma, tau)+Bv(u, rho, sigma, kappa, tau)*v0);
end

function b = Bv(u, rho, sigma, kappa, tau)
    b = ((beta(u,rho,sigma,kappa)-D(u, rho, sigma, kappa)).*(1-exp(-D(u, rho, sigma, kappa)*tau)))./(sigma.^2*(1-G(u, rho, sigma, kappa).*exp(-D(u, rho, sigma, kappa)*tau)));
end

function a = A(u, kappa, theta, rho, sigma, tau)
    a = (kappa*theta*((beta(u,rho,sigma,kappa)-D(u, rho, sigma, kappa))*tau-2*log(phi2(u, rho, sigma, kappa, tau))))/sigma.^2;
end

function p = phi2(u, rho, sigma, kappa, tau)
    p = (G(u, rho, sigma, kappa).*exp(-D(u, rho, sigma, kappa)*tau)-1)./(G(u, rho, sigma, kappa)-1);
end

function g = G(u, rho, sigma, kappa)
    g = (beta(u,rho,sigma,kappa)-D(u, rho, sigma, kappa))./(beta(u,rho,sigma,kappa)+D(u, rho, sigma, kappa));
end

function d = D(u, rho, sigma, kappa)
    d = sqrt(beta(u,rho,sigma,kappa).^2-4*alphahat(u)*gamma(sigma));
end

function a = alphahat(u)
    a = -0.5*u.*(1i+u);
end

function b = beta(u,rho,sigma,kappa)
    b = kappa-rho*sigma*u*1i;
end

function y = gamma(sigma)
    y = 0.5*sigma.^2;
end


%  Figure 10.67      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% fig10_67.m is a script to generate Fig. 10.67 the linear RTP response
% PI controller
% RTP chamber demo Example
clf;
a=[-0.068208813989939 0.014929776357245 0.000000065782442;...
   0.045809672136430 -0.118134773528570 0.021802006306129;...
   0.000000433637498 0.046839194867347 -0.100884399149872];
b3=[0.37873508304055 0.110575403544586 0.022912893467038;...
   0.000000002974222 0.449046982554739 0.073572900808161;...
   0.000000027324116 0.000702342292121 0.417797228783230];
c3=eye(3,3);
d3=0*eye(3,3);

% Combine 3 lamps into a single actuator
b=b3(:,1)+b3(:,2)+b3(:,3);
% Select center temperature
c=c3(2,:)
%
d=[0];
% PI controller
ac=[0];
bc=[1.0];
cc=[0.0527];
dc=[1];
sysG=ss(a,b,c,d);
sysD=ss(ac,bc,cc,dc);
sysL=series(sysD,sysG);
sysH=tf(1,1);
sysCL=feedback(sysL,sysH);

%CL Step Response
t=0:.1:100;
R=[0:.1:25, 25*ones(1,500), 0*ones(1,250)];
R11=[R'];
[yy,t]=lsim(sysCL,R11,t);
plot(t,yy,'--');
grid;
hold on;
plot(t,R11,'-');
xlabel('Time (sec)');
ylabel('Temperature (K)');
title('Fig. 10.67 (a) PI controller: temperature tracking response');
%legend('y')
pause
hold off
% Control effort
sysCLu=feedback(sysD,sysG);
[uuu,t]=lsim(sysCLu,R11,t);
plot(t(1:753,:),uuu(1:753,:));
hold on;
plot(t(752:818,:),0*ones(67,3));
hold on;
plot(t(753:818,:),uuu(753:818,:),'--');
hold on;
plot(t(819:1001,:),uuu(819:1001,:));
xlabel('Time (sec)');
ylabel('Lamp voltage');
title('Fig. 10.67 (b) PI controller :control effort');
legend('u')
grid;
hold off;


function value = r4_tan ( x )

%*****************************************************************************80
%
%% R4_TAN evaluates the tangent of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the tangent of X.
%
  persistent nterms
  persistent sqeps
  persistent tancs
  persistent xmax
  persistent xsml

  pi2rec = 0.0116197723675813430;

  if ( isempty ( nterms ) )
    tancs = [ ...
      0.226279327631293578, ...
      0.0430179131465489618, ...
      0.0006854461068256508, ...
      0.0000110453269475970, ...
      0.0000001781747790392, ...
      0.0000000028744968582, ...
      0.0000000000463748541, ...
      0.0000000000007481760, ...
      0.0000000000000120704, ...
      0.0000000000000001947, ...
      0.0000000000000000031 ]';
    nterms = r4_inits ( tancs, 11, 0.1 * r4_mach ( 3 ) );
    xmax = 1.0 / r4_mach ( 4 );
    xsml = sqrt ( 3.0 * r4_mach ( 3 ) );
    sqeps = sqrt ( r4_mach ( 4 ) );
  end

  y = abs ( x );

  if ( xmax < y )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_TAN - Warning!\n' );
    fprintf ( 1, '  No precision because |X| is big.\n' );
    value = 0.0;
    return
  end
%
%  Carefully compute y * (2/pi) = (aint(y) + rem(y)) * (.625 + pi2rec)
%  = aint(.625*y) + rem(.625*y) + y*pi2rec  =  aint(.625*y) + z
%  = aint(.625*y) + aint(z) + rem(z)
%
  ainty = floor ( y );
  yrem = y - ainty;
  prodbg = 0.625 * ainty;
  ainty = floor ( prodbg );
  y = ( prodbg - ainty ) + 0.625 * yrem + y * pi2rec;
  ainty2 = floor ( y );
  ainty = ainty + ainty2;
  y = y - ainty2;

  ifn = floor ( mod ( ainty, 2.0 ) );

  if ( ifn == 1 )
    y = 1.0 - y;
  end

  if ( 1.0 - y < abs ( x ) * sqeps )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_TAN - Warning!\n' );
    fprintf ( 1, '  Answer < half precision.\n' );
    fprintf ( 1, '  |X| big or X near pi/2 or 3*pi/2.\n' );
  end

  if ( y == 1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_TAN - Fatal error!\n' );
    fprintf ( 1, '  X is pi/2 or 3*pi/2.\n' );
    error ( 'R4_TAN - Fatal error!' )
  end

  if ( y <= 0.25 )

    value = y;
    if ( xsml < y )
      value = y * ( 1.5 ...
        + r4_csevl ( 32.0 * y * y - 1.0, tancs, nterms ) );
    end

  elseif ( y <= 0.5 )

    value = 0.5 * y * ( 1.5 ...
      + r4_csevl ( 8.0 * y * y - 1.0, tancs, nterms ) );

    value = 2.0 * value / ( 1.0 - value * value );

  else

    value = 0.25 * y * ( 1.5 ...
      + r4_csevl ( 2.0 * y * y - 1.0, tancs, nterms ) );
    value = 2.0 * value / ( 1.0 - value * value );
    value = 2.0 * value / ( 1.0 - value * value );

  end

  if ( x < 0.0 )
    value = - abs ( value );
  elseif ( 0.0 < x )
    value = + abs ( value );
  end

  if ( ifn == 1 )
    value = - value;
  end

  return
end


function b = r83t_mv ( m, n, a, x )

%*****************************************************************************80
%
%% R83T_MV multiplies an R83T matrix times an R8VEC.
%
%  Discussion:
%
%    The R83T storage format is used for a tridiagonal matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,3), the matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(M), the product A * x.
%
  b = zeros ( m, 1 );
  x = x(:);

  mn = min ( m, n );

  if ( n == 1 )
    b(1) = a(1,2) * x(1);
    if ( 1 < m )
      b(2) = a(2,1) * x(1);
    end
    return
  end

  b(1)      = a(1,2)       * x(1) ...
            + a(1,3)       * x(2);

  b(2:mn-1) = a(2:mn-1,1) .* x(1:mn-2) ...
            + a(2:mn-1,2) .* x(2:mn-1) ...
            + a(2:mn-1,3) .* x(3:mn);

  b(mn)     = a(mn,1)      * x(mn-1) ...
            + a(mn,2)      * x(mn);

  if ( n < m )
    b(mn+1) = b(mn+1) + a(mn+1,1) * x(mn);
  elseif ( m < n )
    b(mn) = b(mn) + a(mn,3) * x(mn+1);
  end

  return
end


% test of hufmann coding by concatening several token

% probability of having 0
t = .12;
n = 4096*2;
x = (rand(n,1)>t)+1;

% entropy lower bound
p = [t 1-t];
e =  -sum(p.*log2(p));

% create a new vector by lifting
q = 3;
n1 = ceil(n/q)*q;
x1 = x;
x1(end+1:n1) = 1;
x1 = reshape(x1,[q n1/q]);
[Y,X] = meshgrid(1:n1/q,0:q-1);
x1 = sum( (x1-1) .* (2.^X), 1 )' + 1;

% generate probability table
P = p(:); p = p(:);
for i=1:q-1
    Pold = P;
    P = [];
    for i=1:length(p)
        P = [P; Pold*p(i)];
    end
end

% compute the tree
T = compute_hufftree(P);
% do the coding
y = perform_huffcoding(x1,T,+1);
% average number of bits
e1 = length(y)/length(x);

disp(['Entropy=' num2str(e) ', Huffman(block size ' num2str(q) ')=' num2str(e1)]);


function f = tone2freq(T)
% MUSIC.TONE2FREQ converts a musical semitone to a frequency.
%    F = MUSIC.TONE2FREQ(T) converts the musical semitones in T to frequencies.
%
%    Example
%       f = music.tone2freq(0:2);  % returns [261.63  277.19  293.67]
%
%    See also music.tone2interval, music.tone2note, music.freq2tone.

%    Author: E. Johnson
%    Copyright 2010 The MathWorks, Inc.

fC4 = 261.625565300599;  % Middle C (C4) is 261.63 Hz

f = fC4 .* 2 .^ (T / 12);


function [W,H] = nmf_cjlin(V,Winit,Hinit,tol,timelimit,maxiter)
%
% Copyright (c) 2005-2006 Chih-Jen Lin
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions
% are met:
%
% 1. Redistributions of source code must retain the above copyright
% notice, this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright
% notice, this list of conditions and the following disclaimer in the
% documentation and/or other materials provided with the distribution.
%
% 3. Neither name of copyright holders nor the names of its contributors
% may be used to endorse or promote products derived from this software
% without specific prior written permission.
%
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
% ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
% LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
% A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
%
% NMF by alternative non-negative least squares using projected gradients
% Author: Chih-Jen Lin, National Taiwan University
%
% W,H: output solution
% Winit,Hinit: initial solution
% tol: tolerance for a relative stopping condition
% timelimit, maxiter: limit of time and iterations

W = Winit; H = Hinit; initt = cputime;

gradW = W*(H*H') - V*H'; gradH = (W'*W)*H - W'*V;
initgrad = norm([gradW; gradH'],'fro');
fprintf('Init gradient norm %f\n', initgrad);
tolW = max(0.001,tol)*initgrad; tolH = tolW;

for iter=1:maxiter,
  % stopping condition
  projnorm = norm([gradW(gradW<0 | W>0); gradH(gradH<0 | H>0)]);
  if projnorm < tol*initgrad | cputime-initt > timelimit,
    break;
  end

  [W,gradW,iterW] = nlssubprob(V',H',W',tolW,1000); W = W'; gradW = gradW';
  if iterW==1,
    tolW = 0.1 * tolW;
  end

  [H,gradH,iterH] = nlssubprob(V,W,H,tolH,1000);
  if iterH==1,
    tolH = 0.1 * tolH;
  end

  if (iterW==1 & iterH==1 & tolH + tolW < tol*initgrad),
    fprintf('Failed to move\n'); break;
  end
  if rem(iter,10)==0, fprintf('.'); end
end
fprintf('\nIter = %d Final proj-grad norm %f\n', iter, projnorm);

function [H,grad,iter] = nlssubprob(V,W,Hinit,tol,maxiter)
% H, grad: output solution and gradient
% iter: #iterations used
% V, W: constant matrices
% Hinit: initial solution
% tol: stopping tolerance
% maxiter: limit of iterations

H = Hinit;
WtV = W'*V;
WtW = W'*W;

alpha = 1; beta = 0.1;
for iter=1:maxiter,
  grad = WtW*H - WtV;
  projgrad = norm(grad(grad < 0 | H >0));
  if projgrad < tol,
    break
  end

  % search step size
  Hn = max(H - alpha*grad, 0); d = Hn-H;
  gradd=sum(sum(grad.*d)); dQd = sum(sum((WtW*d).*d));
  if gradd + 0.5*dQd > 0.01*gradd,
    % decrease alpha
    while 1,
      alpha = alpha*beta;
      Hn = max(H - alpha*grad, 0); d = Hn-H;
      gradd=sum(sum(grad.*d)); dQd = sum(sum((WtW*d).*d));
      if gradd + 0.5*dQd <= 0.01*gradd | alpha < 1e-20,
        H = Hn; break;
      end
    end
  else
    % increase alpha
    while 1,
      Hp = Hn;
      alpha = alpha/beta;
      Hn = max(H - alpha*grad, 0); d = Hn-H;
      gradd=sum(sum(grad.*d)); dQd = sum(sum((WtW*d).*d));
      if gradd + 0.5*dQd > 0.01*gradd | Hn == Hp | alpha > 1e10,
        H = Hp; alpha = alpha*beta; break;
      end
    end
  end
end

if iter==maxiter,
  fprintf('Max iter in nlssubprob\n');
end


function Z=SoftThreshold_GS( A, thres )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Soft-thresholding for group lasso
%
% Reference:
% Yuan, Ming, and Yi Lin.
% "Model selection and estimation in regression with grouped variables."
% Journal of the Royal Statistical Society: Series B
% (Statistical Methodology) 68.1 (2006): 49-67.
%
% Provider:
% Hongteng Xu @ Georgia Tech
% June 12, 2017
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Z = zeros(size(A));
for u = 1:size(A, 3)
    for v = 1:size(A, 1)
        tmp = 1 - thres/norm(A(v,:,u));
        if tmp>0
            Z(v,:,u) = tmp*A(v,:,u);
        end
    end
end


function [d pred] = shortest_paths(A,u,varargin)
% SHORTEST_PATHS Compute the weighted single source shortest path problem.
%
% [d pred] = shortest_paths(A,u) returns the distance (d) and the predecessor
% (pred) for each of the vertices along the shortest path from u to every
% other vertex in the graph.
%
% ... = shortest_paths(A,u,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%   options.algname: the algorithm to use
%       [{'auto'} | 'dijkstra' | 'bellman_ford' | 'dag']
%   options.inf: the value to use for unreachable vertices
%       [double > 0 | {Inf}]
%   options.target: a special vertex that will stop the search when hit
%       [{'none'} | any vertex number besides the u]; target is ignored if
%       visitor is set.
%   options.visitor: a structure with visitor callbacks.  This option only
%       applies to dijkstra or bellman_ford algorithms.  See dijkstra_sp or
%       bellman_ford_sp for details on the visitors.
%   options.edge_weight: a double array over the edges with an edge
%       weight for each edge, see EDGE_INDEX and EXAMPLES/REWEIGHTED_GRAPHS
%       for information on how to use this option correctly
%       [{'matrix'} | length(nnz(A)) double vector]
%
% Note: if you need to compute shortest paths with 0 weight edges, you must
% use an edge_weight vector, see the examples for details.
%
% Note: 'auto' cannot be used with 'nocheck' = 1.  The 'auto' algorithm
% checks if the graph has negative edges and uses bellman_ford in that
% case, otherwise, it uses 'dijkstra'.  In the future, it may check if the
% graph is a dag and use 'dag'.
%
% Example:
%    load graphs/clr-25-2.mat
%    shortest_paths(A,1)
%    shortest_paths(A,1,struct('algname','bellman_ford'))
%
% See also DIJKSTRA_SP, BELLMAN_FORD_SP, DAG_SP

% David Gleich
% Copyright, Stanford University, 2006-2008

%% History
%  2006-04-19: Initial coding
%  2007-04-18: Added edge_weight option.
%  2007-04-19: Added target option.
%    Added additional error checks.
%  2007-07-12: Fixed edge_weight documentation
%%

[trans check full2sparse] = get_matlab_bgl_options(varargin{:});
if full2sparse && ~issparse(A), A = sparse(A); end

options = struct('algname', 'auto', 'inf', Inf, 'edge_weight', 'matrix', ...
    'target', 'none');
options = merge_options(options,varargin{:});

% edge_weights is an indicator that is 1 if we are using edge_weights
% passed on the command line or 0 if we are using the matrix.
edge_weights = 0;
edge_weight_opt = 'matrix';

if strcmp(options.edge_weight, 'matrix')
    % do nothing if we are using the matrix weights
else
    edge_weights = 1;
    edge_weight_opt = options.edge_weight;
end

if strcmp(options.target,'none')
    target = 0; % a flag used to denote "no target" to the mex
elseif isa(options.target, 'double')
    target = options.target;
else
    error('matlab_bgl:invalidParameter', ...
        'options.target is not ''none'' or a vertex number.');
end

if check
    % check the values of the matrix
    check_matlab_bgl(A,struct('values',edge_weights ~= 1));

    if edge_weights && nnz(A) ~= length(edge_weight_opt)
        error('matlab_bgl:invalidParameter', 'the vector of edge weights must have length nnz(A)');
    end

    % set the algname
    if (strcmpi(options.algname, 'auto'))
        if edge_weights
            mv = min(edge_weights);
        else
            mv = min(min(A));
        end

        if (mv < 0)
            options.algname = 'bellman_ford';
        else
            options.algname = 'dijkstra';
        end
    else
        % check the data provided to match the algorithm
        if strcmpi(options.algname, 'dijkstra')
            if edge_weights
                mv = min(edge_weight_opt);
            else
                mv = min(min(A));
            end
            if mv < 0
                error('matlab_bgl:invalidParameter', ...
                    'dijkstra''s algorithm cannot be used with negative edge weights.');
            end
        end
    end

else
    if (strcmpi(options.algname, 'auto'))
        error('shortest_paths:invalidParameter', ...
            'algname auto is not compatible with no check');
    end
end

if options.inf < 0, error('options.inf must be larger than 0'); end

if trans, A = A'; end

if isfield(options,'visitor')
    [d pred] = matlab_bgl_sp_mex(A,u,target,lower(options.algname),options.inf,...
        edge_weight_opt, options.visitor);
else
    [d pred] = matlab_bgl_sp_mex(A,u,target,lower(options.algname),options.inf,...
        edge_weight_opt);
end


function ttf = sin_test()

d = 10;
r = 2;
n = 2;
N = 20000;
x = 2*pi*rand(d,N);
y = sin(sum(x));
tt_rank = [1 ; r*ones(d-1,1) ; 1];
n_basis = n*ones(d,1);
basis_ps = cumsum([1 ; n_basis*N]);
basis_cr = zeros(basis_ps(d+1) - basis_ps(1), 1);
for dim = 1: d
    t = zeros(n, N);
    t(1,:) = sin(x(dim,:));
    t(2,:) = cos(x(dim,:));
	basis_cr(basis_ps(dim):basis_ps(dim+1)-1) = reshape(t, [n*N 1]);
end
coeff = reg_als(basis_cr, y, tt_rank, n_basis);
ttf = tt_function(@fun, coeff);

end

function fx = fun(~, j, x)
% i-th dimension, j-th basis function at x

if j == 1
    fx = sin(x);
elseif j == 2
    fx = cos(x);
end

end


function [convRVTOut, rvtOut, verbose] = tapas_physio_create_rvt_regressors(...
    ons_secs, sqpar, model_rvt, verbose)
% computes respiratory response function regressor and respiratory volume per time
%
%    [convRVTOut, rvtOut, verbose] = tapas_physio_create_rvt_regressors(...
%                                   ons_secs, sqpar, model_rvt, verbose)
% References:
%
%   Birn, R.M., Smith, M.A., Jones, T.B., Bandettini, P.A., 2008.
%       The respiration response function: The temporal dynamics of
%       fMRI signal fluctuations related to changes in respiration.
%       NeuroImage 40, 644-654.
%
%   Harrison, S.J., Bianchi, S., Heinzle, J., Stephan, K.E., Iglesias, S.,
%   Kasper L., 2021.
%   A Hilbert-based method for processing respiratory timeseries.
%   NeuroImage, 117787. https://doi.org/10.1016/j.neuroimage.2021.117787

% IN
%   ons_secs.
%       ons_secs            ons_secs structure with variable `fr`
%                           (filtered respiratory signal time series)
%       sqpar               scan timing information (sequence parameters)
%                           slice onsets etc.
%       model_rvt           rvt modeling parameter structure. e.g.
%                           model_rvt.method
%                           'hilbert' (default, [Harrison2021]) or
%                           'peaks' [Birn2006]
%
% OUT
%   convRVTOut          [nScans, nDelays, nSampleSlices]
%                       respiratory response function regressor after
%                       convolution for specified delays and downsampled
%                       to given slices.
% EXAMPLE
%   [convHRV, hr] = tapas_physio_create_hrv_regressor(physio_out.ons_secs, physio_out.sqpar);
%
%   See also tapas_physio_rvt_hilbert tapas_physio_rvt_peaks tapas_physio_rrf

% Author: Lars Kasper
% Created: 2014-01-20
% Copyright (C) 2014 TNU, Institute for Biomedical Engineering,
%               University of Zurich and ETH Zurich.
%
% This file is part of the physIO toolbox, which is released under the
% terms of the GNU General Public Licence (GPL), version 3. You can
% redistribute it and/or modify it under the terms of the GPL (either
% version 3 or, at your option, any later version). For further details,
% see the file COPYING or <http://www.gnu.org/licenses/>.

if nargin < 3
    physio = tapas_physio_new;
    model_rvt = physio.model.rvt;
end

delays = model_rvt.delays;

if nargin < 4
    verbose.level = [];
    verbose.fig_handles = [];
end

slicenum = 1:sqpar.Nslices;

% Calculate RVT
sample_points  = tapas_physio_get_sample_points(ons_secs, sqpar, slicenum);
switch lower(model_rvt.method)
    case 'peaks'
        [rvt, ~, ~, verbose] = tapas_physio_rvt_peaks(ons_secs.fr, ons_secs.t, sample_points, verbose);
    case 'hilbert'
        [rvt, verbose] = tapas_physio_rvt_hilbert(ons_secs.fr, ons_secs.t, sample_points, verbose);
    otherwise
        error('Unrecognised value for ''rvt.method'' (%s)!', model_rvt.method)
end
rvt = rvt / max(abs(rvt)); % normalize for reasonable range of regressor

if verbose.level >=2
    verbose.fig_handles(end+1) = tapas_physio_get_default_fig_params();
    set(gcf, 'Name', 'Model: Convolution Respiration RVT X RRF');
    subplot(2,2,1)
    plot(sample_points,rvt, 'g');xlabel('time (seconds)');
    title('Respiratory volume per time');
    ylabel('a.u.');
end

% Generate RRF
dt = sqpar.TR / sqpar.Nslices;
t = 0:dt:60;  % seconds
rrf = tapas_physio_rrf(t);
rrf = rrf / max(abs(rrf));

if verbose.level >= 2
    subplot(2,2,2)
    plot(t, rrf,'g'); xlabel('time (seconds)');
    title('Respiratory response function');
end

% Convolve and rescale for display purposes
convRVT = tapas_physio_conv(rvt, rrf, 'causal');
convRVT = convRVT / max(abs(convRVT));

if verbose.level >= 2
    subplot(2,2,3)
    plot(sample_points, convRVT,'g');xlabel('time (seconds)');
    title('Resp vol time X resp response function');
end

% Create shifted regressors convolved time series, which is equivalent to
% delayed response functions according to Wikipedia (convolution)
%
% "Translation invariance[edit]
% The convolution commutes with translations, meaning that
%
% \tau_x ({f}*g) = (\tau_x f)*g = {f}*(\tau_x g)\,
% where \tau_x is the translation of the function f by x defined by
% (\tau_x f)(y) = f(y-x).

% remove mean and linear trend to fulfill periodicity condition for
% shifting
convRVT = detrend(convRVT);

% TODO: what happens at the end/beginning of shifted convolutions?
nDelays = numel(delays);
nShiftSamples = ceil(delays/dt);

% resample to slices needed
nSampleSlices = numel(sqpar.onset_slice);
nScans = numel(sample_points(sqpar.onset_slice:sqpar.Nslices:end));

rvtOut = zeros(nScans,nSampleSlices);
convRVTOut = zeros(nScans,nDelays,nSampleSlices);
samplePointsOut = zeros(nScans,nSampleSlices);

for iDelay = 1:nDelays
    convRVTShifted = circshift(convRVT, nShiftSamples(iDelay));
    for iSlice = 1:nSampleSlices
        onset_slice = sqpar.onset_slice(iSlice);
        rvtOut(:,iSlice) = rvt(onset_slice:sqpar.Nslices:end)';
        convRVTOut(:,iDelay,iSlice) = convRVTShifted(onset_slice:sqpar.Nslices:end);
        samplePointsOut(:,iSlice) = sample_points(onset_slice:sqpar.Nslices:end);
    end
end

if verbose.level >= 2
    subplot(2,2,4)
    [tmp, iShiftMin] = min(abs(delays));
    hp{1} = plot(samplePointsOut, rvtOut,'k--');hold all;
    hp{2} = plot(samplePointsOut, squeeze(convRVTOut(:,iShiftMin,:)),'g');
    xlabel('time (seconds)');
    title('RVT regessor');
    legend([hp{1}(1), hp{2}(1)], 'respiratory volume / time (a. u.)', ...
        'respiratory response regressor');
end

end


addpath('../src/')
addpath('../src/utils/')

% fix random seed
rng(0);

% make your own discrete linear system with disturbance
A = [1 1; 0 1];
B = [0.5; 1];
Q = diag([1, 1]);
R = 0.1;

W_vertex = [0.15, 0.15; 0.15, -0.15; -0.15, -0.15; -0.15, 0.15]; % construct a convex set of disturbance (2dim here)
W = Polyhedron(W_vertex);

% construct disturbance Linear system
disturbance_system = DisturbanceLinearSystem(A, B, Q, R, W);

% constraints on state Xc and input Uc
Xc_vertex = [2, -2; 2 2; -10 2; -10 -2];
Uc_vertex = [1; -1];
Xc = Polyhedron(Xc_vertex);
Uc = Polyhedron(Uc_vertex);

% create a tube_mpc simulater
% if N_horizon is too small, the path will never reach inside the robust MPI-set X_mpi_robust in time step N_horizon, then the problem becomes infeasible.
N_horizon = 10;
w_min = [0; -0.10];
w_max = [0; 0.10];
mpc = TubeModelPredictiveControl(disturbance_system, Xc, Uc, N_horizon);

% The robust MPC guidances the path inside the robust MPI-set so that the path will reach the robust MPI-set in N_horizon.
x = [-7; -2];
savedir_name = './results/';
mkdir(savedir_name);

for i = 1:15
    disp(i)
    u_next = mpc.solve(x);
    x = disturbance_system.propagate(x, u_next); % additive disturbance is considered inside the method
    mpc.show_prediction();
    filename = strcat(savedir_name, 'tmpc_seq', number2string(i), '.png')
    saveas(gcf, char(filename)); % removing this line makes the code much faster
    clf;
end


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 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,[])


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 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 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 [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;


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