function [vert,conn,tria,tnum] = refine2(varargin)
%REFINE2 (Frontal)-Delaunay-refinement for two-dimensional,
%polygonal geometries.
%   [VERT,EDGE,TRIA,TNUM] = REFINE2(NODE,EDGE) returns a co-
%   nstrained Delaunay triangulation of the polygonal region
%   {NODE,EDGE}. NODE is an N-by-2 array of polygonal verti-
%   ces and EDGE is an E-by-2 array of edge indexing. Each
%   row in EDGE represents an edge of the polygon, such that
%   NODE(EDGE(JJ,1),:) and NODE(EDGE(JJ,2),:) are the coord-
%   inates of the endpoints of the JJ-TH edge. If the argum-
%   ent EDGE is omitted it assumed that the vertices in NODE
%   are connected in ascending order.
%
%   [...] = REFINE2(NODE,EDGE,PART) computes a triangulation
%   for a multiply-connected geometry. PART is a cell-array
%   of polygonal "parts", where each element PART{KK} is an
%   array of edge indices defining a given polygonal region.
%   EDGE(PART{KK}, :) is the set of edges in the KK-TH part.
%
%   VERT is a V-by-2 array of XY coordinates in the triangu-
%   lation, EDGE is an array of constrained edges, TRIA is a
%   T-by-3 array of triangles, and TNUM is a T-by-1 array of
%   part indices. Each row of TRIA and EDGE define an eleme-
%   nt. VERT(TRIA(II,1),:), VERT(TRIA(II,2),:) and VERT(TRIA
%   (II,3),:) are the coordinates of the II-TH triangle. The
%   edges in EDGE are defined in a similar manner. NUM is an
%   array of part indexing, such that TNUM(II) is the index
%   of the part in which the II-TH triangle resides.
%
%   [...] = REFINE2(..., OPTS) passes an additional options
%   structure OPTS, containing various user-defined paramet-
%   ers, including:
%
% - OPTS.KIND = {'DELFRONT'}, 'DELAUNAY' -- the type of ref-
%   inement employed. The 'DELFRONT' algorithm is typically
%   slower, but produces higher quality output.
%
% - OPTS.RHO2 = {1.025} -- the maximum allowable radius-edge
%   ratio. Refinement proceeds until all interior triangles
%   satisfy the radius-edge threshold. Smaller radius-edge
%   ratios lead to improved triangle shape, with RHO2=1 req-
%   uiring that all angles exceed 30 degrees. Setting RHO2<1
%   may lead to non-convergence.
%
% - OPTS.REF1 = {'REFINE'}, 'PRESERVE' -- refinement 'flag'
%   for 1-dimensional faces (i.e. edges). The 'PRESERVE' op-
%   tion results in minimal refinement, attempting to retain
%   the initial edges without further subdivision. Edges are
%   split only to satisfy basic geomertical conformance.
%
% - OPTS.REF2 = {'REFINE'}, 'PRESERVE' -- refinement 'flag'
%   for 2-dimensional faces (i.e. trias). The 'PRESERVE' op-
%   tion results in minimal refinement, attempting to retain
%   the initial trias without further subdivision. Trias are
%   split only to satisfy basic geomertical conformance.
%
% - OPTS.SIZ1 = {1.333} -- the normalised rel.-length th-
%   reshold for edge-elements. Each exterior edge is refined
%   until LL/HH<SIZ1, where LL is the edge-length, HH is the
%   edge-centred mesh-size value.
%
% - OPTS.SIZ2 = {1.300} -- the normalised rel.-length th-
%   reshold for tria-elements. Each interior tria is refined
%   until RE/HH<SIZ2, where RE is an effective tria length,
%   based on the circumradius, HH is the tria-centred mesh-
%   size value.
%
% - OPTS.DISP = { +10 } -- refinement verbosity. Set to INF
%   for quiet execution.
%
%   [...] = REFINE2(..., HFUN,ARGS) also passes an optional
%   mesh-size function argument. Setting HFUN = HMAX, where
%   HMAX is a scalar value, imposes a constant size constra-
%   int over the full domain. HFUN can also be defined as a
%   general function handle [HH] = HFUN(PP), where PP is an
%   N-by-2 array of XY coordinates and HH is the associated
%   vector of mesh-size values. User-defined HFUN must be
%   fully vectorised. Additional arguments {A1,A2,...AN} for
%   HFUN can be passed as trailing parameters to REFINE2. In
%   such cases, HFUN must adopt a signature [HH] = HFUN(PP,
%   A1,A2,...,AN). HFUN must return positive values.
%
%   See also SMOOTH2, TRIDIV2, TRICOST, TRIDEMO

%   This routine implements a "multi-refinement" variant of
%   Delaunay-refinement type mesh-generation. Both standard
%   Delaunay-refinement and Frontal-Delaunay type algorithms
%   are available. The Frontal-Delaunay approach is a simpl-
%   ified version of the JIGSAW algorithm, described in:
%
% * D. Engwirda, (2014): "Locally-optimal Delaunay-refineme-
%   nt and optimisation-based mesh generation", Ph.D. Thesis
%   School of Mathematics and Statistics, Univ. of Sydney.
%   http://hdl.handle.net/2123/13148
%
% * D. Engwirda & D. Ivers, (2016): "Off-centre Steiner poi-
%   nts for Delaunay-refinement on curved surfaces", Comput-
%   er-Aided Design, (72), 157--171.
%   http://dx.doi.org/10.1016/j.cad.2015.10.007

%   This work is an extension of the "off-centre" type tech-
%   niques introduced in:
%
% * H. Erten & A. Ungor, (2009): "Quality triangulation with
%   locally optimal Steiner points", SIAM Journal on Scient-
%   ific Comp. 31(3), 2103--2130.
%   http://doi.org/10.1137/080716748
%
% * S. Rebay, (1993): "Efficient Unstructured Mesh Generati-
%   on by Means of Delaunay Triangulation and Bowyer-Watson
%   Algorithm, J. Comp. Physics 106(1), 125--138.
%   http://dx.doi.org/10.1006/jcph.1993.1097

%   Generally speaking, the Delaunay-refinement method impl-
%   emented here is a variantion of the "classical" algorit-
%   hm introduced in:
%
% * J. Ruppert, (1995): "A Delaunay refinement algorithm for
%   quality 2-dimensional mesh generation." Journal of Algo-
%   rithms 18(3), 548--585.
%   http://dx.doi.org/10.1006/jagm.1995.1021
%
%   See also: S. Cheng, T. Dey & J. Shewchuk, (2012): "Dela-
%   unay mesh generation", CRC Press, for comprehensive cov-
%   erage of Delaunay-based meshing techniques.

%   A much more advanced, and fully three-dimensional imple-
%   mentation is available in the JIGSAW library. For addit-
%   ional information, see:
%   https://github.com/dengwirda/jigsaw-matlab

%-----------------------------------------------------------
%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 13/02/2020
%-----------------------------------------------------------

    node = []; PSLG = []; part = {}; opts = [] ;
    hfun = []; harg = {};

%---------------------------------------------- extract args
    if (nargin>=+1), node = varargin{1}; end
    if (nargin>=+2), PSLG = varargin{2}; end
    if (nargin>=+3), part = varargin{3}; end
    if (nargin>=+4), opts = varargin{4}; end
    if (nargin>=+5), hfun = varargin{5}; end
    if (nargin>=+6), harg = varargin(6:end); end

   [opts] = makeopt(opts) ;

%---------------------------------------------- default EDGE
    nnod = size(node,1) ;

    if (isempty(PSLG))
        PSLG = [(1:nnod-1)',(2:nnod)'; nnod,1] ;
    end

%---------------------------------------------- default PART
    ncon = size(PSLG,1) ;

    if (isempty(part)), part{1} = (1:ncon)'; end

%---------------------------------------------- basic checks
    if (~isnumeric(node) || ~isnumeric(PSLG) || ...
        ~iscell   (part) || ~isstruct (opts) )
        error('refine2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(node) ~= +2 || ndims(PSLG) ~= +2)
        error('refine2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(node,2) < +2 || size(PSLG,2) < +2)
        error('refine2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

%---------------------------------------------- basic checks
    if (min([PSLG(:)])<+1 || max([PSLG(:)])>nnod)
        error('refine2:invalidInputs', ...
            'Invalid EDGE input array.') ;
    end

    pmin = cellfun(@min,part);
    pmax = cellfun(@max,part);

    if (min([pmin(:)])<+1 || max([pmax(:)])>ncon)
        error('refine2:invalidInputs', ...
            'Invalid PART input array.') ;
    end

%-------------------------------- prune any non-unique topo.
   [ivec,ivec,jvec] = ...
        unique(sort(PSLG,+2),'rows') ;

    PSLG = PSLG(ivec,:) ;

    for ppos = +1:length(part)

        if ( ~isnumeric(part{ppos}) )
            error (  ...
            'refine2:incorrectInputClass', ...
                'Incorrect input class. ') ;
        end

        part{ppos} = ...
            unique(jvec(part{ppos})) ;

    end

%-------------------------------- check part "manifold-ness"
    for ppos = +1:length(part)

        eloc = PSLG(part{ppos},:) ;
        nadj = ...
            accumarray(eloc(:),1) ;

        if (any(mod(nadj,2) ~= 0) )
        error('refine2:nonmanifoldInputs', ...
            'Non-manifold PART detected.') ;
        end

    end

%---------------------------------------------- output title
    if (~isinf(opts.disp))
        fprintf(1,'\n') ;
        fprintf(1,' Refine triangulation...\n') ;
        fprintf(1,'\n') ;
        fprintf(1,[...
' -------------------------------------------------------\n', ...
'      |ITER.|          |CDT1(X)|          |CDT2(X)|     \n', ...
' -------------------------------------------------------\n', ...
             ] ) ;
    end

%-------------------------------- PASS 0: inflate box bounds
    vert = node; tria = []; tnum = []; iter = 0 ;
    conn = PSLG;

    vmin = min(vert,[],1);      % inflate bbox for stability
    vmax = max(vert,[],1);

    vdel = vmax - 1.*vmin;
    vmin = vmin - .5*vdel;
    vmax = vmax + .5*vdel;

    vbox = [
        vmin(1), vmin(2)
        vmax(1), vmin(2)
        vmax(1), vmax(2)
        vmin(1), vmax(2)
           ] ;
    vert = [vert ; vbox] ;

%-------------------------------- PASS 0: shield sharp feat.
   [vert,conn,tria,tnum,iter] = ...
        cdtbal0(vert,conn,tria,tnum, ...
            node,PSLG,part,opts,hfun,harg,iter);

%-------------------------------- PASS 1: refine 1-simplexes
   [vert,conn,tria,tnum,iter] = ...
        cdtref1(vert,conn,tria,tnum, ...
            node,PSLG,part,opts,hfun,harg,iter);

%-------------------------------- PASS 2: refine 2-simplexes
   [vert,conn,tria,tnum,iter] = ...
        cdtref2(vert,conn,tria,tnum, ...
            node,PSLG,part,opts,hfun,harg,iter);

    if (~isinf(opts.disp)), fprintf(1,'\n'); end

%-------------------------------- trim extra adjacency info.
    tria = tria( :,1:3) ;

%-------------------------------- trim vert. - deflate bbox.
    keep = false(size(vert,1),1);
    keep(tria(:)) = true;
    keep(conn(:)) = true;

    redo = zeros(size(vert,1),1);
    redo(keep) = ...
        (+1:length(find(keep)))';

    conn = ...
        reshape(redo(conn),[],2);
    tria = ...
        reshape(redo(tria),[],3);

    vert = vert(keep,:) ;

end

function [vert,conn,tria,tnum,iter] = ...
            cdtbal0(vert,conn,tria,tnum, ...
                node,PSLG,part,opts,hfun,harg,iter)
%CDTBAL0 constrained Delaunay-refinement for "sharp" 0-dim.
%features at PSLG vertices.
%   [...] = CDTBAL0(...) refines the set of 1-simplex eleme-
%   nts incident to "sharp" features in the PSLG. Specifica-
%   lly, edges that subtend "small" angles are split about a
%   set of new "collar" vertices, equi-distributed about the
%   centre of "sharp" features. Collar size is computed as a
%   min. of the incident edge-len. and local mesh-size cons-
%   traints.

    if (iter <= opts.iter)

    %------------------------------------- build current CDT
       [vert,conn, ...
        tria,tnum] = deltri2(vert,conn, ...
                             node,PSLG, ...
                             part, ...
                             opts.dtri) ;

    %------------------------------------- build current adj
       [edge,tria] = tricon2(tria,conn) ;

       [feat,ftri] = isfeat2(vert, ...
                             edge,tria) ;

        apex = false(size(vert,1), 1) ;
        apex(tria(ftri)) =  true ;

    %------------------------------------- eval. length-fun.
        if (~isempty(hfun))
            if (isnumeric(hfun))
            vlen = hfun * ...
              ones(size(vert,1),1) ;
            else
            vlen = feval( ...
                hfun,vert,harg{:}) ;
            vlen = vlen(:) ;
            end
        else
            vlen = +inf * ...
              ones(size(vert,1),1) ;
        end

    %------------------------------------- form edge vectors
        evec = vert(conn(:,2),:) ...
             - vert(conn(:,1),:) ;
        elen = sqrt(sum(evec.^2,2));
        evec = evec./[elen,elen] ;

    %------------------------------------- min. adj. lengths
        for epos = +1 : size(conn,1)

            ivrt = conn(epos,1) ;
            jvrt = conn(epos,2) ;

            vlen(ivrt) = min( ...
            vlen(ivrt), .67*elen(epos)) ;
            vlen(jvrt) = min( ...
            vlen(jvrt), .67*elen(epos)) ;

        end

    %------------------------------------- mark feature edge
        iref = apex(conn(:,1)) ...      %- refine at vert. 1
            & ~apex(conn(:,2)) ;
        jref = apex(conn(:,2)) ...      %- refine at vert. 2
            & ~apex(conn(:,1)) ;
        dref = apex(conn(:,1)) ...      %- refine at both!
            &  apex(conn(:,2)) ;

        keep =~apex(conn(:,1)) ...      %- refine at neither
            & ~apex(conn(:,2)) ;

    %------------------------------------- protecting collar
        ilen = vlen(conn(iref,1)) ;
        inew = vert(conn(iref,1),:) ...
        + [ilen,ilen].*evec(iref,:) ;

        jlen = vlen(conn(jref,2)) ;
        jnew = vert(conn(jref,2),:) ...
        - [jlen,jlen].*evec(jref,:) ;

        Ilen = vlen(conn(dref,1)) ;
        Inew = vert(conn(dref,1),:) ...
        + [Ilen,Ilen].*evec(dref,:) ;

        Jlen = vlen(conn(dref,2)) ;
        Jnew = vert(conn(dref,2),:) ...
        - [Jlen,Jlen].*evec(dref,:) ;

        vnew = [inew; jnew; Inew; Jnew] ;

    %------------------------------------- add new vert/edge
        iset = (1:size(inew,1))' ...
                + size(vert,1) ;

        jset = (1:size(jnew,1))' ...
                + size(inew,1) + ...
                + size(vert,1) ;

        Iset = (1:size(Inew,1))' ...
                + size(inew,1) + ...
                + size(jnew,1) + ...
                + size(vert,1) ;

        Jset = (1:size(Jnew,1))' ...
                + size(inew,1) + ...
                + size(jnew,1) + ...
                + size(Inew,1) + ...
                + size(vert,1) ;

        vert = [vert ; vnew] ;

        cnew = [conn(iref,1), iset ;
                conn(iref,2), iset ;
                conn(jref,2), jset ;
                conn(jref,1), jset ;
                conn(dref,1), Iset ;
                conn(dref,2), Jset ;
                Iset, Jset] ;
        conn = [conn(keep,:); cnew ] ;

    end

end

function [vert,conn,tria,tnum,iter] = ...
            cdtref1(vert,conn,tria,tnum, ...
                node,PSLG,part,opts,hfun,harg,iter)
%CDTREF1 constrained Delaunay-refinement for 1-simplex elem-
%nts embedded in R^2.
%   [...] = CDTREF1(...) refines the set of 1-simplex eleme-
%   nts embedded in the triangulation until all constraints
%   are satisfied. Specifically, edges are refined until all
%   local mesh-spacing and encroachment conditions are met.
%   Refinement proceeds according to either a Delaunay-refi-
%   nement or Frontal-Delaunay type approach, depending on
%   user-settings. In either case, new steiner vertices are
%   introduced to split "bad" edges - those that violate the
%   set of prescribed constraints. In the "-DR" type process
%   edges are split about their circumballs (midpoints). In
%   the "-FD" approach, new vertices are positioned such th-
%   at mesh-spacing constraints are satisfied in a "locally-
%   optimal" fashion.

    tcpu.full = +0. ;
    tcpu.ball = +0. ;
    tcpu.hfun = +0. ;
    tcpu.encr = +0. ;
    tcpu.offc = +0. ;

    vidx = (1:size(vert,1))';     %- "new" vert list to test

    tnow =  tic ;

    ntol = +1.55;

    while (strcmpi(opts.ref1,'refine'))

        iter = iter + 1 ;

        if (iter>=opts.iter),break; end

    %------------------------------------- calc. circumballs
        ttic = tic ;

        bal1 = cdtbal1(vert,conn) ;

        tcpu.ball = ...
            tcpu.ball + toc(ttic) ;

    %------------------------------------- eval. length-fun.
        ttic = tic ;

        if (~isempty(hfun))
            if (isnumeric(hfun))
            fun0 = hfun * ...
              ones(size(vert,1),1);
            fun1 = hfun ;
            else
            fun0(vidx) = ...
                feval(hfun, ...
            vert(vidx,:), harg{:});
            fun0 = fun0(:) ;
            fun1 = fun0(conn(:,1))...
                 + fun0(conn(:,2));
            fun1 = fun1 / +2. ;
            end
        else
            fun0 = +inf * ...
              ones(size(vert,1),1);
            fun1 = +inf ;
        end

        siz1 = ...
         +4. * bal1(:,3)./(fun1.*fun1) ;

        tcpu.hfun = ...
            tcpu.hfun + toc(ttic) ;

    %------------------------------------- test encroachment
        ttic = tic ;

        bal1(:,3) = ...
            (1.-eps^.75) * bal1(:,3) ;

       [vp,vi] = ...
           findball(bal1,vert(:,1:2));

    %------------------------------------- near=>[vert,edge]
        next = +0;
        ebad = false(size(conn,1),1) ;
        near = zeros(size(conn,1),1) ;
        for ii = +1 : size(vp,1)
            for ip = vp(ii,1):vp(ii,2)
                jj = vi(ip);
                if (ii ~= conn(jj,1) ...
                &&  ii ~= conn(jj,2) )
                next = next + 1;
                near(next,1) = ii;
                near(next,2) = jj;
                end
            end
        end

        near = near(1:next-0,:);

        if (~isempty(near))
    %-- mark edge "encroached" if there is a vert within its
    %-- dia.-ball that is not joined to either of its vert's
    %-- via an existing edge...
            ivrt = conn(near(:,2),1);
            jvrt = conn(near(:,2),2);

            pair = [near(:,1), ivrt];
            ivec = setset2(pair,conn) ;

            pair = [near(:,1), jvrt];
            jvec = setset2(pair,conn) ;

            okay = ~ivec & ~jvec ;

            ebad(near(okay,2))=true ;

        end

        tcpu.encr = ...
            tcpu.encr + toc(ttic);

    %------------------------------------- refinement queues
        ref1 = false(size(conn,1),1);
        ref1(ebad)           = true ;   %- edge encroachment
        ref1(siz1>opts.siz1* ...        %- bad equiv. length
                  opts.siz1) = true ;

        num1 = find(ref1)  ;

    %------------------------------------- dump-out progess!
        if (mod(iter,opts.disp)==0)
            numc = size(conn,1) ;
            numt = size(tria,1) ;
            fprintf(+1, ...
            '%11i %18i %18i\n', ...
            [iter,numc,numt]) ;
        end

    %------------------------------------- nothing to refine
        if (isempty(num1)), break; end

    %------------------------------------- refine "bad" tria
        switch (lower(opts.kind))
        case 'delaunay'
    %------------------------------------- do circ-ball pt's
        new1 = bal1(ref1, 1:2) ;

        vidx = (1:size(new1,1))' ...
                + size(vert,1) ;

        cnew = [conn( ref1,1), vidx
                conn( ref1,2), vidx];
        conn = [conn(~ref1,:); cnew];

    %------------------------------------- update vertex set
        vert = [vert; new1(:,1:2)];

        case 'delfront'
    %-- symmetric off-centre scheme:- refine edges from both
    %-- ends simultaneously, placing new vertices to satisfy
    %-- the worst of mesh-spacing and local voronoi constra-
    %-- ints.

        ttic = tic ;

        evec = vert(conn(ref1,2),:) ...
             - vert(conn(ref1,1),:) ;
        elen = sqrt(sum(evec.^2,2)) ;
        evec = evec ./ [elen, elen] ;

    %------------------------------------- "voro"-type dist.
        vlen = sqrt(bal1(ref1,3));

    %------------------------------------- "size"-type dist.
        ihfn = fun0(conn(ref1,1));
        jhfn = fun0(conn(ref1,2));

    %------------------------------------- bind "safe" dist.
        ilen = min(vlen,ihfn) ;
        jlen = min(vlen,jhfn) ;

    %------------------------------------- locate offcentres
        inew = vert(conn(ref1,1),:) ...
             + [ilen,ilen].*evec ;
        jnew = vert(conn(ref1,2),:) ...
             - [jlen,jlen].*evec ;

    %------------------------------------- iter. "size"-type
        for ioff = +1 : +3
    %------------------------------------- eval. length-fun.
        if (~isempty(hfun))
            if (isnumeric(hfun))
            iprj = hfun * ...
              ones(size(inew,1),1);
            jprj = hfun * ...
              ones(size(jnew,1),1);
            else
            iprj = feval( ...
                hfun,inew,harg{:});
            jprj = feval( ...
                hfun,jnew,harg{:});
            iprj = iprj(:);
            jprj = jprj(:);
            end
        else
            iprj = +inf * ...
              ones(size(inew,1),1);
            jprj = +inf * ...
              ones(size(jnew,1),1);
        end

        iprj = 0.5*ihfn + 0.5*iprj;
        jprj = 0.5*jhfn + 0.5*jprj;

    %------------------------------------- bind "safe" dist.
        ilen = min(vlen,iprj) ;
        jlen = min(vlen,jprj) ;

    %------------------------------------- locate offcentres
        inew = vert(conn(ref1,1),:) ...
             + [ilen,ilen].*evec ;
        jnew = vert(conn(ref1,2),:) ...
             - [jlen,jlen].*evec ;

        end

    %------------------------------------- merge i,j if near
        near = ...
            ilen+jlen>=vlen*ntol ;

        znew = inew(near,:) * .5 ...
             + jnew(near,:) * .5 ;

        inew = inew(~near,1:2) ;
        jnew = jnew(~near,1:2) ;

    %------------------------------------- split constraints
        zset = (1:size(znew,1))' ...
                + size(vert,1) ;

        iset = (1:size(inew,1))' ...
                + size(znew,1) + ...
                + size(vert,1) ;

        jset = (1:size(jnew,1))' ...
                + size(znew,1) + ...
                + size(inew,1) + ...
                + size(vert,1) ;

        set1 = num1( near);
        set2 = num1(~near);

        cnew = [conn( set1,1), zset
                conn( set1,2), zset
                conn( set2,1), iset
                conn( set2,2), jset
                iset, jset ] ;
        conn = [conn(~ref1,:); cnew];

    %------------------------------------- update vertex set
        vert = [vert; znew(:,1:2)];
        vert = [vert; inew(:,1:2)];
        vert = [vert; jnew(:,1:2)];

        vidx = [zset; iset; jset] ;

        tcpu.offc = ...
            tcpu.offc + toc(ttic) ;

        end % switch(lower(opts.kind))

    end

    tcpu.full = ...
        tcpu.full + toc(tnow) ;

    if (~isinf(opts.disp) )
    %------------------------------------- print final stats
        numc = size(conn,1) ;
        numt = size(tria,1) ;
        fprintf(+1, ...
        '%11i %18i %18i\n', ...
        [iter,numc,numt]) ;
    end

    if (opts.dbug)
    %------------------------------------- print debug timer
        fprintf(1,'\n') ;
        fprintf(1,' 1-simplex REF. timer...\n');
        fprintf(1,'\n') ;
        fprintf(1, ...
        ' FULL: %f \n', tcpu.full);
        fprintf(1, ...
        ' BALL: %f \n', tcpu.ball);
        fprintf(1, ...
        ' HFUN: %f \n', tcpu.hfun);
        fprintf(1, ...
        ' ENCR: %f \n', tcpu.encr);
        fprintf(1, ...
        ' OFFC: %f \n', tcpu.offc);
        fprintf(1,'\n') ;
    end

end

function [vert,conn,tria,tnum,iter] = ...
            cdtref2(vert,conn,tria,tnum, ...
                node,PSLG,part,opts,hfun,harg,iter)
%CDTREF2 constrained Delaunay-refinement for 2-simplex elem-
%nts embedded in R^2.
%   [...] = CDTREF2(...) refines the set of 2-simplex eleme-
%   nts embedded in the triangulation until all constraints
%   are satisfied. Specifically, triangles are refined until
%   all local mesh-spacing and element-shape conditions are
%   met. Refinement proceeds according to either a Delaunay-
%   refinement or Frontal-Delaunay type approach, depending
%   on user-settings. In either case, new steiner points are
%   introduced to split "bad" triangles - those that violate
%   the set of prescribed constraints. In the "-DR" type pr-
%   ocess triangles are split about their circumballs. In
%   the "-FD" approach, new vertices are positioned such th-
%   at mesh-spacing and element-shape constraints are satis-
%   fied in a "locally-optimal" fashion.

    tcpu.full = +0. ;
    tcpu.dtri = +0. ;
    tcpu.tcon = +0. ;
    tcpu.ball = +0. ;
    tcpu.hfun = +0. ;
    tcpu.offc = +0. ;
    tcpu.filt = +0. ;

    vidx = (1:size(vert,1))';     %- "new" vert list to test

    tnow =  tic ;

    near = +.775;

    while (strcmpi(opts.ref2,'refine'))

        iter = iter + 1 ;

    %------------------------------------- build current CDT
        ttic = tic ;

        nold = size(vert,1) ;

       [vert,conn, ...
        tria,tnum]= deltri2(vert,conn, ...
                            node,PSLG, ...
                            part, ....
                            opts.dtri) ;

        nnew = size(vert,1) ;

        vidx = ...
       [vidx; (nold:nnew)'] ;

        tcpu.dtri = ...
            tcpu.dtri + toc(ttic) ;

    %------------------------------------- build current adj
        ttic = tic ;

       [edge,tria]= tricon2(tria,conn) ;

        tcpu.tcon = ...
            tcpu.tcon + toc(ttic) ;

        if (iter>=opts.iter),break; end

    %------------------------------------- calc. circumballs
        ttic = tic ;

        bal1 = cdtbal1(vert,conn) ;
        bal2 = cdtbal2(vert, ...
                       edge,tria) ;
        len2 = minlen2(vert,tria) ;

        rho2 = bal2(:,+3) ./ len2 ;

    %------------------------------------- refinement scores
        scr2 = rho2 .* bal2(:,+3) ;

        tcpu.ball = ...
            tcpu.ball + toc(ttic) ;

    %------------------------------------- eval. length-fun.
        ttic = tic ;

        if (~isempty(hfun))
            if (isnumeric(hfun))
            fun0 = hfun * ...
              ones(size(vert,1),1);
            fun2 = hfun ;
            else
            fun0(vidx) = ...
                feval(hfun, ...
            vert(vidx,:), harg{:});
            fun0 = fun0(:) ;
            fun2 = fun0(tria(:,1))...
                 + fun0(tria(:,2))...
                 + fun0(tria(:,3));
            fun2 = fun2 / +3. ;
            end
        else
            fun0 = +inf * ...
              ones(size(vert,1),1);
            fun2 = +inf ;
        end

        siz2 = ...
         +3. * bal2(:,3)./(fun2.*fun2) ;

        tcpu.hfun = ...
            tcpu.hfun + toc(ttic) ;

    %------------------------------------- refinement queues
        ref1 = false(size(conn,1),1);
        ref2 = false(size(tria,1),1);

        stri = isfeat2(vert,edge,tria) ;

        ref2(rho2>opts.rho2* ...        %- bad rad-edge len.
                  opts.rho2) = true ;
        ref2(stri) = false ;
        ref2(siz2>opts.siz2* ...        %- bad equiv. length
                  opts.siz2) = true ;

        num2 = find(ref2);

    %------------------------------------- dump-out progess!
        if (mod(iter,opts.disp)==0)
            numc = size(conn,1) ;
            numt = size(tria,1) ;
            fprintf(+1, ...
            '%11i %18i %18i\n', ...
            [iter,numc,numt]) ;
        end

    %------------------------------------- nothing to refine
        if (isempty(num2)), break; end

       [scr2,idx2] = sort( ...
            scr2(num2),'descend');
        num2 = num2(idx2);

    %------------------------------------- refine "bad" tria
        switch (lower(opts.kind))
        case 'delaunay'
    %------------------------------------- do circ-ball pt's
        new2 = zeros(length(num2),3);
        new2(:,1:2) = bal2(num2,1:2);

        rmin = ...                      %- min. insert radii
            len2(num2)*(1.-eps^.75)^2 ;

        new2(:,  3) = max( ...
            bal2(num2,3)*near^2,rmin) ;

        case 'delfront'
    %-- off-centre scheme -- refine triangles by positioning
    %-- new vertices along a local segment of the voronoi
    %-- diagram, bounded by assoc. circmballs. New points
    %-- are placed to satisfy the worst of local mesh-length
    %-- and element-shape constraints.

        ttic = tic ;

    %------------------------------------- find frontal edge
       [lmin,emin] = ...
            minlen2(vert,tria(num2,:)) ;

        ftri = false(length(num2),1) ;
        epos = zeros(length(num2),1) ;
        tadj = zeros(length(num2),1) ;

        for ii = +1 : length(epos)
            epos(ii) = tria( ...
                num2(ii),emin(ii)+3) ;
        end

    %------------------------------------- find frontal tria
        for enum = +1 : +3

            eidx = tria(num2,enum+3) ;

            ftri = ...
            ftri | edge(eidx,5) > +0 ;

            ione = ...
                num2 ~= edge(eidx,3) ;
            itwo = ~ione ;

            tadj(ione) = ...
                edge(eidx(ione),3);
            tadj(itwo) = ...
                edge(eidx(itwo),4);

            okay = tadj > +0 ;
            tidx = tadj(okay);

            ftri(okay) = ...
            ftri(okay) | ~ref2(tidx) ;

        end

        if (~any(ftri))                 %- can this happen!?
        ftri = true(length(num2),+1) ;
        end

    %------------------------------------- locate offcentres
        emid = vert(edge(epos,+1),:) ...
             + vert(edge(epos,+2),:) ;
        emid = emid * +0.50 ;

        elen = sqrt(lmin(:));

    %------------------------------------- "voro"-type dist.
        vvec = bal2(num2,1:2)-emid ;
        vlen = sqrt(sum(vvec.^2,2));
        vvec = vvec ./ [vlen,vlen] ;

        hmid = fun0(edge(epos,+1),:) ...
             + fun0(edge(epos,+2),:) ;
        hmid = hmid * +0.50 ;

    %------------------------------------- "ball"-type dist.
        rtri = elen * opts.off2 ;
        rfac = elen * +0.50 ;
        dsqr = rtri.^2 - rfac.^2;
        doff = rtri + ...
            sqrt(max(+0.,dsqr)) ;

    %------------------------------------- "size"-type dist.
        dsiz = +sqrt(3.)/2. * hmid ;

    %------------------------------------- bind "safe" dist.
       [dist,ioff] = ...
          min([dsiz,doff,vlen],[],2) ;

    %------------------------------------- locate offcentres
        off2 = ...
        emid + [dist,dist] .* vvec ;

    %------------------------------------- iter. "size"-type
        for isub = +1 : +3
    %------------------------------------- eval. length-fun.
        if (~isempty(hfun))
            if (isnumeric(hfun))
            hprj = hfun * ...
              ones(size(off2,1),1) ;
            else
            hprj = feval( ...
                hfun,off2,harg{:}) ;
            hprj = hprj(:) ;
            end
        else
            hprj = +inf * ...
              ones(size(off2,1),1) ;
        end

    %------------------------------------- "size"-type dist.
        hprj = .33*hmid + .67*hprj ;

        dsiz = +sqrt(3.)/2. * hprj ;

        dsiz(dsiz<elen*.50) = +inf ;    %- edge-ball limiter
        dsiz(dsiz>vlen*.95) = +inf ;    %- circ-ball limiter

    %------------------------------------- bind "safe" dist.
       [dist,ioff] = ...
          min([dsiz,doff,vlen],[],2) ;

    %------------------------------------- locate offcentres
        off2 = ...
        emid + [dist,dist] .* vvec ;

        end

        orad = ...
        sqrt((elen*.5).^2 + dist.^2) ;

    %------------------------------------- do offcentre pt's
        new2 = ...
        zeros(length(find(ftri)),+3) ;
        new2(:,1:2) = off2(ftri,1:2) ;

        rmin = ...                      %- min. insert radii
            lmin(ftri)*(1.-eps^.75)^2 ;

        new2(:,  3) = max( ...
            (orad(ftri)*near).^2,rmin);

        tcpu.offc = ...
            tcpu.offc + toc (ttic) ;

        end % switch(lower(opts.kind))

    %------------------------------------- inter.-ball dist.
        ttic = tic ;

    %------------------------------------- proximity filters
       [vp,vi] = ...
          findball(new2,new2(:,1:2)) ;

        keep = true (size(new2,1),1) ;
        for ii = size(vp,1):-1:+1
            for ip = vp(ii,1) ...
                   : vp(ii,2)
                jj = vi(ip);
                if (keep(jj) && ...
                    keep(ii) && ...
                    jj < ii )

                keep(ii) = false ;
                break;

                end
            end
        end

        new2 = new2(keep,:);

    %------------------------------------- test encroachment
        bal1(:,3) = ...
            (1.-eps^.75) * bal1(:,3);

       [vp,vi] = ...
          findball(bal1,new2(:,1:2));

        keep = true (size(new2,1),1);
        for ii = +1:+1:size(vp,1)
            for ip = vp(ii,1) ...
                   : vp(ii,2)
                jj = vi(ip);
                ref1(jj) =  true ;
                keep(ii) = false ;
            end
        end

    %------------------------------------- leave sharp edges
        ebnd = false(size(edge,1),1);
        ebnd(tria(stri,4:6)) = true ;

        enot = ...
        setset2(conn,edge(ebnd,1:2));

        ref1(enot) = false ;

    %------------------------------------- preserve boundary
        if (strcmp(lower(opts.ref1),...
            'preserve'))
        ref1(:)    = false ;
        end

    %------------------------------------- refinement points
        new2 = new2(keep,:);
        new1 = bal1(ref1,:);

        tcpu.filt = ...
            tcpu.filt + toc(ttic) ;

    %------------------------------------- split constraints
        idx1 = ...
       (1:size(new1))'+size(vert,1) ;

        idx2 = ...
       (1:size(new2))'+size(new1,1) ...
                      +size(vert,1) ;

        cnew = [conn( ref1,1), idx1
                conn( ref1,2), idx1];
        conn = [conn(~ref1,:); cnew];

        vidx = [idx1; idx2];

    %------------------------------------- update vertex set
        nold = size(vert,1);
        vert = [vert; new1(:,1:2)];
        vert = [vert; new2(:,1:2)];
        nnew = size(vert,1);

        if (nnew == nold), break; end   %- we *must* be done

    end

    tcpu.full = ...
        tcpu.full + toc(tnow) ;

    if (~isinf(opts.disp) )
    %------------------------------------- print final stats
        numc = size(conn,1) ;
        numt = size(tria,1) ;
        fprintf(+1, ...
        '%11i %18i %18i\n', ...
        [iter,numc,numt]) ;
    end

    if (opts.dbug)
    %------------------------------------- print debug timer
        fprintf(1,'\n') ;
        fprintf(1,' 2-simplex REF. timer...\n');
        fprintf(1,'\n') ;
        fprintf(1, ...
        ' FULL: %f \n', tcpu.full);
        fprintf(1, ...
        ' DTRI: %f \n', tcpu.dtri);
        fprintf(1, ...
        ' TCON: %f \n', tcpu.tcon);
        fprintf(1, ...
        ' BALL: %f \n', tcpu.ball);
        fprintf(1, ...
        ' HFUN: %f \n', tcpu.hfun);
        fprintf(1, ...
        ' OFFC: %f \n', tcpu.offc);
        fprintf(1, ...
        ' FILT: %f \n', tcpu.filt);
        fprintf(1,'\n') ;
    end

end

function [opts] = makeopt(opts)
%MAKEOPT setup the options structure for REFINE2.

    if (~isfield(opts,'dtri'))
        opts.dtri = 'constrained';
    else
    if (~strcmpi(opts.dtri, 'conforming') && ...
        ~strcmpi(opts.dtri,'constrained') )
        error( ...
    'refine2:invalidOption','Invalid constraint DTRI.');
    end
    end

    if (~isfield(opts,'kind'))
        opts.kind = 'delfront';
    else
    if (~strcmpi(opts.kind, 'delfront') && ...
        ~strcmpi(opts.kind, 'delaunay') )
        error( ...
    'refine2:invalidOption','Invalid refinement KIND.');
    end
    end

    if (~isfield(opts,'ref1'))
        opts.ref1 = 'refine';
    else
    if (~strcmpi(opts.ref1,   'refine') && ...
        ~strcmpi(opts.ref1, 'preserve') )
        error( ...
    'refine2:invalidOption','Invalid refinement REF1.');
    end
    end

    if (~isfield(opts,'ref2'))
        opts.ref2 = 'refine';
    else
    if (~strcmpi(opts.ref2,   'refine') && ...
        ~strcmpi(opts.ref2, 'preserve') )
        error( ...
    'refine2:invalidOption','Invalid refinement REF2.');
    end
    end

    if (~isfield(opts,'iter'))
        opts.iter = +inf;
    else
    if (~isnumeric(opts.iter))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.iter)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.iter <= +0)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.ITER selection.') ;
    end
    end

    if (~isfield(opts,'disp'))
        opts.disp = +10 ;
    else
    if (~isnumeric(opts.disp))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.disp)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.disp <= +0)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.DISP selection.') ;
    end
    end

    if (~isfield(opts,'rho2'))
        opts.rho2 = 1.025;
    else
    if (~isnumeric(opts.rho2))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.rho2)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.rho2 < +1.)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.RHO2 selection.') ;
    end
    end

    if (~isfield(opts,'off2'))
        opts.off2 = 0.933;
    else
    if (~isnumeric(opts.off2))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.off2)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.off2 < +.7)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.OFF2 selection.') ;
    end
    end

    if (~isfield(opts,'siz1'))
        opts.siz1 = 1.333;
    else
    if (~isnumeric(opts.siz1))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.siz1)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.siz1 <= 0.)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.SIZ1 selection.') ;
    end
    end

    if (~isfield(opts,'siz2'))
        opts.siz2 = 1.300;
    else
    if (~isnumeric(opts.siz2))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.siz2)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.siz2 <= 0.)
        error('refine2:invalidOptionValues', ...
            'Invalid OPT.SIZ2 selection.') ;
    end
    end

    if (~isfield(opts,'dbug'))
        opts.dbug = false;
    else
    if (~islogical(opts.dbug))
        error('refine2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.dbug)~= +1)
        error('refine2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    end

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = MC_merton_sgs(S,r,sigma,T, a,b,lambda,NSim)
%Implements the SGS method for the Merton model
nu = r - lambda*(exp(a+0.5*b^2)-1)-0.5*sigma^2; % martingale correction

X = log(S)*ones(NSim,1);          % X is the log price path

for k=1:NSim
    t = 0;
    tau = [];
    %simulate the jump times first
    while t < T
        dt = -log(rand)/lambda;% jump time
        t = t + dt;            % add the jump times
        tau = [tau; dt];             % not good matlab but works!
    end
    tau(end) = T-(t-dt);
    N = length(tau);                    % N number of jumps + 1
    W1 = randn(1,N);                    % uniforms for diffusion
    if N > 1
        W2 = randn(1,N-1);              % uniforms for jumps
        for i = 1:N-1
            Z = nu*tau(i) + sigma*sqrt(tau(i)) * W1(i);
            lnY = a+b*W2(i);
            X(k) = X(k) + Z + lnY;
        end
    end

    dt_end = tau(end);
    Z = nu * dt_end + sigma * sqrt(dt_end) * W1(end);
    X(k) = X(k) + Z;
end
y = exp(X);
end


function a = asinh(a)
%ASINH        Gradient inverse hyperbolic sine asinh(a)
%

% written  10/16/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 08/26/12     S.M. Rump  global variables removed
%

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

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');
  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  ax = 1 ./ sqrt( 1 + sqr(full(a.x(:))) );
  a.x = asinh(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
      else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  if rndold
    setround(rndold)
  end


classdef OptimalControler < handle
    % Let s :=[x(0), .., x(n), u(0)....u(n-1)] be decision variables, then
    % optimal control problem is composed by
    % (i) objective function V(s) = s'*H*S in quadratic form
    % (ii) equality constraints specified by C_eq1 and C_eq2 where C_eq1*s =C_eq2
    % (iii) inequality constraints specified by C_ineq1 and C_ineq2 where C_ineq1 * s <= C_ineq2
    %
    % For potential future extensions, all constraints will be managed by ConstraintManager class.
    % Any time you add a new constraint, those constraint is pushed into the manager with a key (name of constraint).

    properties (SetAccess = private)
        sys; %system
        Xc; Uc; % constraints set for statespace and input space
        x_min; x_max; % lower and upper bound of Xc
        N; % prediction horizon
        Ak; % S.T.M of closed-roop system with LQR feedback
        n_opt; % dim. of optimization parameter s :=[x(0), .., x(n), u(0)....u(n-1)]
        H; % positive definite matrix for objective function V(s) = s'*H*s
        constraint_manager;
    end

    %% Public Methods
    methods (Access = public)

        function obj = OptimalControler(sys, Xc, Uc, N)
            obj.sys = sys;
            obj.Xc = Xc;
            obj.x_min = min(Xc.V, [], 1)';
            obj.x_max = max(Xc.V, [], 2)';
            obj.Uc = Uc;
            obj.N = N;

            obj.n_opt = obj.sys.nx*(obj.N+1)+obj.sys.nu*obj.N;
            obj.constraint_manager = ConstraintManager();

            obj.H = obj.construct_costfunction();
            [C_eq1, C_eq2] = obj.construct_dynamics_constraint();
            [C_ineq1, C_ineq2] = obj.construct_ineq_constraint(Xc, Uc);

            %% Let's change initial and dynamics constraints!!
            obj.constraint_manager.add_eq_constraint('dynamics', C_eq1, C_eq2);
            obj.constraint_manager.add_ineq_constraint('feasible', C_ineq1, C_ineq2);
        end

        function add_initial_eq_constraint(obj, x_init)
            % E * x0 = x_init
            idx_x0_start = 1;
            idx_x0_end = obj.sys.nx;

            C_eq1_init = zeros(obj.sys.nx, obj.n_opt);
            C_eq1_init(:, idx_x0_start:idx_x0_end) = eye(obj.sys.nx);
            C_eq2_init = x_init;
            obj.constraint_manager.add_eq_constraint('initial', C_eq1_init, C_eq2_init);
        end

        function add_terminal_constraint(obj, Xadd)
            [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, obj.N+1);
            obj.constraint_manager.add_ineq_constraint('terinal', C_ineq1_add, C_ineq2_add);
        end

        function add_initial_constraint(obj, Xadd)
            [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, 1);
            obj.constraint_manager.add_ineq_constraint('initial', C_ineq1_add, C_ineq2_add);
        end

        function [x_seq, u_seq] = solve(obj)
            quadprog_solved = 0;
            [C_eq1, C_eq2] = obj.constraint_manager.combine_all_eq_constraints();
            [C_ineq1, C_ineq2] = obj.constraint_manager.combine_all_ineq_constraints();

            options = optimoptions('quadprog', 'Display', 'none');
            [var_optim, ~, exitflag] = quadprog(obj.H, [], C_ineq1, C_ineq2, C_eq1, C_eq2, [], [], [], options);
            x_seq = reshape(var_optim(1:obj.sys.nx*(obj.N+1)), obj.sys.nx, obj.N+1);
            u_seq = reshape(var_optim(obj.sys.nx*(obj.N+1)+1:obj.n_opt), obj.sys.nu, obj.N);

        end

    end

    %% Methods Used in Constoructor
    methods (Access = private)

        function H = construct_costfunction(obj)
            % compute H
            Q_block = [];
            R_block = [];
            for itr=1:obj.N
                Q_block = blkdiag(Q_block, obj.sys.Q);
                R_block = blkdiag(R_block, obj.sys.R);
            end
            H = blkdiag(Q_block, obj.sys.P, R_block);
        end

        function [C_eq1, C_eq2] = construct_dynamics_constraint(obj)
            % compute C_eq1 and C_eq2
            function C_ss_eq1 = single_step_dynamics_eq1(k)
                % A x(k) - E x(k+1) + Bu(k) = 0
                idx_xk_start = obj.sys.nx * k + 1;
                idx_xk_end = obj.sys.nx * (k + 1);

                idx_xkp1_start = obj.sys.nx * (k + 1) + 1;
                idx_xkp1_end = obj.sys.nx * (k + 2);

                idx_uk_start  = obj.sys.nx * (obj.N+1) + obj.sys.nu * k + 1;
                idx_uk_end  = obj.sys.nx * (obj.N+1) + obj.sys.nu * (k + 1);

                C_ss_eq1 = zeros(obj.sys.nx, obj.n_opt);

                C_ss_eq1(:, idx_xk_start:idx_xk_end) = obj.sys.A;
                C_ss_eq1(:, idx_xkp1_start:idx_xkp1_end) = - eye(obj.sys.nx);
                C_ss_eq1(:, idx_uk_start:idx_uk_end) = obj.sys.B;
            end

            C_eq1 = [];
            for k = 0:obj.N-1
                C_ss_eq1 = single_step_dynamics_eq1(k);
                C_eq1 = [C_eq1; C_ss_eq1];
            end
            C_eq2 = zeros(size(C_eq1, 1), 1);
        end

        function [C_ineq1, C_ineq2] = construct_ineq_constraint(obj, Xc, Uc)
            % compute C_ineq
            [F, G, nc] = convert_Poly2Mat(Xc, Uc);

            F_block = [];
            G_block = [];
            for itr = 1:obj.N
                G_block = blkdiag(G_block, G);
            end
            for itr = 1:obj.N+1
                F_block = blkdiag(F_block, F);
            end
            C_ineq1 = [F_block, [G_block; zeros(nc, obj.sys.nu*obj.N)]];
            nc_total = size(C_ineq1, 1);
            C_ineq2 = ones(nc_total, 1);
        end

    end

    methods (Access = private)

        function [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, k_add)
            % add a new constraint at time step k
            if Xadd.contains(zeros(2, 1)) % If Xadd contains the origin, the contraint can be expressed as C1*x<=1
                [F_add, ~, nc_add] = convert_Poly2Mat(Xadd, Polyhedron());
                C_ineq2_add = ones(nc_add, 1);

            else % in other cases, expressed in a general affine form C1*x<=C2
                F_add = Xadd.A;
                nc_add = size(F_add, 1);
                C_ineq2_add = Xadd.b;
            end

            C_ineq1_add = zeros(nc_add, obj.n_opt);
            C_ineq1_add(:, (k_add-1)*obj.sys.nx+1:k_add*obj.sys.nx) = F_add;
        end

    end
end


sigmoid1 = @(x) 1./(1+exp(-x));

kl_tmp=zeros(1,maxiter);
beta_tmp=zeros(1,maxiter);
dm_tmp=zeros(1,maxiter);
iter=0;
dm=1;
eta=eta0;
m=m+1e-13*randn(1,n);
m=max(m,1e-10);
m=min(m,1-1e-10);
m0 = m;
if (beta>=beta_max), beta=beta_max-1; end;
while ((dm>dmmin)&&(iter<maxiter)&&(beta<beta_max))
	iter=iter+1;
	z=(1-m)./m.*diag(C)';
	C1=C+diag(z);
    v=C1\b;
	w1=v./m';
	betaold=beta;
	beta=1/(sigmay-b'*v);

    mold=m;
    m=(1-eta)*m+eta*sigmoid1(gamma+0.5*p*beta*(w1').^2.*diag(C)');
    m=max(m,1e-10);
    m=min(m,1-1e-10);
    dm=max(abs(m-mold));
    kl_tmp(iter)=-p/2*log(beta)+beta*p/2*(v'*C*v+sum((1-m')./m'.*v.^2.*diag(C))-2*b'*v)+beta*p/2*sigmay- gamma*sum(m)+sum(m.*log(m)+(1-m).*log(1-m));
    if iter>1
        dkl =(kl_tmp(iter)-kl_tmp(iter-1));
        if dkl>1e-10,
            eta=eta/2;
            fprintf('\t[%d] dkl=%e eta=%.3e, dm=%.3e\n',iter,kl_tmp(iter)-kl_tmp(iter-1),eta,dm);
            m=mold;
        end
    end
end;
if iter==maxiter, gamma, iter,
	figure(3)
	plot(1:iter,kl_tmp(1:iter));
end;
kl1=-p/2*log(beta)+beta*p/2*(v'*C*v+sum((1-m')./m'.*v.^2.*diag(C))-2*b'*v)+beta*p/2*sigmay- gamma*sum(m)+sum(m.*log(m)+(1-m).*log(1-m));


function [x,y,z,s,w,flag] = squadsolve(Q,c,A,b,C),
%SQUADSOLVE
%
% USAGE:   [x,y,z,s,w,flag] = squadsolve(Q,c,A,b,C)
%
% PARAMETERS:  Q -> (n,n) symetric matrix (definite positive)
%              c -> (n,1) vector
%              A -> (m,n) matrix
%              b -> (m,1) vector
%              C -> scalar
%
%            x -> primal variables
%            y -> lagrangian coeff of equality constraints
%            z -> dual variables of x
%            s -> primal auxiliary variable (only if C < Inf)
%            w -> dual variable of s
%            flag -> set to 0 => no problem, set to 1 => problem
%
% DESCRIPTION: Primal-dual method for quadratic programming
%
%            minimize c'*x + 0.5*x'*Q*x
%
%            subject to  A*x=b
%                        0<= x <= C
%            The method used here is a primal dual method with a predictor-corrector
%            approach and a logarithmic barrier. I used the heuristic from two
%            existing methods LOQO and HOPDM. The method is an iterative method. The
%            maximal number of iteration is stored in the variable 'max_iter'.
%
% ERRORS AND BUGS: there is no test about the conditionning of the matrix Q. If the iteration
%                  50 has been reached, then the optimization may not be finished and the output
%                  may be wrong.
%
% NOTES: 50 iterations have always been sufficient to solve all problems.
%        This code should be read with the tech. report:
%                "Regularized Symmetric Indefinite Systems in Interior Point
%                 Methods for Linear and Quadratic Optimization",
%                 A. Altman and J. Gondzio, Logilab Tech. Report 1998.6
%
% Andre Elisseeff, Dec. 1999
% aelissee@eric.univ-lyon2.fr

 disp('toto');
% init
  verbose = 1;
  n = size(Q,1);
  m = size(A,1);
  H = zeros(n+m,n+m);
  flag=1;

% Values of the original HOPDM of Gondzio and Altmann
  dinf = 10^(-14);
  smallz = 10^(-14);
  smallt = 2.3*10^(-16);
  opttol = 10^(-5)/C;
  itref = 1;
  mu = 1;
  maxiter = 50;

% init values of the primal and dual variables
  x=ones(n,1);
  z=ones(n,1);
  y=ones(m,1);
  if C < Inf,
    s=ones(n,1);
    w=ones(n,1);
  else
    s=[];w=[];
  end;
% Description of variables:
%
%    x,s     -> primal variables
%    z,w     -> dual variables
%    n       -> number of variables in the initial pb (size of x)
%    m       -> number of constraints in A
%
%    dinf     -> smallest value for all variables
%    smallz   -> smallest value of z
%    smallt   -> smallest value for t in the computation of the matrix theta
%    opttol   -> acceptable tolerance for optimality conditions
%    itref    -> iteration counter
%    maxiter  -> maximum number of iteration
%    DEBUG    -> 0 => no debug, 1 => debug

% Analyze the constraints...
  if verbose,
   disp(sprintf('Analyzing the equality constraints...\n'));
  end;
  [QQ,RR]=qr(A',0);
  [mm,nb] = size(QQ); %% number of eq constraints
  ind = 1:1:nb;
  for i=1:nb,
    if abs(RR(i,i)) < 10^(-10),
      disp(sprintf('Constraints %d removed because of dependence\n',i));
      ind(i)=0;
    end;
  end;
  indice = find(ind >0);

  if (isempty(indice))
    disp(sprintf('No equality constraints... \n'));
    A=[];
    m=0;
  else
    A = A(indice,:); %% new independent eq constraints
    b = b(indice);
    y = y(indice);
    m=length(indice);
  end;
  clear QQ;clear RR;
%%%% AE : scale the problem, has been tested and seems to work
%%%% better : more stable...
  if C < Inf,
    u = ones(n,1);
    Q = Q*C;
    b = b/C;
    c = c;
  end;
% init values before looping
  cont = 1;
  objQ = 0.5*x'*Q*x;
% init values of primal and dual objective functions
  pobjo = abs(c'*x+objQ) + 1;
  if C < inf,
    dobjo = abs(b'*y - u'*w - objQ);
  else
    dobjo = abs(b'*y - objQ);
  end;
%%%%%%%%%%%%%%%
%% MAIN LOOP
%%%%%%%%%%%%%%%
  while (cont)&(itref<=maxiter)
  % Compute the primal objective function
    objQ = 0.5*x'*Q*x;
    pobj = c'*x+objQ;
    if (C<Inf)
        if ~isempty(A),
            dobj = b'*y - u'*w - objQ;
        else
            dobj = - u'*w - objQ;
        end;
    else
        if ~isempty(A),
            dobj = b'*y - objQ;
        else
            dobj = - objQ;
        end;
    end;
    dlgap = pobj - dobj;
    dp = abs(pobj)/(abs(pobjo)+1);
    dd = abs(dobj)/(abs(dobjo)+1);
    dobjo = dobj;
    pobjo = pobj;
    if verbose,
        disp(sprintf('%d - pobj : %f - dobj : %f\n',itref,pobj,dobj));
    end;
  % Check if the solution are bounded
    if (dp > 10^6)
      disp(sprintf('Solution not bounded in the primal. Exit.\n'));
      return;
    end;
    if (dd > 10^6)
      disp(sprintf('Solution not bounded in the dual. Exit.\n'));
      return;
    end;

  % test if optimality
    oldgap = dlgap;
    dp = abs(dobj) + 1;
    if ((abs(dlgap)/dp) <= opttol)
      if verbose,
       disp(sprintf('Optimal solution found. Exit.\n'));
      end;
      cont = 0;
      break;
    end;

    dp = dp + abs(pobj);
    T = abs(dlgap)/dp;

  % put the variables away from zero (from HOPDM)
    if (itref <= 3)
      ax = 2*10^(-3);
      az = 10^(-3);
    elseif (T >= 0.8)
      ax = 2*10^(-4);
      az = 10^(-4);
    elseif (T >= 0.1)
      ax = 2*10^(-5);
      az = 10^(-5);
    elseif (T >=0.01)
      ax = 2*10^(-6);
      az = 10^(-6);
    elseif (T>=0.001)
      ax = 2*10^(-7);
      az = 10^(-7);
    elseif (T>=0.0001)
      ax = 2*10^(-8);
      az = 10^(-7);
    elseif (T>=0.00001)
      ax = 2*10^(-9);
      az = 10^(-9);
    else
      ax = T*10^(-5);
      az = ax;
    end;

  % consider only variables that can be changed
    x = x + ax;
    z = z + az;
    if C < Inf
      s = s + ax;
      w = w + az;
    end;
  % Compute the values of xi_b, xi_c and xi_u
    if ~isempty(A),
     xi_b = -A*x + b;
     xi_c = c - A'*y - z + Q*x;
    else
      xi_b = [];
      xi_c = c - z + Q*x;
     end;

    xi_z =   - x.*z;
    if C < Inf,
      xi_c=xi_c + w;
      xi_u = u - x - s;
      xi_w =  - s.*w;
    end;
  % Compute theta = (z/x + w/s)

  % for bounded variables
    if C<Inf,
      dp = x;
      if (max(abs(dp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      dpp= s;
      if (max(abs(dpp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      theta=z./dp + w./dpp;
    end;
  % for unbounded variables
    if C==Inf,
      dp = x;
      if (max(abs(dp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      theta = z./dp;
    end;

  % neglect small elements of theta array

    neglect = find(theta < smallt);
    if ~isempty(neglect)
      theta(neglect)=zeros(size(neglect));
    end;

  % and control large elements of theta

    neglect = find(theta >= 10^8);
    if ~isempty(neglect)
      theta(neglect)=(10^4)*sqrt(theta(neglect));
    end;
  % factorize H = [-Q-theta^(-1)   A^T]
  %               [ A               0 ]

    H = zeros(n+m,n+m);
    H(1:n,1:n) = -Q-diag(theta);
    H(n+1:n+m,1:n) = A;
    H(1:n,n+1:n+m) = A';

  % Compute the predictor step
    if C < Inf,
      f = xi_c-xi_z./x+(xi_w - xi_u.*w)./s ;
      h = xi_b;
    else
      f = xi_c - xi_z./x;
      h = xi_b;
    end;
    delta=H\[f;h];
    dx = delta(1:n);
    dy = delta(n+1:n+m);
    dz = (xi_z-z.*dx)./x;
    if C<Inf,
      ds = xi_u - dx;
      dw = (xi_w-w.*ds)./s;
    end;

 % determine the maximum step size alpha_p (primal) and
 % alpha_d (dual) to stay in feasible region
 % (x,s,z,w must be positive and greater than dinf)
      indz = find(dz<0);
      indx = find(dx<0);
      inds=[];mins=1;
      indw=[];minw=1;
      if C < Inf,
        inds = find(ds<0);
        indw = find(dw<0);
        if ~isempty(inds)
          mins = min(-(s(inds)-dinf)./ds(inds));
        else
         mins = 1;
        end;
        if ~isempty(indw)
          minw = min(-(w(indw)-dinf)./dw(indw));
        else
         minw = 1;
        end;
      end;
      if ~isempty(indx)
        minx = min(-(x(indx)-dinf)./dx(indx));
      else
        minx = 1;
      end;
      apk = min([minx,mins,1]);
      if ~isempty(indz),
        minz = min(-(z(indz)-dinf)./dz(indz));
      else
        minz = 1;
      end;
      adk = min([minw,minz,1]);

      ax = sum(x.*z);
      as = sum((x+apk*dx).*(z+adk*dz));
      az = sum(dx.^2+dz.^2);
      if C < Inf,
        ax = ax + sum(s.*w);
        as = as + sum((s+apk*ds).*(w+adk*dw));
        az = az + sum(ds.^2+dw.^2);
      end;
    % check if complementary gap is less than opttol
      if (as <= opttol)
        if verbose,
            disp(sprintf('Complementary gap is less than %f\n',opttol));
        end;
        cont = 0;
        x = x + apk*dx;
        y = y + adk*dy;
        z = z + adk*dz;
        if C < Inf,
         s = s + apk*ds;
         w = w + adk*dw;
        end;
        break;
      end;

    % Set the barrier parameter : LOQO's heuristic
      ap = min(apk,adk);
      mu = (ax/(2*n))*(0.95*(1/ap) -1)^2/(0.95*(1/ap)+10)^2;

    % Compute the new direction (algo. of Mehrotra) of order 1 (corrector step)
      xi_z =  - x.*z + mu*ones(size(x)) - dx.*dz;
      f = xi_c - xi_z./x;
      if C < Inf
        xi_w = - s.*w + mu*ones(size(s)) - ds.*dw;
        f=f+ xi_w./s - (w.*xi_u)./s;
      end;
      h = xi_b;

      delta=H\[f;h];
      dx = delta(1:n);
      dy = delta(n+1:n+m);
      dz = (xi_z-z.*dx)./x;
      if C<Inf,
        ds = xi_u - dx;
        dw = (xi_w-w.*ds)./s;
      end;

    % determine the maximum step size alpha_p (primal) and
    % alpha_d (dual) to stay in feasible region
    % (x,s,z,w must be positiv)
      indz = find(dz<0);
      indx = find(dx<0);
      if C < Inf,
        inds = find(ds<0);
        indw = find(dw<0);
        if ~isempty(inds)
          mins = min(-s(inds)./ds(inds));
        else
      mins=1;
        end;
        if ~isempty(indw)
          minw = min(-w(indw)./dw(indw));
        else
      minw=1;
        end;
      end;
      if ~isempty(indx)
        minx = min(-x(indx)./dx(indx));
      else
        minx=1;
      end;
      alpha_p = min([minx,mins,1]);
      if ~isempty(indz)
        minz = min(-z(indz)./dz(indz));
      else
        minz=1;
      end;
      alpha_d = min([minw,minz,1]);

    % Compute step factors
      fp =0.9*min(alpha_p,alpha_d);
      fd =0.9*min(alpha_d,alpha_p);
      x = x +fp*dx;
      y = y +fd*dy;
      z= z + fd*dz;
      if C < Inf,
        w = w + fd*dw;
        s = s +fp*ds;
      end;
      itref=itref+1;
  end;
%%%%%%%%%%%%%%%%%%%%%
%%% End of main loop
%%%%%%%%%%%%%%%%%%%%%

    if (cont==0)
        if verbose,
          disp(sprintf('Optimal Solution found after %d iteration.\n',itref));
          disp(sprintf('Value of the objective : %f.\n',pobj));
        end;
      flag=0;
    end;
  % rescale x and y
    if C<Inf,
      x= C*x;
      %y = C*y;
    end;

end;


%gaussian mixture model
%>
%> @param FeatureMatrix: features for all train observations (dimension iNumFeatures x iNumObservations)
%> @param k: number of gaussians
%> @param numMaxIter: maximum number of iterations (stop if not converged before)
%> @param prevState: internal state that can be stored to continue clustering later
%>
%> @retval mu means
%> @retval sigma standard deviations
%> @retval state result containing internal state (if needed)
% ======================================================================
function [mu, sigma, state] = ToolGmm(V, k, numMaxIter, prevState)

    if (nargin < 3)
        numMaxIter  = 1000;
    end
    if (nargin == 4)
        state = prevState;
    else
        % initialize state
        state = initState_I(V, k);
    end

    for j = 1:numMaxIter
        prevState = state;

        % compute weighted gaussian
        p = computeProb_I(V, state);

        % update clusters
        state = updateGaussians_I(V, p, state);

        % if we have converged, break
        if (max(sum(abs(state.m-prevState.m))) <= 1e-20)
            break;
        end
    end

    mu = state.m;
    sigma = state.sigma;
end

function [state] = updateGaussians_I(FeatureMatrix, p, state)

    % number of clusters
    K = size(state.m, 2);

    % update priors
    state.prior = mean(p, 1)';

    for k = 1:K
        s = 0;

        % update means
        state.m(:, k) = FeatureMatrix * p(:, k) / sum(p(:, k));

        % subtract mean
        F = FeatureMatrix - repmat(state.m(:, k), 1, size(FeatureMatrix, 2));

        for n = 1:size(FeatureMatrix, 2)
            s = s + p(n, k) * (F(:, n) * F(:, n)');
        end
        state.sigma(:, :, k) = s / sum(p(:, k));
    end
end

function [p] = computeProb_I(FeatureMatrix, state)

    K = size(state.m, 2);
    p = zeros(size(FeatureMatrix, 2), K);

    % for each cluster
    for k = 1:K
        % subtract mean
        F = FeatureMatrix - repmat(state.m(:, k), 1, size(FeatureMatrix, 2));

        % weighted gaussian
        p(:, k) = 1 / sqrt((2*pi)^size(F, 1) * det(state.sigma(:, :, k))) *...
            exp(-1/2 * sum((F .* (inv(state.sigma(:, :, k)) * F)), 1)');
        p(:, k) = state.prior(k) * p(:, k);
    end

    % norm over clusters
    p = p ./ repmat(sum(p, 2), 1, K);
end

function [state] = initState_I(FeatureMatrix, K)

    %init
    m       = zeros(size(FeatureMatrix, 1), K);
    sigma   = zeros(size(FeatureMatrix, 1), size(FeatureMatrix, 1), K);
    prior   = zeros(1, K);

    % pick random points as cluster means
    mIdx    = round(rand(1, K) * (size(FeatureMatrix, 2)-1)) + 1;

    % assign means etc.
    m       = FeatureMatrix(:, mIdx);
    prior   = ones(1, K) / K;
    sigma   = repmat(cov(FeatureMatrix'), 1, 1, K);

    % write initial state
    state   = struct('m', m, 'sigma', sigma, 'prior', prior);
end


function fh = decomp_reconst_full(im,Nsc,Nor,block,noise,parent,covariance,optim,sig);

% Decompose image into subbands, denoise, and recompose again.
%		fh = decomp_reconst(im,Nsc,Nor,block,noise,parent,covariance,optim,sig);
%       covariance:	 are we considering covariance or just variance?
%       optim:		 for choosing between BLS-GSM (optim = 1) and MAP-GSM (optim = 0)
%       sig:        standard deviation (scalar for uniform noise or matrix for spatially varying noise)
% Version using the Full steerable pyramid (2) (High pass residual
% splitted into orientations).

% JPM, Univ. de Granada, 5/02
% Last Revision: 11/04

if (block(1)/2==floor(block(1)/2))|(block(2)/2==floor(block(2)/2)),
   error('Spatial dimensions of neighborhood must be odd!');
end

if ~exist('parent'),
        parent = 1;
end

if ~exist('covariance'),
        covariance = 1;
end

if ~exist('optim'),
        optim = 1;
end

if ~exist('sig'),
        sig = sqrt(mean(noise.^2));
end

[pyr,pind] = buildFullSFpyr2(im,Nsc,Nor-1);
[pyrN,pind] = buildFullSFpyr2(noise,Nsc,Nor-1);
pyrh = real(pyr);
Nband = size(pind,1)-1;
for nband = 2:Nband, % everything except the low-pass residual
  fprintf('%d % ',round(100*(nband-1)/(Nband-1)))
  aux = pyrBand(pyr, pind, nband);
  auxn = pyrBand(pyrN, pind, nband);
  [Nsy,Nsx] = size(aux);
  prnt = parent & (nband < Nband-Nor);   % has the subband a parent?
  BL = zeros(size(aux,1),size(aux,2),1 + prnt);
  BLn = zeros(size(aux,1),size(aux,2),1 + prnt);
  BL(:,:,1) = aux;
  BLn(:,:,1) = auxn*sqrt(((Nsy-2)*(Nsx-2))/(Nsy*Nsx));     % because we are discarding 2 coefficients on every dimension
  if prnt,
  	aux = pyrBand(pyr, pind, nband+Nor);
    auxn = pyrBand(pyrN, pind, nband+Nor);
    if nband>Nor+1,     % resample 2x2 the parent if not in the high-pass oriented subbands.
	   aux = real(expand(aux,2));
       auxn = real(expand(auxn,2));
    end
  	BL(:,:,2) = aux;
    BLn(:,:,2) = auxn*sqrt(((Nsy-2)*(Nsx-2))/(Nsy*Nsx)); % because we are discarding 2 coefficients on every dimension
  end

  sy2 = mean2(BL(:,:,1).^2);
  sn2 = mean2(BLn(:,:,1).^2);
  if sy2>sn2,
     SNRin = 10*log10((sy2-sn2)/sn2);
  else
     disp('Signal is not detectable in noisy subband');
  end

  % main
  BL = denoi_BLS_GSM_band(BL,block,BLn,prnt,covariance,optim,sig);
  pyrh(pyrBandIndices(pind,nband)) = BL(:)';
end
fh = reconFullSFpyr2(pyrh,pind);


function [l,d,perm] = mchol(A,mu)
% [l,d,perm] = mchol(A,mu)
% Compute the Gill-Murray modified LDL factorization of A,

if nargin < 2
    mu = 1e-12;
end

n = size(A,1);
l = eye(n);
d = zeros(n,1);
perm = 1:n;

for i = 1:n
    c(i,i) = A(i,i);
end

% Compute modification parameters
gamma = max(abs(diag(A)));
xi = max(max(abs(setdiag(A,0))));
delta = mu*max(gamma+xi,1);
if n > 1
    beta = sqrt(max([gamma xi/sqrt(n^2-1) mu]));
else
    beta = sqrt(max([gamma mu]));
end

for j = 1:n

    % Find q that results in Best Permutation with j
    [maxVal maxPos] = max(abs(diag(c(j:end,j:end))));
    q = maxPos+j-1;

    % Permute d,c,l,a
    d([j q]) = d([q j]);
    perm([j q]) = perm([q j]);
    c([j q],:) = c([q j],:);
    c(:,[j q]) = c(:,[q j]);
    l([j q],:) = l([q j],:);
    l(:,[j q]) = l(:,[q j]);
    A([j q],:) = A([q j],:);
    A(:,[j q]) = A(:,[q j]);

    for s = 1:j-1
        l(j,s) = c(j,s)/d(s);
    end
    for i = j+1:n
        c(i,j) = A(i,j) - sum(l(j,1:j-1).*c(i,1:j-1));
    end
    theta = 0;
    if j < n
        theta = max(abs(c(j+1:n,j)));
    end
    d(j) = max([abs(c(j,j)) (theta/beta)^2 delta]);
    if j < n
        for i = j+1:n
            c(i,i) = c(i,i) - (c(i,j)^2)/d(j);
        end
    end
end


% Version 1.000
%
% Code provided by Ruslan Salakhutdinov and Geoff Hinton
%
% Permission is granted for anyone to copy, use, modify, or distribute this
% program and accompanying programs and documents for any purpose, provided
% this copyright notice is retained and prominently displayed, along with
% a note saying that the original programs are available from our
% web page.
% The programs and documents are distributed without any warranty, express or
% implied.  As the programs were written for research purposes only, they have
% not been tested to the degree that would be advisable in any important
% application.  All use of these programs is entirely at the user's own risk.

% This program fine-tunes an autoencoder with backpropagation.
% Weights of the autoencoder are going to be saved in mnist_weights.mat
% and trainig and test reconstruction errors in mnist_error.mat
% You can also set maxepoch, default value is 200 as in our paper.

maxepoch=200;
fprintf(1,'\nFine-tuning deep autoencoder by minimizing cross entropy error. \n');
fprintf(1,'60 batches of 1000 cases each. \n');

load mnistvh
load mnisthp
load mnisthp2
load mnistpo

makebatches;
[numcases numdims numbatches]=size(batchdata);
N=numcases;

%%%% PREINITIALIZE WEIGHTS OF THE AUTOENCODER %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w1=[vishid; hidrecbiases];
w2=[hidpen; penrecbiases];
w3=[hidpen2; penrecbiases2];
w4=[hidtop; toprecbiases];
w5=[hidtop'; topgenbiases];
w6=[hidpen2'; hidgenbiases2];
w7=[hidpen'; hidgenbiases];
w8=[vishid'; visbiases];

%%%%%%%%%% END OF PREINITIALIZATIO OF WEIGHTS  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

l1=size(w1,1)-1;
l2=size(w2,1)-1;
l3=size(w3,1)-1;
l4=size(w4,1)-1;
l5=size(w5,1)-1;
l6=size(w6,1)-1;
l7=size(w7,1)-1;
l8=size(w8,1)-1;
l9=l1;
test_err=[];
train_err=[];

for epoch = 1:maxepoch

%%%%%%%%%%%%%%%%%%%% COMPUTE TRAINING RECONSTRUCTION ERROR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
err=0;
[numcases numdims numbatches]=size(batchdata);
N=numcases;
 for batch = 1:numbatches
  data = [batchdata(:,:,batch)];
  data = [data ones(N,1)];
  w1probs = 1./(1 + exp(-data*w1)); w1probs = [w1probs  ones(N,1)];
  w2probs = 1./(1 + exp(-w1probs*w2)); w2probs = [w2probs ones(N,1)];
  w3probs = 1./(1 + exp(-w2probs*w3)); w3probs = [w3probs  ones(N,1)];
  w4probs = w3probs*w4; w4probs = [w4probs  ones(N,1)];
  w5probs = 1./(1 + exp(-w4probs*w5)); w5probs = [w5probs  ones(N,1)];
  w6probs = 1./(1 + exp(-w5probs*w6)); w6probs = [w6probs  ones(N,1)];
  w7probs = 1./(1 + exp(-w6probs*w7)); w7probs = [w7probs  ones(N,1)];
  dataout = 1./(1 + exp(-w7probs*w8));
  err= err +  1/N*sum(sum( (data(:,1:end-1)-dataout).^2 ));
  end
 train_err(epoch)=err/numbatches;

%%%%%%%%%%%%%% END OF COMPUTING TRAINING RECONSTRUCTION ERROR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%% DISPLAY FIGURE TOP ROW REAL DATA BOTTOM ROW RECONSTRUCTIONS %%%%%%%%%%%%%%%%%%%%%%%%%
fprintf(1,'Displaying in figure 1: Top row - real data, Bottom row -- reconstructions \n');
output=[];
 for ii=1:15
  output = [output data(ii,1:end-1)' dataout(ii,:)'];
 end
   if epoch==1
   close all
   figure('Position',[100,600,1000,200]);
   else
   figure(1)
   end
   mnistdisp(output);
   drawnow;

%%%%%%%%%%%%%%%%%%%% COMPUTE TEST RECONSTRUCTION ERROR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[testnumcases testnumdims testnumbatches]=size(testbatchdata);
N=testnumcases;
err=0;
for batch = 1:testnumbatches
  data = [testbatchdata(:,:,batch)];
  data = [data ones(N,1)];
  w1probs = 1./(1 + exp(-data*w1)); w1probs = [w1probs  ones(N,1)];
  w2probs = 1./(1 + exp(-w1probs*w2)); w2probs = [w2probs ones(N,1)];
  w3probs = 1./(1 + exp(-w2probs*w3)); w3probs = [w3probs  ones(N,1)];
  w4probs = w3probs*w4; w4probs = [w4probs  ones(N,1)];
  w5probs = 1./(1 + exp(-w4probs*w5)); w5probs = [w5probs  ones(N,1)];
  w6probs = 1./(1 + exp(-w5probs*w6)); w6probs = [w6probs  ones(N,1)];
  w7probs = 1./(1 + exp(-w6probs*w7)); w7probs = [w7probs  ones(N,1)];
  dataout = 1./(1 + exp(-w7probs*w8));
  err = err +  1/N*sum(sum( (data(:,1:end-1)-dataout).^2 ));
  end
 test_err(epoch)=err/testnumbatches;
 fprintf(1,'Before epoch %d Train squared error: %6.3f Test squared error: %6.3f \t \t \n',epoch,train_err(epoch),test_err(epoch));

%%%%%%%%%%%%%% END OF COMPUTING TEST RECONSTRUCTION ERROR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 tt=0;
 for batch = 1:numbatches/10
 fprintf(1,'epoch %d batch %d\r',epoch,batch);

%%%%%%%%%%% COMBINE 10 MINIBATCHES INTO 1 LARGER MINIBATCH %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 tt=tt+1;
 data=[];
 for kk=1:10
  data=[data
        batchdata(:,:,(tt-1)*10+kk)];
 end

%%%%%%%%%%%%%%% PERFORM CONJUGATE GRADIENT WITH 3 LINESEARCHES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  max_iter=3;
  VV = [w1(:)' w2(:)' w3(:)' w4(:)' w5(:)' w6(:)' w7(:)' w8(:)']';
  Dim = [l1; l2; l3; l4; l5; l6; l7; l8; l9];

  [X, fX] = minimize(VV,'CG_MNIST',max_iter,Dim,data);

  w1 = reshape(X(1:(l1+1)*l2),l1+1,l2);
  xxx = (l1+1)*l2;
  w2 = reshape(X(xxx+1:xxx+(l2+1)*l3),l2+1,l3);
  xxx = xxx+(l2+1)*l3;
  w3 = reshape(X(xxx+1:xxx+(l3+1)*l4),l3+1,l4);
  xxx = xxx+(l3+1)*l4;
  w4 = reshape(X(xxx+1:xxx+(l4+1)*l5),l4+1,l5);
  xxx = xxx+(l4+1)*l5;
  w5 = reshape(X(xxx+1:xxx+(l5+1)*l6),l5+1,l6);
  xxx = xxx+(l5+1)*l6;
  w6 = reshape(X(xxx+1:xxx+(l6+1)*l7),l6+1,l7);
  xxx = xxx+(l6+1)*l7;
  w7 = reshape(X(xxx+1:xxx+(l7+1)*l8),l7+1,l8);
  xxx = xxx+(l7+1)*l8;
  w8 = reshape(X(xxx+1:xxx+(l8+1)*l9),l8+1,l9);

%%%%%%%%%%%%%%% END OF CONJUGATE GRADIENT WITH 3 LINESEARCHES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 end

 save mnist_weights w1 w2 w3 w4 w5 w6 w7 w8
 save mnist_error test_err train_err;

end


function p=v_mos2pesq(m)
%V_MOS2PESQ convert MOS speech quality scores to PESQ p=(m)
%Inputs:    m  is a matrix of MOS scores
%
%Outputs:   p  is a matrix, the same size as m, of PESQ scores
%
% The PESQ measure is defined in [2]. The mapping function, defined in [3],
% converts raw PESQ scores (which lie in the range -0.5 to 4.5) onto the
% MOS-LQO (Mean Opinion Score - Listening Quality Objective [2]) scale in the
% range 1 to 5. The MOS scale is defined in [1] as
%           5=Excellent, 4=Good, 3=Fair, 2=Poor, 1=Bad.
%
% Refs: [1]	ITU-T. Methods for subjective determination of transmission quality.
%           Recommendation P.800, Aug. 1996.
%       [2]	ITU-T. Mean opinion score (MOS) terminology.
%           Recommendation P.800.1, July 2006.
%       [2]	ITU-T. Perceptual evaluation of speech quality (PESQ), an objective
%           method for end-to-end speech quality assessment of narrowband telephone
%           networks and speech codecs. Recommendation P.862, Feb. 2001.
%       [3]	ITU-T. Mapping function for transforming P.862 raw result scores to MOS-LQO.
%           Recommendation P.862.1, Nov. 2003.

%      Copyright (C) Mike Brookes 2012-2013
%      Version: $Id: v_mos2pesq.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b c d
if isempty(a)
    a=0.999;
    b=4.999-a;
    c=-1.4945;
    d=4.6607;
end
if nargout>0
    p=(log(b./(m-a)-1)-d)/c;
else
    if nargin<1 || isempty(m)
        pp=linspace(-0.5,4.5,100);
        mm=v_pesq2mos(pp);
    else
        mm=m;
    end
    p=v_mos2pesq(mm);
    plot(mm,p);
    ylabel('PESQ (P.862)');
    xlabel('Mean Opimion Score (MOS)');
end


function [x, infos] = snpa(V, num_col, in_options)
% Successive Nonnegative Projection Algorithm (variant with f(.) = ||.||^2)
%
%       At each step of the algorithm, the column of X maximizing ||.||_2 is
%       extracted, and X is updated with the residual of the projection of its
%       columns onto the convex hull of the columns extracted so far.
%
% Inputs:
%       matrix      V
%       num_col     number of columns to be extracted.
%       options     options
%           normalization: 1: scale the columns of X so that they sum to one,
%                           hence matrix H will satisfy the assumption above for any
%                           nonnegative separable matrix X.
%                          0: the default value for which no scaling is
%                           performed. For example, in hyperspectral imaging, this
%                           assumption is already satisfied and normalization is not
%                           necessary.
%
% Output:
%       w           solution of w
%           K        : index set of the extracted columns.
%           H        : optimal weights, that is, H = argmin_{Y >= 0} ||X-X(:,K)Y||_F
%       infos       information
%
% References:
%       N. Gillis,
%       "Successive Nonnegative Projection Algorithm for Robust Nonnegative Blind Source Separation,"
%       SIAM J. on Imaging Sciences 7 (2),
%       pp. 1420-1450, 2014.
%
%
% This file is part of NMFLibrary.
%
% This file has been ported from
%   SNPA.m at https://gitlab.com/ngillis/nmfbook/-/tree/master/algorithms
%   by Nicolas Gillis (nicolas.gillis@umons.ac.be)
%
% Change log:
%
%   June. 21, 2022 (Hiroyuki Kasai): Added initialization module.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.disp_freq = 1;
    local_options.normalize = 0;
    local_options.relerr = 1e-6;
    local_options.inner_max_epoch = 500;
    local_options.inner_nnls_alg = 'fpgm';
    local_options.special_stop_condition = @(epoch, infos, options, stop_options) spna_stop_func(epoch, infos, options, stop_options);

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize
    method_name = 'SNPA';
    i = 0;
    grad_calc_count = 0;
    stop_options = [];

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % initialize for this algorithm
    options.max_epoch = num_col+1;
    if options.normalize == 1
        % normalize the columns of V of which colum is L1-norm = 1
        V = normalize_W(V, 1);
    end

    U = zeros(m, num_col);
    K = zeros(1, num_col);
    H = zeros(num_col, n);
    normV0 = sum(V.^2);
    nVmax = max(normV0);
    normR = normV0;
    VtUK = [];
    UKtUK = [];

    % set for nnls subsolver
    nnls_options = [];
    nnls_options.verbose = 0;
    nnls_options.inner_max_epoch = options.inner_max_epoch;
    nnls_options.algo = options.inner_nnls_alg;

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, eye(m), V, [], options, [], i, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    i = i + 1;
    while true

        % check stop condition
        stop_options.normR = normR;
        stop_options.nVmax = nVmax;
        [stop_flag, reason, max_reached_flag] = check_stop_condition(i, infos, options, stop_options);
        if stop_flag
            display_stop_reason(i, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        % select the column of the residual R with largest l2-norm
        [a, ~] = max(normR);

        % check ties up to 1e-6 precision
        b = find((a-normR)/a <= 1e-6);

        % In case of a tie, select column with largest norm of the input matrix X
        if length(b) > 1
            [~, d] = max(normX0(b));
            b = b(d);
        end

        % update the index set, and extracted column
        K(i) = b;
        U(:, i) = V(:, b);

        % update MtUJ
        VtUK = [VtUK, V' * U(:, i)];

        % update UJtUJ
        if i == 1
            UtUi = [];
        else
            UtUi = U(:, 1:i-1)' * U(:, i);
        end
        UKtUK = [UKtUK, UtUi ; UtUi', U(:, i)' * U(:, i)];

        % update residual
        if i == 1
            % Fast gradient method for min_{y in Delta} ||M(:, i)-M(:,J)y||
            [H, ~, ~, ~] = nnls_solver(V, V(:, K(1:i)), nnls_options);
        else
            H(:, K(i)) = 0;
            h = zeros(1,n); h(K(i)) = 1;
            H = [H; h];
            nnls_options.init = H;
            [H, ~, ~, ~] = nnls_solver(V, V(:, K(1:i)), nnls_options);
        end

        % update the norm of the columns of the residual without computing it explicitely.
        if i == 1
            normR = normV0 - 2 * ((VtUK') .* H) + (H .* (UKtUK*H));
        else
            normR = normV0 - 2 * sum((VtUK') .* H) + sum(H .* (UKtUK*H));
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % update epoch
        i = i + 1;

        % store info
        infos = store_nmf_info(V, U(:, 1:i-1), H, [], options, infos, i-1, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, i-1, infos, options);

    end

    x.K = K;
    x.U = U;
    x.H = H;

end

function [stop_flag, reason, rev_infos] = spna_stop_func(epoch, infos, options, stop_options)

    stop_flag = false;
    reason = [];
    rev_infos = [];

    normR = stop_options.normR;
    nVmax = stop_options.nVmax;

    if sqrt(max(normR)/nVmax) < options.relerr
        stop_flag = true;
        reason = sprintf('precision reached: sqrt(max(normR)/nVmax) = %.4e < options.relerr = %.4e\n', sqrt(max(normR)/nVmax), options.relerr);
        return;
    end

end


function days = ymd_dif_common ( y1, m1, d1, y2, m2, d2 )

%*****************************************************************************80
%
%% YMD_DIF_COMMON gets the day difference between two Common YMD dates.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y1, M1, D1, the first YMD date.
%
%    Input, integer Y2, M2, D2, the second YMD date.
%
%    Output, integer DAYS, the number of days between the dates.
%
  days = 0;
%
%  Check the dates.
%
  [ y1, m1, d1, ierror ] = ymd_check_common ( y1, m1, d1 );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'YMD_DIF_COMMON - Fatal error!\n' );
    fprintf ( 1, '  Y1/M1/D1 is illegal.\n' );
    error ( 'YMD_DIF_COMMON - Fatal error!' );
  end

  [ y2, m2, d2, ierror ] = ymd_check_common ( y2, m2, d2 );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'YMD_DIF_COMMON - Fatal error!\n' );
    fprintf ( 1, '  Y2/M2/D2 is illegal.\n' );
    error ( 'YMD_DIF_COMMON - Fatal error!' );
  end

  jed1 = ymd_to_jed_common ( y1, m1, d1 );

  jed2 = ymd_to_jed_common ( y2, m2, d2 );

  days = round ( jed2 - jed1 );

  return
end


function [disp_row, disp_col, sind] = resp_newton(response, responsef, iterations, ky, kx, use_sz)

[max_resp_row, max_row] = max(response, [], 1);
[init_max_response, max_col] = max(max_resp_row, [], 2);
max_row_perm = permute(max_row, [2 3 1]);
col = max_col(:)';
row = max_row_perm(sub2ind(size(max_row_perm), col, 1:size(response,3)));

trans_row = mod(row - 1 + floor((use_sz(1)-1)/2), use_sz(1)) - floor((use_sz(1)-1)/2);
trans_col = mod(col - 1 + floor((use_sz(2)-1)/2), use_sz(2)) - floor((use_sz(2)-1)/2);
init_pos_y = permute(2*pi * trans_row / use_sz(1), [1 3 2]);
init_pos_x = permute(2*pi * trans_col / use_sz(2), [1 3 2]);
max_pos_y = init_pos_y;
max_pos_x = init_pos_x;

% pre-compute complex exponential
exp_iky = exp(bsxfun(@times, 1i * ky, max_pos_y));
exp_ikx = exp(bsxfun(@times, 1i * kx, max_pos_x));

% gradient_step_size = gradient_step_size / prod(use_sz);

ky2 = ky.*ky;
kx2 = kx.*kx;

iter = 1;
while iter <= iterations
    % Compute gradient
    ky_exp_ky = bsxfun(@times, ky, exp_iky);
    kx_exp_kx = bsxfun(@times, kx, exp_ikx);
    y_resp = mtimesx(exp_iky, responsef, 'speed');
    resp_x = mtimesx(responsef, exp_ikx, 'speed');
    grad_y = -imag(mtimesx(ky_exp_ky, resp_x, 'speed'));
    grad_x = -imag(mtimesx(y_resp, kx_exp_kx, 'speed'));
    ival = 1i * mtimesx(exp_iky, resp_x, 'speed');
    H_yy = real(-mtimesx(bsxfun(@times, ky2, exp_iky), resp_x, 'speed') + ival);
    H_xx = real(-mtimesx(y_resp, bsxfun(@times, kx2, exp_ikx), 'speed') + ival);
    H_xy = real(-mtimesx(ky_exp_ky, mtimesx(responsef, kx_exp_kx, 'speed'), 'speed'));
    det_H = H_yy .* H_xx - H_xy .* H_xy;

    % Compute new position using newtons method
    max_pos_y = max_pos_y - (H_xx .* grad_y - H_xy .* grad_x) ./ det_H;
    max_pos_x = max_pos_x - (H_yy .* grad_x - H_xy .* grad_y) ./ det_H;

    % Evaluate maximum
    exp_iky = exp(bsxfun(@times, 1i * ky, max_pos_y));
    exp_ikx = exp(bsxfun(@times, 1i * kx, max_pos_x));

    iter = iter + 1;
end
max_response = 1 / prod(use_sz) * real(mtimesx(mtimesx(exp_iky, responsef, 'speed'), exp_ikx, 'speed'));

% check for scales that have not increased in score
ind = max_response < init_max_response;
max_response(ind) = init_max_response(ind);
max_pos_y(ind) = init_pos_y(ind);
max_pos_x(ind) = init_pos_x(ind);

[max_scale_response, sind] = max(max_response(:));
disp_row = (mod(max_pos_y(1,1,sind) + pi, 2*pi) - pi) / (2*pi) * use_sz(1);
disp_col = (mod(max_pos_x(1,1,sind) + pi, 2*pi) - pi) / (2*pi) * use_sz(2);
end


function [all] = VBA_classification(X,y,k,verbose,options,sparse)
% performs binary classification using VBA
% function [pv,stat,df,all] = VBA_classification(X,y,k,verbose,options)
% In brief, this function fits the following logistic regresion model:
%   p(y=1) = s(X*beta)
% where y is the binary data, s is the standard sigmoid mapping, X is the
% design matrix and beta are unknown weight parameters.
% Statistical testing is performed using cross-validation scheme, such that
% the p-value computes the probability of the test score under the null H0.
% In cross-validation, the test score is the binary accuracy outcome
% sampled across test sets. The null distribution accounts for potential
% imbalance in the data in that the first-order moment of the corresponding
% binomial distribution is specified in terms of the sample mean of the
% data. VBA_classification uses a k-fold cross-validation scheme, which
% involves partitioning the data into k subsamples of equal size, each of
% which is used as the test set in turn.
% Note: VBA_classification also performs a fully Bayesian model inversion
% on the entire dataset (full-data inversion).
% IN:
%   - X: nXp design matrix
%   - y: nX1 binary data matrix
%   - k: the number of folds for the k-fold cross-validation scheme. If k
%   is set to n (default), then VBA_classification uses a leave-one-out
%   scheme. If k is set to 0, then the cross-validation scheme is entirely
%   skipped (only the full-data inversion is performed).
%   - verbose: flag for displaying results (default is 0)
%   - options: structure containing VBA's options (can be used for passing,
%   e.g., priors on classification weight parameters)
%   - sparse: when sparse=1, VBA_classification uses sparsifying priors
%   (default=0).
% OUT:
%   - all: structure array with fields:
%       .stat: structure of summary statistics:
%           .pv: classical p-value on classifier accuracy
%           .success: nx1 vector of successful classifications
%           .pa: cross-validation prediction accuracy
%           .bpa: balanced prediction accuracy
%           .pBayes: Bayesian exceedance prob. on prediction accuracy
%       .P: pxk matrix of estimated classification weights
%       .r: data imbalance (r=0.5 means balanced data)
%       .in: structure storing the inputs to VBA_classification
%       .date: date stamp
%       .dt: computing time
%       .handles: a structure containing the handles of the graphical
%       objects (filled-in by VBA_classication_display.m)
% Note: cross-validation results can be displayed using the following
% command line: VBA_classification_display(all).

% fill in default I/O
tStart = tic;
all = [];
[n,m] = size(y);
[n0,p] = size(X);

try,k;catch,k=n;end
try,verbose;catch,verbose=0;end
try % check whether VBA's options structure has been sepecified
    options;
catch
    options = [];
end
try % use sparsify tansform?
    sparse = ~~sparse(1);
catch
    sparse = 0;
end

% check basic numerical requirements
try
    if VBA_isWeird (y)
        disp('Error: data contains weird values!')
        return
    end
end
if ~ VBA_isBinary (y)
    disp('Error: data should be binary!')
    return
end
if ~isequal(n,n0)
    disp('Error: design matrix has to have as many rows as the data matrix!')
    return
end
if ~isequal(m,1)
    disp('Error: data should be a nx1 vector!')
    return
end
if k>n || k<0
    disp('Warning: number of folds reduced to data length!')
    k = n;
end

% randomly re-order the data (for k-fold partitioning)
if k ~= n
    io = randperm(n);
    y = y(io);
    X = X(io,:);
end

% specify options and priors for VBA inversion
dim.p = n;
dim.n_t = 1;
dim.n_phi = p;
dim.n_theta = 0;
dim.n = 0;
g_fname = @g_classif0;
options.sources = struct('type',1,'out',1);
options.DisplayWin = 0;
options.verbose = 0;
options.inG.X = X';
options.inG.sparse = sparse;
options.n0 = 0; % number of dummy counts

if ~isequal(k,0) % performing cross-validation scheme
    if verbose
        disp(' ')
        fprintf(1,'Performing k-folds cross-validation scheme...')
        fprintf(1,'%6.2f %%',0)
        et0 = clock; % get time
    end
    sizeFolds = floor(n./k);
    acc = zeros(n,1); % test accuracy
    acc0 = zeros(n,1); % training accuracy
    Eg = zeros(n,1); % out-of-sample prediction E[y] (on test data)
    Vg = zeros(n,1); % out-of-sample prediction V[y] (on test data)
    P = zeros(p,k);
    for i=1:k
        if i<k
            itest = (i-1)*sizeFolds+1:i*sizeFolds;
        else % last test fold contains all remaining data
            itest = (i-1)*sizeFolds+1:n;
        end
        options.isYout = zeros(n,1);
        options.isYout(itest) = 1;
        [posterior,out] = VBA_NLStateSpaceModel(y,[],[],g_fname,dim,options);
        Eg(itest) = out.suffStat.gx(itest);
        Vg(itest) = out.suffStat.vy(itest);
        ytest = Eg(itest)>=0.5;
        acc(itest) = [y(itest)==ytest];
        acc0(itest) = out.fit.acc;
        if sparse
            P(:,i) = VBA_sparsifyPrior (posterior.muPhi);
        else
            P(:,i) = posterior.muPhi;
        end
        if verbose
            fprintf(1,repmat('\b',1,8))
            fprintf(1,'%6.2f %%',floor(100*i/k))
        end
    end
    if verbose
        fprintf(1,repmat('\b',1,8))
        fprintf(1,[' OK (took ',num2str(etime(clock,et0)),' seconds).'])
        fprintf(1,'\n')
    end
end

% performing full-data inversion
if verbose
    fprintf(1,'Performing whole-data inversion...')
    et0 = clock; % get time
end
options.isYout = zeros(n,1);
[posterior,out] = VBA_NLStateSpaceModel(y,[],[],g_fname,dim,options);
if verbose
    fprintf(1,[' OK (took ',num2str(etime(clock,et0)),' seconds).'])
    fprintf(1,'\n')
end

% wrap-up
all.in.X = X;
all.in.y = y;
all.in.k = k;
all.in.sparse = sparse;
all.date = clock;
all.dt = toc(tStart);
all.posterior = posterior;
all.out = out;
if ~isequal(k,0)
    r = mean(y);
    if r<0.5
        r = 1-r;
    end
    nok = sum(acc);
    [pdf0,cdf0] = VBA_binomial(nok,n,r);
    all.stat.pv = 1 - cdf0; % classical p-value on classifier accuracy
    all.stat.success = acc; % nx1 vector of successful classifications
    all.stat.pa = nok./n; % cross-validation prediction accuracy
    all.stat.bpa = 0.5*(sum(acc.*y)./sum(y) + sum(acc.*(1-y))./sum(1-y)); % balanced class. acc.
    all.stat.pBayes = VBA_PPM(nok+options.n0,n-nok+options.n0,r,'beta',0); % Bayesian exceedance prob.
    all.Eg = Eg;
    all.Vg = Vg;
    all.P = P; % set of estimated weights (for each k-fold)
    all.r = r; % data imbalance (r=0.5 means balanced data)
    if verbose
        [all] = VBA_classification_display(all);
    end
end


%% This file is the PDE simulation file of Belousov-Zhabotinsky reaction.
% Coded By: K
% Last Updated: 2019/06/24
%%
function [rhs,x_t,z_t,s_t,u_t,x,z,s,u,x_x,z_x,s_x,u_x,x_y,z_y,s_y,u_y,x_xx,z_xx,s_xx,u_xx,x_yy,z_yy,s_yy,u_yy,x_lap,z_lap,s_lap,u_lap]=...
    BZ_Reaction_PDE(t,xzsut,Kx,Kxx,Ky,Kyy,K22,n,N,Dx,Dz,Ds,Du,q,f,ksi,alpha,beta,gama,ksi2,ksi3,phi,NeedDev)

% Calculate u and v terms
xt=reshape((xzsut(1:N)),n,n);
zt=reshape((xzsut((N+1):(2*N))),n,n);
st=reshape((xzsut(2*N+1:3*N)),n,n);
ut=reshape((xzsut((3*N+1):(4*N))),n,n);

x=real(ifft2(xt));
z=real(ifft2(zt));
s=real(ifft2(st));
u=real(ifft2(ut));

% Reaction Terms
xtrhs=reshape((fft2(  (1/ksi)*( f.*z.*(q-x)./(q+x) + x - x.^2 - beta*x + s )  )),N,1);
ztrhs=reshape((fft2(  x - z - alpha*z + gama*u  )),N,1);
strhs=reshape((fft2(  (1/ksi2)*(beta*x - s + phi*u )  )),N,1);
utrhs=reshape((fft2(  (1/ksi3)*(alpha*z - gama*u )  )),N,1);

rhs=[-(Dx/Dx)*K22.*xzsut(1:N)+xtrhs
     -(Dz/Du)*K22.*xzsut(N+1:2*N)+ztrhs
     -(Ds/Du)*K22.*xzsut(2*N+1:3*N)+strhs
     -(Du/Du)*K22.*xzsut(3*N+1:4*N)+utrhs
     ];

 % If you don't need to extract the value, don't run the following code to
 % speed up
if NeedDev==1
    % Get the derivative you want
    x_x=real(ifft2(reshape((1j*Kx).*xzsut(1:N),n,n)));
    z_x=real(ifft2(reshape((1j*Kx).*xzsut(N+1:2*N),n,n)));
    s_x=real(ifft2(reshape((1j*Kx).*xzsut(2*N+1:3*N),n,n)));
    u_x=real(ifft2(reshape((1j*Kx).*xzsut(3*N+1:4*N),n,n)));
    %
    x_y=real(ifft2(reshape((1j*Ky).*xzsut(1:N),n,n)));
    z_y=real(ifft2(reshape((1j*Ky).*xzsut(N+1:2*N),n,n)));
    s_y=real(ifft2(reshape((1j*Ky).*xzsut(2*N+1:3*N),n,n)));
    u_y=real(ifft2(reshape((1j*Ky).*xzsut(3*N+1:4*N),n,n)));
    %
    x_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(1:N),n,n)));
    z_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(N+1:2*N),n,n)));
    s_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(2*N+1:3*N),n,n)));
    u_xx=real(ifft2(reshape((1j*Kxx).^2.*xzsut(3*N+1:4*N),n,n)));
    %
    x_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(1:N),n,n)));
    z_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(N+1:2*N),n,n)));
    s_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(2*N+1:3*N),n,n)));
    u_yy=real(ifft2(reshape((1j*Kyy).^2.*xzsut(3*N+1:4*N),n,n)));
    %
    x_t=real(ifft2(reshape(rhs(1:N),n,n)));
    z_t=real(ifft2(reshape(rhs(N+1:2*N),n,n)));
    s_t=real(ifft2(reshape(rhs(2*N+1:3*N),n,n)));
    u_t=real(ifft2(reshape(rhs(3*N+1:4*N),n,n)));
    %
    x_lap=real(ifft2(reshape(-K22.*xzsut(1:N),n,n)));
    z_lap=real(ifft2(reshape(-K22.*xzsut(N+1:2*N),n,n)));
    s_lap=real(ifft2(reshape(-K22.*xzsut(2*N+1:3*N),n,n)));
    u_lap=real(ifft2(reshape(-K22.*xzsut(3*N+1:4*N),n,n)));

end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: midpoint quadrature rule for 2D spline
%
%==============================================================================

clear, close all, help(mfilename)

omega = [0,6,0,8]; I = 36; T = zeros(6,8); T(3,4) = 1; h = []; Q = [];
psi = @(xc) splineInter(T,omega,xc);
for j=1:10,
  m    = 2^j*[1,1];
  h(j) = prod((omega(2:2:end)-omega(1:2:end))./m);
  xc   = getCellCenteredGrid(omega,m);
  Q(j) = h(j)*sum(psi(xc));
end;
figure(1); clf; p1=semilogx(h/h(1),Q+eps,'kx',h/h(1),Q,'k-');
figure(2); clf; p2=loglog(h/h(1),abs(I-Q)+eps,'kx',h/h(1),abs(I-Q),'k-');
%==============================================================================