function [vert,conn,tria,tnum] = smooth2(varargin)
%SMOOTH2 "hill-climbing" mesh-smoothing for two-dimensional,
%2-simplex triangulations.
%   [VERT,EDGE,TRIA,TNUM] = SMOOTH2(VERT,EDGE,TRIA,TNUM) re-
%   turns a "smoothed" triangulation {VERT,TRIA}, incorpora-
%   ting "optimised" vertex coordinates and mesh topology.
%
%   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.
%
%   [VERT,EDGE,TRIA,TNUM] = SMOOTH2(... ,OPTS) passes an ad-
%   ditional options structure OPTS, containing user-defined
%   parameters, including:
%
% - OPTS.VTOL = {+1.0E-02} -- relative vertex movement tole-
%   rance, smoothing is converged when (VNEW-VERT) <= VTOL *
%   VLEN, where VLEN is a local effective length-scale.
%
% - OPTS.ITER = {+32} -- max. number of smoothing iterations
%
% - OPTS.DISP = {+ 4} -- smoothing verbosity. Set to INF for
%   quiet execution.
%
%   See also REFINE2, TRICOST, TRIDEMO

%   This routine is loosely based on the DISTMESH algorithm,
%   employing a "spring-based" analogy to redistribute mesh
%   vertices. Such an approach is described in: P.O. Persson
%   and Gilbert Strang. "A simple mesh generator in MATLAB."
%   SIAM review 46(2) 2004, pp: 329--345. Details of the al-
%   gorithm used here are somewhat different, with an alter-
%   ative spring-based update employed, in addition to hill-
%   climbing element quality guarantees, and vertex density
%   controls.

%-----------------------------------------------------------
%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 21/07/2017
%-----------------------------------------------------------

    vert = []; conn = []; tria = [] ;
    tnum = [];
    opts = []; hfun = []; harg = {} ;

%---------------------------------------------- extract args
    if (nargin>=+1), vert = varargin{1}; end
    if (nargin>=+2), conn = varargin{2}; end
    if (nargin>=+3), tria = varargin{3}; end
    if (nargin>=+4), tnum = varargin{4}; end
    if (nargin>=+5), opts = varargin{5}; end

   [opts] = makeopt(opts) ;

%---------------------------------------------- default CONN
    if (isempty(conn))

       [edge] = tricon2(tria);

        ebnd = edge(:,4) < +1;              %-- use bnd edge
        conn = edge(ebnd,1:2);

    end

%---------------------------------------------- default TNUM
    if (isempty(tnum))
        tnum = ones(size(tria, 1), 1) ;
    end

%---------------------------------------------- basic checks
    if ( ~isnumeric(vert) || ...
         ~isnumeric(conn) || ...
         ~isnumeric(tria) || ...
         ~isnumeric(tnum) || ...
         ~isstruct (opts) )
        error('smooth2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(vert) ~= +2 || ...
        ndims(conn) ~= +2 || ...
        ndims(tria) ~= +2 || ...
        ndims(tnum) ~= +2 )
        error('smooth2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    if (size(vert,2)~= +2 || ...
        size(conn,2)~= +2 || ...
        size(tria,2)~= +3 || ...
        size(tnum,2)~= +1 || ...
        size(tria,1)~= size(tnum,1) )
        error('smooth2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    nvrt = size(vert,1) ;

%---------------------------------------------- basic checks
    if (min(min(conn(:,1:2))) < +1 || ...
            max(max(conn(:,1:2))) > nvrt )
        error('smooth2:invalidInputs', ...
            'Invalid EDGE input array.') ;
    end

    if (min(min(tria(:,1:3))) < +1 || ...
            max(max(tria(:,1:3))) > nvrt )
        error('smooth2:invalidInputs', ...
            'Invalid TRIA input array.') ;
    end

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

%---------------------------------------------- polygon bnds
    node = vert; PSLG = conn; part = {};

    pmax = max(tnum(:));
    for ppos = +1 : pmax

        tsel = tnum == ppos ;
        tcur = tria(tsel,:) ;

       [ecur,tcur] ...
           = tricon2 (tcur) ;

        ebnd = ecur(:,4)==0 ;

        same = setset2( ...
            PSLG,ecur(ebnd,1:2));

        part{ppos} = find(same) ;

    end

%---------------------------------------------- inflate bbox
    vmin = min(vert,[],1);
    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] ;

%---------------------------------------------- DO MESH ITER
    tnow =  tic ;

    tcpu = struct('full',0.,'dtri',0., ...
        'tcon',0.,'iter',0.,'undo',0., ...
        'keep',0.) ;

    for iter = +1 : opts.iter

    %------------------------------------------ inflate adj.
        ttic = tic ;

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

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

    %------------------------------------------ compute scr.
        oscr = triscr2(vert,tria) ;

    %------------------------------------------ vert. iter's
        ttic =  tic ;

        nvrt = size(vert,1);
        nedg = size(edge,1);

        IMAT = sparse( ...
           edge(:,1),(1:nedg)',+1,nvrt,nedg) ;
        JMAT = sparse( ...
           edge(:,2),(1:nedg)',+1,nvrt,nedg) ;

        EMAT = IMAT + JMAT ;

        vdeg = sum(EMAT,2) ;                %-- vertex |deg|
        free = (vdeg == 0) ;

        vold = vert ;
        for isub = +1 : max(+2,min(+8,iter))

        %-- compute HFUN at vert/midpoints
            hvrt = evalhfn(vert, ...
                edge,EMAT,hfun,harg) ;

            hmid = hvrt(edge(:,1),:) ...
                 + hvrt(edge(:,2),:) ;
            hmid = hmid * +.5 ;

        %-- calc. relative edge extensions
            evec = vert(edge(:,2),:) ...
                 - vert(edge(:,1),:) ;
            elen = ...
                sqrt(sum(evec.^2,2)) ;

            scal = +1.0-elen./hmid ;
            scal = min (+1.0, scal);
            scal = max (-1.0, scal);

        %-- projected points from each end
            ipos = vert(edge(:,1),:) ...
                 -.67*[scal,scal].*evec;
            jpos = vert(edge(:,2),:) ...
                 +.67*[scal,scal].*evec;

           %scal = ...                      %-- nlin. weight
           %   max(abs(scal).^.5,eps^.75);
            scal = ...
               max(abs(scal).^ 1,eps^.75);

        %-- sum contributions edge-to-vert
            vnew = ...
            IMAT*([scal,scal] .* ipos) ...
          + JMAT*([scal,scal] .* jpos) ;

            vsum = max(EMAT*scal,eps^.75);

            vnew = vnew ./ [vsum,vsum] ;

        %-- fixed points. edge projection?
            vnew(conn(:),1:2) = ...
            vert(conn(:),1:2) ;

            vnew(vdeg==0,1:2) = ...
            vert(vdeg==0,1:2) ;

        %-- reset for the next local iter.
            vert = vnew ;

        end

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

    %------------------------------------------ hill-climber
        ttic = tic ;

    %-- unwind vert. upadte if score lower
        nscr = ones(size(tria,1),1);
        btri = true(size(tria,1),1);

        umax = + 8 ;

        for undo = +1 : umax

            nscr(btri) = triscr2( ...
                vert,tria(btri,:)) ;

        %-- TRUE if tria needs "unwinding"
            smin = +.70 ;
            smax = +.90 ;
            sdel = .025 ;

            stol = smin+iter*sdel;
            stol = min (smax,stol) ;

            btri = nscr <= stol ...
                 & nscr <  oscr ;

            if (~any(btri)), break; end

        %-- relax toward old vert. coord's
            ivrt = ...
               unique(tria(btri,1:3));

            bvrt = ...
               false(size(vert,1),1) ;
            bvrt(ivrt) = true;

            if (undo ~= umax)
                bnew =  +.75 ^ undo ;
                bold =  +1.0 - bnew ;
            else
                bnew =  +0.0 ;
                bold =  +1.0 - bnew ;
            end

            vert(bvrt,:) = ...
                bold * vold(bvrt,:) ...
              + bnew * vert(bvrt,:) ;

            btri = any( ...
               bvrt(tria(:,1:3)),2) ;

        end

        oscr = nscr ;

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

    %------------------------------------- test convergence!
        ttic = tic ;

        vdel = ...
            sum((vert-vold).^2,2) ;

        evec = vert(edge(:,2),:) ...
             - vert(edge(:,1),:) ;
        elen = ...
            sqrt(sum(evec.^2,2)) ;

        hvrt = evalhfn(vert, ...
            edge,EMAT,hfun,harg) ;

        hmid = hvrt(edge(:,1),:) ...
             + hvrt(edge(:,2),:) ;
        hmid = hmid * 0.5 ;
        scal = elen./hmid ;

        emid = vert(edge(:,1),:) ...
             + vert(edge(:,2),:) ;
        emid = emid * 0.5 ;

    %------------------------------------- |deg|-based prune
        keep = false(size(vert,1),1);
        keep(vdeg>+4) = true ;

        keep(conn(:)) = true ;
        keep(free(:)) = true ;

    %------------------------------------- 'density' control
        lmax = +5. / +4.  ;
        lmin = +1. / lmax ;

        less = scal<=lmin ;
    	more = scal>=lmax ;

        vbnd = false(size(vert,1),1);
        vbnd(conn(:,1)) = true ;
        vbnd(conn(:,2)) = true ;

        ebad = vbnd(edge(:,1)) ...     %-- not at boundaries
             | vbnd(edge(:,2)) ;

        less(ebad(:)) = false;
        more(ebad(:)) = false;

    %------------------------------------- force as disjoint
        lidx = find (less) ;

        for lpos = 1 : length(lidx)
            epos = lidx(lpos,1);
            inod = edge(epos,1);
            jnod = edge(epos,2);
        %--------------------------------- if still disjoint
            if (keep(inod) && ...
                keep(jnod) )

            keep(inod) = false ;
            keep(jnod) = false ;

            else

            less(epos) = false ;

            end
        end

        ebad = ...
           keep(edge(less,1)) ...
         & keep(edge(less,2)) ;

        more(ebad(:)) = false ;

    %------------------------------------- reindex vert/tria
        redo = ...
            zeros(size(vert,1),1);
        itop = ...
            length(find(keep));
        iend = ...
            length(find(less));

        redo(keep) = (1:itop)';

        redo(edge(less,1)) = ...        %-- to new midpoints
            (itop+1 : itop+iend)';
        redo(edge(less,2)) = ...
            (itop+1 : itop+iend)';

        vnew =[vert(keep,:) ;
               emid(less,:) ;
              ] ;
        tnew = redo(tria(:,1:3)) ;

        ttmp = sort(tnew,2) ;           %-- filter collapsed
        okay = all( ...
            diff(ttmp,1,2)~=0,2) ;
        okay = ...
            okay & ttmp(:,1) > 0 ;
        tnew = tnew(okay,:) ;

    %------------------------------------- quality preserver
        nscr = ...
            triscr2  (vnew,tnew) ;

        stol = +0.80 ;

        tbad = nscr < stol ...
             & nscr < oscr(okay) ;

        vbad = ...
            false(size(vnew,1),1);
        vbad(tnew(tbad,:)) = true;

    %------------------------------------- filter edge merge
        lidx = find (less) ;

        ebad = ...
        vbad(redo(edge(lidx,1))) | ...
        vbad(redo(edge(lidx,2))) ;

        less(lidx(ebad)) = false ;

        keep(edge(...
        lidx(ebad),1:2)) =  true ;

    %------------------------------------- reindex vert/conn
        redo = ...
            zeros(size(vert,1),1);
        itop = ...
            length(find(keep));
        iend = ...
            length(find(less));

        redo(keep) = (1:itop)';

        redo(edge(less,1)) = ...
            (itop+1 : itop+iend)';
        redo(edge(less,2)) = ...
            (itop+1 : itop+iend)';

        vert =[vert(keep,:);
               emid(less,:);
               emid(more,:);
              ] ;
        conn = redo(conn(:,1:2)) ;

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

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

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

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

    %------------------------------------- dump-out progess!
        vdel = vdel./(hvrt.*hvrt) ;
        move = vdel > opts.vtol^2 ;
        nmov = ...
            length(find(move));

        ntri = size(tria,1) ;

        if (mod(iter,opts.disp)==+0)
            fprintf(+1, ...
            '%11i %18i %18i\n', ...
            [iter,nmov,ntri]) ;
        end

    %------------------------------------- loop convergence!
        if (nmov == +0), break; end

    end

    tria = tria( :,1:3);

%----------------------------------------- prune unused vert
    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 = redo(conn);
    tria = redo(tria);

    vert = vert(keep,:);

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

    if (opts.dbug)
%----------------------------------------- print debug timer
        fprintf(1,'\n') ;
        fprintf(1,' Mesh smoothing 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, ...
        ' ITER: %f \n', tcpu.iter);
        fprintf(1, ...
        ' UNDO: %f \n', tcpu.undo);
        fprintf(1, ...
        ' KEEP: %f \n', tcpu.keep);
        fprintf(1,'\n') ;
    end

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

end

function [hvrt] = evalhfn(vert,edge,EMAT,hfun,harg)
%EVALHFN eval. the spacing-fun. at mesh vertices.

    if (~isempty (hfun))
        if (isnumeric(hfun))
            hvrt = hfun * ...
              ones(size(vert,1),1) ;
        else
            hvrt = feval( ...
                hfun,vert,harg{:}) ;
        end
    else

%-- no HFUN - HVRT is mean edge-len. at vertices!
        evec = vert(edge(:,2),:) - ...
               vert(edge(:,1),:) ;
        elen = sqrt(sum(evec.^2,2)) ;

        hvrt = (EMAT*elen) ...
            ./ max(sum(EMAT,2),eps) ;

        free = true(size(vert,1),1) ;
        free(edge(:,1)) = false;
        free(edge(:,2)) = false;

        hvrt(free)      =  +inf;

    end

end

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

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

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

    if (~isfield(opts,'vtol'))
        opts.vtol = +1.0E-02;
    else
    if (~isnumeric(opts.vtol))
        error('smooth2:incorrectInputClass', ...
            'Incorrect input class.');
    end
    if (numel(opts.vtol)~= +1)
        error('smooth2:incorrectDimensions', ...
            'Incorrect input dimensions.') ;
    end
    if (opts.vtol <= 0.)
        error('smooth2:invalidOptionValues', ...
            'Invalid OPT.VTOL 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


function metric = metric_euclidean(varargin)
%METRIC_EUCLIDEAN An euclidean metric function
%
%  Description
%    METRIC = METRIC_EUCLIDEAN('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates a an euclidean metric function structure in which the
%    named parameters have the specified values. Either
%    'components' or 'deltadist' has to be specified. Any
%    unspecified parameters are set to default values.
%
%    METRIC = METRIC_EUCLIDEAN(METRIC,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a metric function structure with the named parameters
%    altered with the specified values.
%
%    Parameters for Euclidean metric function [default]
%       components        - cell array of vectors specifying which
%                           inputs are grouped together with a same
%                           scaling parameter. For example, the
%                           component specification {[1 2] [3]}
%                           means that distance between 3
%                           dimensional vectors computed as
%                           r = (r_1^2 + r_2^2 )/l_1 + r_3^2/l_2,
%                           where r_i are distance along component
%                           i, and l_1 and l_2 are lengthscales for
%                           corresponding component sets. If
%                           'components' is not specified, but
%                           'deltadist' is specified, then default
%                           is {1 ... length(deltadist)}
%       deltadist         - indicator vector telling which component sets
%                           are handled using the delta distance
%                           (0 if x=x', and 1 otherwise). Default is
%                           false for all component sets.
%       lengthScale       - lengthscales for each input component set
%                           Default is 1 for each set
%       lengthScale_prior - prior for lengthScales [prior_unif]
%
%  See also
%    GP_SET, GPCF_SEXP
%
% Copyright (c) 2008 Jouni Hartikainen
% Copyright (c) 2008 Jarno Vanhatalo
% Copyright (c) 2010 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'METRIC_EUCLIDEAN';
  ip.addOptional('metric', [], @isstruct);
  ip.addParamValue('components',[], @(x) isempty(x) || iscell(x));
  ip.addParamValue('deltadist',[], @(x) isvector(x) || isempty(x));
  ip.addParamValue('lengthScale',[], @(x) isvector(x) && all(x>0));
  ip.addParamValue('lengthScale_prior',prior_unif, ...
                   @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  metric=ip.Results.metric;

  if isempty(metric)
    % Initialize a Gaussian process
    init=true;
  else
    % Modify a Gaussian process
    if ~isfield(metric,'type') && isequal(metric.type,'metric_euclidean')
      error('First argument does not seem to be a metric structure')
    end
    init=false;
  end

  if init
    % Type
    metric.type = 'metric_euclidean';
  end

  % Components
  if init || ~ismember('components',ip.UsingDefaults)
    metric.components = ip.Results.components;
  end
  % Deltadist
  if init || ~ismember('deltadist',ip.UsingDefaults)
    metric.deltadist = ip.Results.deltadist;
  end
  % Components+Deltadist check and defaults
  if isempty(metric.components) && isempty(metric.deltadist)
    error('Either ''components'' or ''deltadist'' has to be specified')
  elseif isempty(metric.components)
    metric.components=num2cell(1:length(metric.components));
  elseif isempty(metric.deltadist)
    metric.deltadist = false(1,length(metric.components));
  end
  % Lengthscale
  if init || ~ismember('lengthScale',ip.UsingDefaults)
    metric.lengthScale = ip.Results.lengthScale;
    if isempty(metric.lengthScale)
      metric.lengthScale = repmat(1,1,length(metric.components));
    end
  end
  % Prior for lengthscale
  if init || ~ismember('lengthScale_prior',ip.UsingDefaults)
    metric.p=[];
    metric.p.lengthScale = ip.Results.lengthScale_prior;
  end

  if init
    % Set the function handles to the subfunctions
    metric.fh.pak       = @metric_euclidean_pak;
    metric.fh.unpak     = @metric_euclidean_unpak;
    metric.fh.lp        = @metric_euclidean_lp;
    metric.fh.lpg       = @metric_euclidean_lpg;
    metric.fh.dist      = @metric_euclidean_dist;
    metric.fh.distg     = @metric_euclidean_distg;
    metric.fh.ginput    = @metric_euclidean_ginput;
    metric.fh.recappend = @metric_euclidean_recappend;
  end

end

function [w s] = metric_euclidean_pak(metric)
%METRIC_EUCLIDEAN_PAK  Combine GP covariance function
%                      parameters into one vector.
%
%  Description
%    W = METRIC_EUCLIDEAN_PAK(GPCF) takes a covariance function
%    structure GPCF and combines the covariance function
%    parameters and their hyperparameters into a single row
%    vector W and takes a logarithm of the covariance function
%    parameters.
%
%       w = [ log(metric.lengthScale(:))
%             (hyperparameters of metric.lengthScale)]'
%
%  See also
%    METRIC_EUCLIDEAN_UNPAK

  w = []; s = {};
  if ~isempty(metric.p.lengthScale)
    w = log(metric.lengthScale);
    if numel(metric.lengthScale)>1
      s = [s; sprintf('log(metric.lengthScale x %d)',numel(metric.lengthScale))];
    else
      s = [s; 'log(metric.lengthScale)'];
    end
    % Hyperparameters of lengthScale
    [wh sh] = metric.p.lengthScale.fh.pak(metric.p.lengthScale);
    w = [w wh];
    s = [s; sh];
  end

end

function [metric, w] = metric_euclidean_unpak(metric, w)
%METRIC_EUCLIDEAN_UNPAK  Separate metric parameter vector into components
%
%  Description
%    METRIC, W] = METRIC_EUCLIDEAN_UNPAK(METRIC, W) takes a
%    metric structure GPCF and a parameter vector W, and returns
%    a covariance function structure identical to the input,
%    except that the covariance parameters have been set to the
%    values in W. Deletes the values set to GPCF from W and
%    returns the modified W.
%
%    The covariance function parameters are transformed via exp
%    before setting them into the structure.
%
%  See also
%    METRIC_EUCLIDEAN_PAK
%

  if ~isempty(metric.p.lengthScale)
    i2=length(metric.lengthScale);
    i1=1;
    metric.lengthScale = exp(w(i1:i2));
    w = w(i2+1:end);

    % Hyperparameters of lengthScale
    [p, w] = metric.p.lengthScale.fh.unpak(metric.p.lengthScale, w);
    metric.p.lengthScale = p;
  end
end

function lp = metric_euclidean_lp(metric)
%METRIC_EUCLIDEAN_LP  Evaluate the log prior of metric parameters
%
%  Description
%    LP = METRIC_EUCLIDEAN_LP(METRIC) takes a metric structure
%    METRIC and returns log(p(th)), where th collects the
%    parameters.
%
%  See also
%    METRIC_EUCLIDEAN_PAK, METRIC_EUCLIDEAN_UNPAK, METRIC_EUCLIDEAN_G, GP_E
%

% Evaluate the prior contribution to the error. The parameters that
% are sampled are from space W = log(w) where w is all the "real" samples.
% On the other hand errors are evaluated in the W-space so we need take
% into account also the  Jakobian of transformation W -> w = exp(W).
% See Gelman et al. (2013), Bayesian Data Analysis, third edition, p. 21.
  if ~isempty(metric.p.lengthScale)
    lp = metric.p.lengthScale.fh.lp(metric.lengthScale, metric.p.lengthScale) + sum(log(metric.lengthScale));
  else
    lp=0;
  end

end

function lpg = metric_euclidean_lpg(metric)
%METRIC_EUCLIDEAN_LPG  d log(prior)/dth of the metric parameters th
%
%  Description
%    LPG = METRIC_EUCLIDEAN_LPG(METRIC) takes a likelihood
%    structure METRIC and returns d log(p(th))/dth, where th
%    collects the parameters.
%
%  See also
%    METRIC_EUCLIDEAN_PAK, METRIC_EUCLIDEAN_UNPAK, METRIC_EUCLIDEAN, GP_E
%

% Evaluate the prior contribution of gradient with respect to lengthScale
  if ~isempty(metric.p.lengthScale)
    i1=1;
    lll = length(metric.lengthScale);
    lpgs = metric.p.lengthScale.fh.lpg(metric.lengthScale, metric.p.lengthScale);
    lpg(i1:i1-1+lll) = lpgs(1:lll).*metric.lengthScale + 1;
    lpg = [lpg lpgs(lll+1:end)];
  end
end

function gdist  = metric_euclidean_distg(metric, x, x2, mask)
%METRIC_EUCLIDEAN_DISTG  Evaluate the gradient of the metric function
%
%  Description
%    DISTG = METRIC_EUCLIDEAN_DISTG(METRIC, X) takes a metric
%    structure METRIC together with a matrix X of input
%    vectors and return the gradient matrices GDIST and
%    GPRIOR_DIST for each parameter.
%
%    DISTG = METRIC_EUCLIDEAN_DISTG(METRIC, X, X2) forms the
%    gradient matrices between two input vectors X and X2.
%
%    DISTG = METRIC_EUCLIDEAN_DISTG(METRIC, X, X2, MASK) forms
%    the gradients for masked covariances matrices used in sparse
%    approximations.
%
%  See also
%    METRIC_EUCLIDEAN_PAK, METRIC_EUCLIDEAN_UNPAK, METRIC_EUCLIDEAN, GP_E
%

  gdist=[];
  components = metric.components;

  n = size(x,1);
  m = length(components);
  i1=0;i2=1;

  % NOTE! Here we have already taken into account that the parameters
  % are transformed through log() and thus dK/dlog(p) = p * dK/dp

  if ~isempty(metric.p.lengthScale)
    if nargin <= 3
      if nargin == 2
        x2 = x;
      end
      ii1=0;

      dist  =  0;
      distc = cell(1,m);
      % Compute the distances for each component set
      for i=1:m
        if length(metric.lengthScale)==1
          s=1./metric.lengthScale.^2;
        else
          s=1./metric.lengthScale(i).^2;
        end
        distc{i} = 0;
        for j = 1:length(components{i})
          if metric.deltadist(i)
            distc{i} = distc{i} + double(bsxfun(@ne,x(:,components{i}(j)),x2(:,components{i}(j))'));
          else
            distc{i} = distc{i} + bsxfun(@minus,x(:,components{i}(j)),x2(:,components{i}(j))').^2;
          end
        end
        distc{i} = distc{i}.*s;
        % Accumulate to the total distance
        dist = dist + distc{i};
      end
      dist = sqrt(dist);
      % Loop through component sets
      if length(metric.lengthScale)==1
        D = -distc{1};
        D(dist~=0) = D(dist~=0)./dist(dist~=0);
        ii1 = ii1+1;
        gdist{ii1} = D;
      else
        for i=1:m
          D = -distc{i};
          ind = dist~=0;
          D(ind) = D(ind)./dist(ind);
          ii1 = ii1+1;
          gdist{ii1} = D;
        end
      end
% $$$         elseif nargin == 3
% $$$             if size(x,2) ~= size(x2,2)
% $$$                 error('metric_euclidean -> _ghyper: The number of columns in x and x2 has to be the same. ')
% $$$             end
    elseif nargin == 4
      gdist = cell(1,length(metric.lengthScale));
    end

    % Evaluate the prior contribution of gradient with respect to lengthScale
    if ~isempty(metric.p.lengthScale)
      i1=1;
      lll = length(metric.lengthScale);
      gg = -metric.p.lengthScale.fh.lpg(metric.lengthScale, metric.p.lengthScale);
      gprior(i1:i1-1+lll) = gg(1:lll).*metric.lengthScale - 1;
      gprior = [gprior gg(lll+1:end)];
    end
  end
end

function dist = metric_euclidean_dist(metric, x1, x2)
%METRIC_EUCLIDEAN_DIST  Compute the euclidean distence between
%                       one or two matrices.
%
%  Description
%    DIST = METRIC_EUCLIDEAN_DIST(METRIC, X) takes a metric
%    structure METRIC together with a matrix X of input
%    vectors and calculates the euclidean distance matrix DIST.
%
%    DIST = METRIC_EUCLIDEAN_DIST(METRIC, X1, X2) takes a
%    metric structure METRIC together with a matrices X1 and
%    X2 of input vectors and calculates the euclidean distance
%    matrix DIST.
%
%  See also
%    METRIC_EUCLIDEAN_PAK, METRIC_EUCLIDEAN_UNPAK, METRIC_EUCLIDEAN, GP_E
%
  if (nargin == 2 || isempty(x2))
    % use fast c-code for self-distance
    x2=x1;
    % force deltadist to be logical for simplified c-code
    metric.deltadist=logical(metric.deltadist);
    dist = dist_euclidean(metric,x1);
    if ~any(isnan(dist))
      % if c-code was available, result is not NaN
      return
    end
  end

  [n1,m1]=size(x1);
  [n2,m2]=size(x2);

  if m1~=m2
    error('the number of columns of X1 and X2 has to be same')
  end

  components = metric.components;
  m = length(components);
  dist  =  0;

  s=1./metric.lengthScale.^2;
  if m>numel(s)
    s=repmat(s,1,m);
  end
  for i=1:m
    for j = 1:length(components{i})
      if metric.deltadist(i)
        dist = dist + s(i).*double(bsxfun(@ne,x1(:,components{i}(j)),x2(:,components{i}(j))'));
      else
        dist = dist + s(i).*bsxfun(@minus,x1(:,components{i}(j)),x2(:,components{i}(j))').^2;
      end
    end
  end
  dist=sqrt(dist); % euclidean distance

end

function [ginput, gprior_input]  = metric_euclidean_ginput(metric, x1, x2)
%METRIC_EUCLIDEAN_GINPUT  Compute the gradient of the
%                         euclidean distance function with
%                         respect to input. [n, m]=size(x);
  ii1 = 0;
  components = metric.components;

  if nargin == 2 || isempty(x2)
    x2=x1;
  end

  [n1,m1]=size(x1);
  [n2,m2]=size(x2);

  if m1~=m2
    error('the number of columns of X1 and X2 has to be same')
  end

  s = 1./metric.lengthScale.^2;
  dist = 0;
  for i=1:length(components)
    for j = 1:length(components{i})
      if metric.deltadist(i)
        dist = dist + s(i).*double(bsxfun(@ne,x1(:,components{i}(j)),x2(:,components{i}(j))'));
      else
        dist = dist + s(i).*bsxfun(@minus,x1(:,components{i}(j)),x2(:,components{i}(j))').^2;
      end
    end
  end
  dist = sqrt(dist);

  for i=1:m1
    for j = 1:n1
      DK = zeros(n1,n2);
      for k = 1:length(components)
        if ismember(i,components{k})
          if metric.deltadist(i)
            DK(j,:) = DK(j,:)+s(k).*double(bsxfun(@ne,x1(j,i),x2(:,i)'));
          else
            DK(j,:) = DK(j,:)+s(k).*bsxfun(@minus,x1(j,i),x2(:,i)');
          end
        end
      end
      if nargin == 2
        DK = DK + DK';
      end
      DK(dist~=0) = DK(dist~=0)./dist(dist~=0);

      ii1 = ii1 + 1;
      ginput{ii1} = DK;
      gprior_input(ii1) = 0;
    end
  end
  %size(ginput)
  %ginput

end

function recmetric = metric_euclidean_recappend(recmetric, ri, metric)
%RECAPPEND  Record append
%
%  Description
%    RECMETRIC = METRIC_EUCLIDEAN_RECAPPEND(RECMETRIC, RI,
%    METRIC) takes old metric function record RECMETRIC, record
%    index RI and metric function structure. Appends the
%    parameters of METRIC to the RECMETRIC in the ri'th place.
%
%  See also
%    GP_MC and GP_MC -> RECAPPEND

% Initialize record
  if nargin == 2
    recmetric.type = 'metric_euclidean';
    metric.components = recmetric.components;

    % Initialize parameters
    recmetric.lengthScale = [];

    % Set the function handles
    recmetric.fh.pak       = @metric_euclidean_pak;
    recmetric.fh.unpak     = @metric_euclidean_unpak;
    recmetric.fh.lp        = @metric_euclidean_lp;
    recmetric.fh.lpg       = @metric_euclidean_lpg;
    recmetric.fh.dist      = @metric_euclidean_dist;
    recmetric.fh.distg     = @metric_euclidean_distg;
    recmetric.fh.ginput    = @metric_euclidean_ginput;
    recmetric.fh.recappend = @metric_euclidean_recappend;
    return
  end
  mp = metric.p;

  % record parameters
  if ~isempty(metric.lengthScale)
    recmetric.lengthScale(ri,:)=metric.lengthScale;
    recmetric.p.lengthScale = metric.p.lengthScale.fh.recappend(recmetric.p.lengthScale, ri, metric.p.lengthScale);
  elseif ri==1
    recmetric.lengthScale=[];
  end

end


function au_qmr_ilu_m_i(A,mc,max_it_qmr,tol_qmr,tol_ilu,info_qmr)
%
%  Generates the data used in 'au_qmr_ilu_m'. Data are stored in global
%  variables.
%
%  Moreover, it defines the parameters mc, max_it_qmr, tol_qmr,
%  tol_ilu, and info_qmr, which are needed later in 'au_qmr_ilu_l_i' and
%  'au_qmr_ilu_s_i'.
%
%  Calling sequence:
%
%    au_qmr_ilu_m_i(A,mc,max_it_qmr,tol_qmr,tol_ilu,info_qmr)
%
%  Input:
%
%    A          real matrix;
%    mc         (= 'M' or 'C') Optimize for memory ('M') or computation ('C')
%               when shifted systems are solved by 'au_qmr_ilu_l/s' later.
%               Optional (default value: 'M');
%    max_it_qmr maximal number of QMR steps allowed in 'au_qmr_ilu_l/s'.
%               Corresponds to parameter 'MAXIT' in MATLAB function 'qmr'.
%               Optional (default value: min([n-1,100]));
%    tol_qmr    stopping tolerance (w.r.t. normalized residual) for QMR
%               iteration in 'au_qmr_ilu_l/s'. Corresponds to parameter 'TOL'
%               in MATLAB function 'qmr'. Optional (default value: 1e-14);
%    tol_ilu    dropping tolerance for ILU preconditioner. Corresponds to
%               parameter 'DROPTOL' in MATLAB function 'luinc'. Optional
%               (default value: 1e-2);
%    info_qmr   (= 0, 1, ...) If 1, warnings are printed when parameter FLAG
%               of MATLAB function 'qmr' is not equal to zero in QMR
%               iteration in 'au_qmr_ilu_l/s'. If 0, no warnings are
%               printed. Optional (default value: 1).
%
%  Remark:
%
%    If mc = 'M', the incomplete LU factors for A+p(i)*I are computed
%    any time 'au_qmr_ilu_s' is called, but they are not stored as global
%    variables to save memory. That means they are in general computed
%    several times, which results in additional computational cost. If
%    'mc = C', the incomplete LU factors are computed once by calling
%    'au_qmr_ilu_s_i' and stored as global variables, which results is
%    an increased memory demand.
%
%
%  LYAPACK 1.0 (Thilo Penzl, August 1999)

na = nargin;

if na<1 | na>6
  error('Wrong number of input arguments.');
end

if nargin < 2, mc = []; end
if nargin < 3, max_it_qmr = []; end
if nargin < 4, tol_qmr = []; end
if nargin < 5, tol_ilu = []; end
if nargin < 6, info_qmr = []; end

if ~length(mc), mc = 'M'; end
if ~length(max_it_qmr), max_it_qmr = min([size(A,1)-1,100]); end
if ~length(tol_qmr), tol_qmr = 1e-14; end
if ~length(tol_ilu), tol_ilu = 1e-2; end
if ~length(info_qmr), info_qmr = 1; end

global LP_A LP_MC LP_MAX_IT_QMR LP_TOL_QMR LP_TOL_ILU LP_INFO_QMR

LP_A = A;
LP_MC = mc;
LP_MAX_IT_QMR = max_it_qmr;
LP_TOL_QMR = tol_qmr;
LP_TOL_ILU = tol_ilu;
LP_INFO_QMR = info_qmr;


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting %%	lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us %to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the RUBBERBAND-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Dear_KC()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx =  32;        % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Ny =  32;        % # of Eulerian Grid Pts. in y-Direction (MUST BE EVEN!!!)
Lx = 1.0;        % Length of Eulerian Grid in x-Direction
Ly = 1.0;        % Length of Eulerian Grid in y-Direction
ds = Lx/(2*Nx);  % Spatial step!

% Immersed Structure Geometric / Dynamic Parameters %
N = 2*Nx;        % Number of Lagrangian Pts. (2x resolution of Eulerian grid)
ds_Rest = 0;     % Resting length of springs
struct_name = 'dear_KC'; % Name for .vertex, .spring, etc files.

% Line on top and bottom of geometry
ds2 = 0.125*ds;
xLine = Lx/20:ds2:0.95*Lx;
yLineB = 0.025*Lx*ones(1,length(xLine));
yLineT= 0.975*Lx*ones(1,length(xLine));

xSq = [xLine xLine yLineB yLineT];
ySq = [yLineB yLineT xLine xLine];

% Call function to construct geometry
[x1,y1] = give_Me_Immsersed_Boundary_Geometry_1(Lx,Nx,ds);
x1 = [x1 xSq];
y1 = [y1 ySq];

% Call function to construct geometry
[x2,y2] = give_Me_Immsersed_Boundary_Geometry_2(Lx,Nx,ds,x1);
x2 = [x2 xSq];
y2 = [y2 ySq];

% Call function to construct geometry
[x3,y3] = give_Me_Immsersed_Boundary_Geometry_3(Lx,Nx,ds);
x3 = [x3 xSq];
y3 = [y3 ySq];

% Plot Geometry to test BEFORE taking out pts.
figure(1)
plot(x1,y1,'r*'); hold on;
axis([0 Lx 0 Ly]);

figure(2)
plot(x2,y2,'b*'); hold on;
axis([0 Lx 0 Ly]);

figure(3)
plot(x3,y3,'k*'); hold on;
axis([0 Lx 0 Ly]);

% Print files to .txt files
please_Print_Vertices_To_File(x1,y1,x2,y2,x3,y3)

% Prints .vertex file!
print_Lagrangian_Vertices(x1,y1,struct_name);

% Prints .spring file!
%k_Spring = 1e7;
%print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Rest,struct_name);

% Prints .beam file!
%k_Beam = 0.5; C = 0.0;
%print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name);

% Prints .target file!
k_Target = 1e6;
print_Lagrangian_Target_Pts(x1,k_Target,struct_name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called struct.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called struct.target
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

    N = length(xLag);

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        if s > 284
            k_Target = 2.5e9;
        end
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called struct.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %BEAMS BETWEEN VERTICES
    for s = 2:N-1
            if  s <= N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C);
            else
                %Case s=N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 1,   k_Beam, C);
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called struct.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Rest,struct_name)

    N = length(xLag);

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            else
                %Case s=N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, 1,   k_Spring, ds_Rest);
            end
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for PHASE 1
%           msg: "Hi KC!!"
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry_1(Lx,Nx,ds)

% The immsersed structure message #1: Hi Nghieng!! %
len = Lx/6.5;

xC = 0;%-Lx/4;
yC = 0;%-Lx/2;
[xH,yH] = give_Me_The_Letter_Please_2(ds,len,'H',xC,yC); % (0,0)
[xi1,yi1] = give_Me_The_Letter_Please_2(ds,len,'i',xC,yC-0.05);
%
[xN,yN] = give_Me_The_Letter_Please_2(ds,len,'N',xC,yC);
[xg1,yg1] = give_Me_The_Letter_Please_2(ds,len,'g',xC,yC);
[xh,yh] = give_Me_The_Letter_Please_2(ds,len,'h',xC,yC);
[xi2,yi2] = give_Me_The_Letter_Please_2(ds,len,'i',xC,yC-0.05);
[xe,ye] = give_Me_The_Letter_Please_2(ds,len,'e',xC,yC);
[xn,yn] = give_Me_The_Letter_Please_2(ds,len,'n',xC,yC);
[xg2,yg2] = give_Me_The_Letter_Please_2(ds,len,'g',xC,yC);

% No room in domain for !'s
%[xEx1,yEx1] = give_Me_The_Letter_Please_2(ds,1.2*len,'!',xC-0.55,yC);
%[xEx2,yEx2] = give_Me_The_Letter_Please_2(ds,1.2*len,'!',xC-0.59,yC);

% HI
xH = xH + 0.45;   yH = yH + 0.7;
xi1 = xi1 + 0.55; yi1 = yi1 + 0.65;

% Nghieng
xN =  xN + 0.15;     yN = yN + 0.375;
xg1 = xg1 + 0.325;   yg1 = yg1 + 0.375;
xh =  xh + 0.435;      yh = yh + 0.375;
xi2 = xi2 + 0.515;     yi2 = yi2 + 0.325;
xe = xe + 0.59;       ye = ye + 0.375;
xn = xn + 0.7;       yn = yn + 0.375;
xg2 = xg2 + 0.8;     yg2 = yg2 + 0.375;

%
% COMBINE GEOMETRY
%
xLag = [xH xi1 xN xg1 xh xi2 xe xn xg2];
yLag = [yH yi1 yN yg1 yh yi2 ye yn yg2];

%
% Test Plot
%
% figure(1)
% plot(xH,yH,'.'); hold on;
% plot(xi1,yi1,'.'); hold on;
% plot(xN,yN,'.'); hold on;
% plot(xg1,yg1,'.'); hold on;
% plot(xh,yh,'.'); hold on;
% plot(xi2,yi2,'.'); hold on;
% plot(xe,ye,'.'); hold on;
% plot(xn,yn,'.'); hold on;
% plot(xg2,yg2,'.'); hold on;

%
% Construct Horizontal Lines for Added Pts. in Domain
%
xRow = (0:ds:Lx/3) + 1/3;
yRow = ones(size(xRow));

xRow2 = (0:ds:Lx/6+ds) + 0.41666;
yRow2 = ones(size(xRow2));

xRow_T1 = xRow; yRow_T1 = 0.925*yRow;
xRow_T2 = xRow2; yRow_T2 = 0.85*yRow2;

xRow_B1 = [xRow xRow(end)+ds]; yRow_B1 = 0.075*[yRow 1];
xRow_B2 = xRow2; yRow_B2 = 0.15*yRow2;

%
% Combine Letters + Horizontal Lines
%
xLag = [xLag xRow_T1 xRow_T2 xRow_B1 xRow_B2];
yLag = [yLag yRow_T1 yRow_T2 yRow_B1 yRow_B2];

%
% Test Plot 2
%
% figure(11)
% plot(xLag,yLag,'.'); hold on;
% axis([0 1 0 1]);

%
% CHECK TO MAKE SURE EQUAL NUMBER OF MOVING LAGS AS OTHER PHASES
% HERE: 284
%
%size(xLag)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for PHASE 2
%           msg: "Would you like to ..."
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry_2(Lx,Nx,ds,x1)

% The immsersed structure message #2: Woud you like to... %
len = Lx/8;

xC = -Lx/6;
yC = -2*Lx/3;
[xW,yW] = give_Me_The_Letter_Please(ds,len,'W',xC,yC);
[xo,yo] = give_Me_The_Letter_Please(ds,len,'o',xC-0.1,yC);
[xu,yu] = give_Me_The_Letter_Please(ds,len,'u',xC-0.18,yC);
[xl,yl] = give_Me_The_Letter_Please(ds,len,'l',xC-0.27,yC);
[xd,yd] = give_Me_The_Letter_Please(ds,1.2*len,'d',xC-0.29,yC);

[xy,yy] = give_Me_The_Letter_Please(ds,1.2*len,'y',xC-0.47,yC);
[xo2,yo2] = give_Me_The_Letter_Please(ds,len,'o',xC-0.55,yC);
[xu2,yu2] = give_Me_The_Letter_Please(ds,len,'u',xC-0.63,yC);

xC = -Lx/4;
yC = -1*Lx/4 - Lx/6;
[xl2,yl2] = give_Me_The_Letter_Please(ds,len,'l',xC,yC);
[xi,yi] = give_Me_The_Letter_Please(ds,len,'i',xC-0.01,yC);
[xk,yk] = give_Me_The_Letter_Please(ds,len,'k',xC-0.07,yC);
[xe,ye] = give_Me_The_Letter_Please(ds,len,'e',xC-0.13,yC);

[xt,yt] = give_Me_The_Letter_Please(ds,len,'t',xC-0.27,yC);
[xo3,yo3] = give_Me_The_Letter_Please(ds,len,'o',xC-0.32,yC);

yC = -1*Lx/5 - Lx/6;
[xo4,yo4] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC-0.44,yC);
[xo5,yo5] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC-0.48,yC);
[xo6,yo6] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC-0.52,yC);

xLag = [xW xo xu xl xd xy xo2 xu2 xl2 xi xk xe xt xo3 xo4 xo5 xo6];
yLag = [yW yo yu yl yd yy yo2 yu2 yl2 yi yk ye yt yo3 yo4 yo5 yo6];

%x1Len = length(x1)
%xLagLen=length(xLag)
%PtsLeft = abs( length(xLag) - length(x1) )
%ds1 = (Lx/2.5)/( PtsLeft/2 - 1 );

xBL = 0:ds:Lx/2.5; xBL = xBL + 3/10*Lx;
yTL = 0.84*ones(1,length(xBL));
yBL = 0.18*ones(1,length(xBL));

xLag = [xLag xBL xBL];
yLag = [yLag yTL yBL];

% plot(xW,yW,'r*'); hold on;
% plot(xo,yo,'r*'); hold on;
% plot(xu,yu,'r*'); hold on;
% plot(xl,yl,'r*'); hold on;
% plot(xd,yd,'r*'); hold on;
%
% plot(xy,yy,'r*'); hold on;
% plot(xo2,yo2,'r*'); hold on;
% plot(xu2,yu2,'r*'); hold on;
%
% plot(xl2,yl2,'r*'); hold on;
% plot(xi,yi,'r*'); hold on;
% plot(xk,yk,'r*'); hold on;
% plot(xe,ye,'r*'); hold on;
%
% plot(xt,yt,'r*'); hold on;
% plot(xo3,yo3,'r*'); hold on;
%
% plot(xo4,yo4,'r*'); hold on;
% plot(xo5,yo5,'r*'); hold on;
% plot(xo6,yo6,'r*'); hold on;
% axis([0 Lx 0 Lx]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for PHASE 2
%           msg: "Would you like to ..."
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry_3(Lx,Nx,ds)

% The immsersed structure message #3: ...go on a date with me? %
len = Lx/9;

xC = -Lx/8;
yC = -3*Lx/4;
shift = 0.2;

[xo4,yo4] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC,yC+0.09);
[xo5,yo5] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC-0.04,yC+0.09);
[xo6,yo6] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC-0.08,yC+0.09);

[xg,yg] = give_Me_The_Letter_Please(ds,len,'g',xC-shift,yC);
[xo,yo] = give_Me_The_Letter_Please(ds,len,'o',xC-0.1-shift,yC);

[xo2,yo2] = give_Me_The_Letter_Please(ds,1.2*len,'o',xC-0.25-shift,yC);
[xn,yn] = give_Me_The_Letter_Please(ds,1.2*len,'n',xC-0.34-shift,yC);

[xa,ya] = give_Me_The_Letter_Please(ds,len,'a',xC-0.48-shift,yC);

xC = -Lx/5;
yC = -Lx/2;
[xd,yd] = give_Me_The_Letter_Please(ds,len,'d',xC,yC);
[xa2,ya2] = give_Me_The_Letter_Please(ds,len,'a',xC-0.08,yC);
[xt,yt] = give_Me_The_Letter_Please(ds,len,'t',xC-0.16,yC);
[xe,ye] = give_Me_The_Letter_Please(ds,len,'e',xC-0.22,yC);

[xw,yw] = give_Me_The_Letter_Please(ds,1.1*len,'w',xC-0.37,yC);
[xi,yi] = give_Me_The_Letter_Please(ds,len,'i',xC-0.45,yC);
[xt2,yt2] = give_Me_The_Letter_Please(ds,len,'t',xC-0.505,yC);
[xh,yh] = give_Me_The_Letter_Please(ds,len,'h',xC-0.58,yC);

xC = -Lx/2.5;
yC = -Lx/4;
[xm,ym] = give_Me_The_Letter_Please(ds,1.1*len,'m',xC,yC);
[xe2,ye2] = give_Me_The_Letter_Please(ds,len,'e',xC-0.12,yC);
[xQU,yQU] = give_Me_The_Letter_Please(ds,1.2*len,'?',xC-0.22,yC);

xLag = [xo4 xo5 xo6 xg xo xo2 xn xa xd xa2 xt xe xw xi xt2 xh xm xe2 xQU];
yLag = [yo4 yo5 yo6 yg yo yo2 yn ya yd ya2 yt ye yw yi yt2 yh ym ye2 yQU];

% plot(xo4,yo4,'r*'); hold on;
% plot(xo5,yo5,'r*'); hold on;
% plot(xo6,yo6,'r*'); hold on;
%
% plot(xg,yg,'r*'); hold on;
% plot(xo,yo,'r*'); hold on;
%
% plot(xo2,yo2,'r*'); hold on;
% plot(xn,yn,'r*'); hold on;
%
% plot(xa,ya,'r*'); hold on;
%
% plot(xd,yd,'r*'); hold on;
% plot(xa2,ya2,'r*'); hold on;
% plot(xt,yt,'r*'); hold on;
% plot(xe,ye,'r*'); hold on;
%
% plot(xw,yw,'r*'); hold on;
% plot(xi,yi,'r*'); hold on;
% plot(xt2,yt2,'r*'); hold on;
% plot(xh,yh,'r*'); hold on;
%
% plot(xm,ym,'r*'); hold on;
% plot(xe2,ye2,'r*'); hold on;
% plot(xQU,yQU,'r*'); hold on;
%
% axis([0 Lx 0 Lx]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%s%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for the Heart
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry_Heart(Lx,Nx,frac,Nb)

% The immsersed structure is a heart %
ds = 2*pi/(Nb-1);
tVec = 0:ds:2*pi;

for i=1:length(tVec)
    t = tVec(i);
    xLag(i)	=	16*sin(t)^3;
    yLag(i)	=	13*cos(t)-5*cos(2*t)-2*cos(3*t)-cos(4*t);
end

xLag = frac*xLag + 0.5;
yLag = frac*yLag + 0.5;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for PHASE 2
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry_4(Lx,Nx,frac,Nb)

% The immsersed structure is a heart %
ds = 2*pi/(Nb-1);
tVec = 0:ds:2*pi;
for i=1:length(tVec)
    t = tVec(i);
    r(i) = 1 - sin(t);
    xLag(i)	=	r(i)*cos(t);
    yLag(i)	=	r(i)*sin(t);
end

xLag = frac*xLag;
yLag = frac*yLag;

minY = min(yLag);
yLag = yLag - 3*minY/8;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: take out Lagrangian Pts.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x1,y1,x2,y2] = please_Take_Out_Points(x1,y1,x2,y2)

n1 = 50;
n2 = 70;

x1 = [x1(1:n1) x1(n2:end)];
y1 = [y1(1:n1) y1(n2:end)];

x2 = [x2(1:n1) x2(n2:end)];
y2 = [y2(1:n1) y2(n2:end)];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints all Vertices to File
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function please_Print_Vertices_To_File(X1,Y1,X2,Y2,X3,Y3)

fileID = fopen('All_Positions.txt','w');
for j=1:length(X1)
    fprintf(fileID,'%1.16e %1.16e %1.16e %1.16e %1.16e %1.16e %1.16e %1.16e\n', X1(j),Y1(j),X2(j),Y2(j),X3(j),Y3(j),X3(j),Y3(j) );
end
fclose(fileID);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: give me the letter!!!!!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x,y] = give_Me_The_Letter_Please(ds,len,letter,xC,yC)

if strcmp(letter,'a')
    r = len/4.1;
    dt = ds/r;
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xA(i) = r*cos(theta(i)) - xC;
       yA(i) = r*sin(theta(i)) - yC - len/4;
    end

    yLine = [-r:ds:r r]; yLine = yLine - yC - len/4;
    xLine = r*ones(1,length(yLine)) - (xC-ds/6);

    x = [xA xLine];
    y = [yA yLine];

elseif strcmp(letter,'C')

    r = len/2;
        dt = ds/r;
    theta = pi/2:dt:pi-0.25*dt;
    theta = [theta pi -theta];
    for i=1:length(theta)
       x(i) = 3*r/4*cos(theta(i)) - xC;
       y(i) = r*sin(theta(i)) - yC;
    end

    xTB = 0:3*ds/4:len/6;
    xTB = xTB - xC + 0.01;
    yT = r*ones(1,length(xTB))-yC;
    yB = -r*ones(1,length(xTB))-yC;

    x = [xTB x xTB];
    y = [yT y yB];

    length(x)

elseif strcmp(letter,'d')

    r = len/4.1;
    dt = ds/r;
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xA(i) = r*cos(theta(i)) - xC;
       yA(i) = r*sin(theta(i)) - yC - len/4;
    end

    yLine = [-len/2:ds:len/2.25 r]; yLine = yLine - yC;
    xLine = r*ones(1,length(yLine)) - (xC-ds/6);

    x = [xA xLine];
    y = [yA yLine];

 elseif strcmp(letter,'e')

    r = len/4.1;
    yV = [-r:ds:r r];
    xH = yV - xC;
    yH = zeros(1,length(xH)) - yC;
    yH2 = yH - len/4;
    yH3 = yH - len/2;

    yV = yV - yC - len/4;
    xV = r*ones(1,length(yV));

    yV3 = 0:-ds:-len/4;
    yV3 = yV3 - yC;
    xV3 = r*ones(1,length(yV3));

    x = [-xV-(xC-ds/6) xH xV3-(xC-ds/6) xH xH];
    y = [yV yH yV3 yH2 yH3];

elseif strcmp(letter,'g')

    r = len/4.1;
    dt = ds/r;
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xA(i) = r*cos(theta(i)) - xC;
       yA(i) = r*sin(theta(i)) - yC - len/4;
    end

    yLine = [-len/2:ds:len/2.25 r]; yLine = yLine - yC - len/3;
    xLine = r*ones(1,length(yLine)) - (xC-ds/6);

    yV = [-r:ds:r r];
    xH = yV - xC;
    yH = zeros(1,length(xH)) - yC - 0.85*len;

    x = [xA xLine xH];
    y = [yA yLine yH];

elseif strcmp(letter,'h')

   yLine = -len/2:ds:0-ds/4;
   yLine = [yLine 0 -yLine];
   yLine1 = yLine - yC;
   xLine1 = 1/2*len/2*ones(1,length(yLine1));

   yLine2 = -len/2:ds:0-ds/4;
   yLine2 = [yLine2 0];
   yLine2 = yLine2 - yC;
   xLine2 = 1/2*len/2*ones(1,length(yLine2));

   xH = (-1/2*len/2+ds/2:ds:1/2*len/2-ds/4);
   xH = xH - xC;
   yH = zeros(1,length(xH)) - yC;

   x = [-xLine1-xC xH xLine2-xC];
   y = [yLine1 yH yLine2];

elseif strcmp(letter,'H')
   yLine = -len/2:ds:0-ds/4;
   yLine = [yLine 0 -yLine];
   yLine = yLine - yC;
   xLine = 1/2*len/2*ones(1,length(yLine));
   xH = (-1/2*len/2+ds/2:ds:1/2*len/2-ds/4);
   xH = xH - xC;
   yH = zeros(1,length(xH)) - yC;

   x = [-xLine-xC xH xLine-xC];
   y = [yLine yH yLine];

elseif strcmp(letter,'i')

   yLine = -len/2:ds:0-ds/4;
   yLine = yLine - yC;
   xLine = zeros(1,length(yLine)) - xC;

    r = len/21;
    dt = ds/(2*r);
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xDot(i) = r*cos(theta(i)) - xC;
       yDot(i) = r*sin(theta(i)) - yC + len/7;
    end

    x = [xLine xDot];
    y = [yLine yDot];

elseif strcmp(letter,'k')

   yLine = -len/2:ds:len/2;
   yLine = yLine - yC;
   xLine = zeros(1,length(yLine)) -xC-len/3.9;

   XD = 0:ds:len/sqrt(2);
   YD = zeros(1,length(XD));

   %rotate!
   for i=1:length(XD)
      xDt(i) = XD(i)*cos(pi/4)-YD(i)*sin(pi/4);
      yDt(i) = XD(i)*sin(pi/4)+YD(i)*cos(pi/4);

      xDb(i) = XD(i)*cos(pi/4)-YD(i)*sin(-pi/4);
      yDb(i) = XD(i)*sin(-pi/4)+YD(i)*cos(pi/4);
   end
   xDt = xDt - xC - len/4;
   yDt = yDt - yC;

   xDb = xDb - xC - len/4;
   yDb = yDb - yC;

   x = [xLine(1:ceil(0.75*end)) xDt(1:ceil(2*end/3)) xDb(1:ceil(2*end/3))];
   y = [yLine(1:ceil(0.75*end)) yDt(1:ceil(2*end/3))-len/8 yDb(1:ceil(2*end/3))-len/5.5];

elseif strcmp(letter,'K')

   yLine = -len/2:ds:len/2;
   yLine = yLine - yC;
   xLine = zeros(1,length(yLine)) -xC-len/3.9;

   XD = 0:ds:len/sqrt(2);
   YD = zeros(1,length(XD));

   %rotate!
   for i=1:length(XD)
      xDt(i) = XD(i)*cos(pi/4)-YD(i)*sin(pi/4);
      yDt(i) = XD(i)*sin(pi/4)+YD(i)*cos(pi/4);

      xDb(i) = XD(i)*cos(pi/4)-YD(i)*sin(-pi/4);
      yDb(i) = XD(i)*sin(-pi/4)+YD(i)*cos(pi/4);
   end
   xDt = xDt - xC - len/4.5;
   yDt = yDt - yC;

   xDb = xDb - xC - len/5.5;
   yDb = yDb - yC;

   x = [xLine xDt xDb];
   y = [yLine yDt yDb];

elseif strcmp(letter,'l')

   yLine = -len/2:ds:len/2.25;
   yLine = yLine - yC;
   xLine = 1/2*len/2*ones(1,length(yLine));

   x = -xLine-xC;
   y = yLine;

elseif strcmp(letter,'m')

    r = len/2;
    xH = -r:ds:r;  xH = xH - xC;
    yH = zeros(1,length(xH)) - yC;
    yV = 0:-ds:-len/2; yV = yV - yC;
    xV = -r*ones(1,length(yV)) - xC;
    xV2 = zeros(1,length(yV)) - xC;
    xV3 = r*ones(1,length(yV)) - xC;

    yVS = 0:ds:len/12;
    yVS = yVS - yC;
    xVS = -r*ones(1,length(yVS)) - xC;

    x = [xVS xH xV xV2 xV3];
    y = [yVS yH yV yV yV];

elseif strcmp(letter,'n')

    r = len/4.1;
    yV = [-r:ds:r r];
    xH = yV - xC;
    yH = zeros(1,length(xH)) - yC;

    yV = yV - yC - len/4;
    xV = r*ones(1,length(yV));

    yV2 = 0:ds:len/12;
    yV2 = yV2 - yC;
    xV2 = r*ones(1,length(yV2));

    x = [-xV-(xC-ds/6) -xV2-(xC-ds/6) xH xV-(xC-ds/6)];
    y = [yV yV2 yH yV];

elseif strcmp(letter,'o')

    r = len/4.1;
    dt = ds/r;
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       x(i) = r*cos(theta(i)) - xC;
       y(i) = r*sin(theta(i)) - yC - len/4;
    end

 elseif strcmp(letter,'r')

    r = len/4.1;
    yV = [-r:ds:r r];
    xH = yV - xC;
    yH = zeros(1,length(xH)) - yC;

    yV = yV - yC - len/4;
    xV = r*ones(1,length(yV));

    yV2 = 0:ds:len/12;
    yV2 = yV2 - yC;
    xV2 = r*ones(1,length(yV2));

    yV3 = 0:-ds:-len/10;
    yV3 = yV3 - yC;
    xV3 = r*ones(1,length(yV3));

    x = [-xV-(xC-ds/6) -xV2-(xC-ds/6) xH xV3-(xC-ds/6)];
    y = [yV yV2 yH yV3];

elseif strcmp(letter,'t')

   yLine = -len/2:ds:len/2.25;
   yLine = yLine - yC;
   xLine = zeros(1,length(yLine));

   xH = -len/4:ds:len/4;
   xH = xH-xC;
   yH = zeros(1,length(xH)) - yC + len/9;

   x = [xLine-xC xH];
   y = [yLine yH];

elseif strcmp(letter,'u')

    r = len/4.1;
    yV = [-r:ds:r r];
    xH = yV - xC;
    yH = zeros(1,length(xH)) - yC - len/2;

    yV = yV - yC - len/4;
    xV = r*ones(1,length(yV));

    x = [-xV-(xC-ds/6) xH xV-(xC-ds/6)];
    y = [yV yH yV];

elseif strcmp(letter,'v')

   XD = 0:ds:sqrt(5)*len/4;
   YD = zeros(1,length(XD));

   %rotate!
   ang = atan(2);
   for i=1:length(XD)
      xDr(i) = XD(i)*cos(ang)-YD(i)*sin(ang);
      yDr(i) = XD(i)*sin(ang)+YD(i)*cos(ang);

      xDL(i) = XD(i)*cos(pi-ang)-YD(i)*sin(pi-ang);
      yDL(i) = XD(i)*sin(pi-ang)+YD(i)*cos(pi-ang);
   end
   xDr = xDr - xC;
   yDr = yDr - yC - len/2;

   xDL = xDL - xC;
   yDL = yDL - yC - len/2;

   x = [xDL xDr(2:end)];
   y = [yDL yDr(2:end)];

elseif strcmp(letter,'w')

   XD = 0:ds:sqrt(5)*len/4;
   YD = zeros(1,length(XD));

   %rotate!
   ang = atan(2);
   for i=1:length(XD)
      xDr(i) = XD(i)*cos(ang)-YD(i)*sin(ang);
      yDr(i) = XD(i)*sin(ang)+YD(i)*cos(ang);

      xDL(i) = XD(i)*cos(pi-ang)-YD(i)*sin(pi-ang);
      yDL(i) = XD(i)*sin(pi-ang)+YD(i)*cos(pi-ang);
   end
   xDr = xDr - xC - len/4;
   yDr = yDr - yC - len/2;

   xDL = xDL - xC - len/4;
   yDL = yDL - yC - len/2;

   x = [xDL xDr(2:end) xDL+len/2 xDr(2:end)+len/2];
   y = [yDL yDr(2:end) yDL yDr(2:end)];

elseif strcmp(letter,'W')

   XD = 0:ds:sqrt(5)*len/2;
   YD = zeros(1,length(XD));

   %rotate!
   ang = atan(2);
   for i=1:length(XD)
      xDr(i) = XD(i)*cos(ang)-YD(i)*sin(ang);
      yDr(i) = XD(i)*sin(ang)+YD(i)*cos(ang);

      xDL(i) = XD(i)*cos(pi-ang)-YD(i)*sin(pi-ang);
      yDL(i) = XD(i)*sin(pi-ang)+YD(i)*cos(pi-ang);
   end
   xDr = xDr - xC - len/4;
   yDr = yDr - yC - len/2;

   xDL = xDL - xC - len/4;
   yDL = yDL - yC - len/2;

   x = [xDL xDr(2:floor(end/2)) xDL(1:floor(end/2))+len/2 xDr(2:end)+len/2];
   y = [yDL yDr(2:floor(end/2)) yDL(1:floor(end/2))       yDr(2:end)];

elseif strcmp(letter,'y')

   XD = 0:ds:sqrt(5)*len/4;
   YD = zeros(1,length(XD));

   %rotate!
   ang = atan(2);
   for i=1:length(XD)
      xDr(i) = XD(i)*cos(ang)-YD(i)*sin(ang);
      yDr(i) = XD(i)*sin(ang)+YD(i)*cos(ang);

      xDL(i) = XD(i)*cos(pi-ang)-YD(i)*sin(pi-ang);
      yDL(i) = XD(i)*sin(pi-ang)+YD(i)*cos(pi-ang);

      xB(i) = XD(i)*cos(pi+ang)-YD(i)*sin(pi+ang);
      yB(i) = XD(i)*sin(pi+ang)+YD(i)*cos(pi+ang);
   end
   xDr = xDr - xC;
   yDr = yDr - yC - len/2;

   xDL = xDL - xC;
   yDL = yDL - yC - len/2;

   xB = xB - xC;
   yB = yB - yC - len/2;

   x = [xDL xDr(2:end) xB(2:end)];
   y = [yDL yDr(2:end) yB(2:end)];

elseif strcmp(letter,'!')

   yLine = -len/4:ds:len/2;
   yLine = yLine - yC;
   xLine = zeros(1,length(yLine)) - xC;

    r = len/21;
    dt = ds/(2*r);
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xDot(i) = r*cos(theta(i)) - xC;
       yDot(i) = r*sin(theta(i)) - yC - len/2.1;
    end

    x = [xLine xDot];
    y = [yLine yDot];

elseif strcmp(letter,'?')

    %xT = 0:ds:len/8-ds; xT = xT - xC - len/4;
    %yT = zeros(1,length(xT)) - yC + len/2;

    r = len/4;
    dt = ds/r;
    theta = 1.25*pi/2:-dt:-pi/2;
    for i=1:length(theta)
       xL(i) = 1.5*r*cos(theta(i)) - xC - len/8;
       yL(i) = r*sin(theta(i)) - yC + len/4;
    end

    yV = -ds:-ds:-len/3.25;
    xV = zeros(1,length(yV)) - xC - len/8;

    r = len/21;
    dt = ds/(2*r);
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xDot(i) = r*cos(theta(i)) - xC -len/8;
       yDot(i) = r*sin(theta(i)) - yC - len/2.1;
    end

    x = [xL xV xDot];
    y = [yL yV yDot];

end


function varargout = interp_ungridded(pos_from, pos_to, varargin)

% INTERP_UNGRIDDED computes an interpolation matrix for two clouds of 3-D points
%
% To get the interpolated data, use as
%   [valto] = interp_ungridded(pos_from, pos_to, 'data', valfrom, ...)
% or to get the interpolation matrix itself, use as
%   [interpmat, distmat] = interp_ungridded(pos_from, pos_to, ...)
% where
%   pos_from  Nx3 matrix with the vertex positions
%   pos_to    Mx3 matrix with the vertex positions onto which the data should be interpolated
%
% Optional arguments are specified in key-value pairs and can be
%    data         = NxK matrix with functional data
%    distmat      = NxM matrix with precomputed distances
%    projmethod   = 'nearest', 'sphere_avg', 'sphere_weighteddistance', 'smudge'
%    triout       = triangulation for the second set of vertices
%    sphereradius = scalar
%    power        = scalar, power parameter as in the Inverse Distance Weighting function proposed by Shepard (default = 1).

% Copyright (C) 2007-2018, Jan-Mathijs Schoffelen & Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if nargin<3
  ft_error('Not enough input arguments.');
end

% get the optional arguments
projmethod   = ft_getopt(varargin, 'projmethod');     % required
sphereradius = ft_getopt(varargin, 'sphereradius');   % required for some projection methods
powerparam   = ft_getopt(varargin, 'power', 1);
distmat      = ft_getopt(varargin, 'distmat');        % will be computed if needed and not present
triout       = ft_getopt(varargin, 'triout');
dat          = ft_getopt(varargin, 'data');           % functional data defined at pos_from
inside       = ft_getopt(varargin, 'inside');

hasdat    = ~isempty(dat);
hasinside = ~isempty(inside);
npos_from = size(pos_from, 1);
npos_to   = size(pos_to, 1);
dimres    = sqrt(sum((pos_to(2,:)-pos_to(1,:)).^2,2));

if hasinside
  % convert to boolean vector
  tmp         = false(npos_from,1);
  tmp(inside) = true;
  inside      = tmp;
  clear tmp;
else
  inside      = true(npos_from,1);
end

if isempty(distmat)
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % compute a distance matrix
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  switch projmethod
    case 'nearest'
      if ~isempty(sphereradius)
        ft_warning('sphereradius is not used for projmethod ''nearest''');
      end

      % determine the nearest voxel for each surface point
      ind     = find_nearest(pos_to, pos_from, 5);
      sel     = ind>0;
      indx    = 1:npos_to;
      distmat = sparse(indx(sel), ind(sel), ones(size(ind(sel))), npos_to, npos_from);

    case {'sphere_avg', 'sphere_weighteddistance'}
      if isempty(sphereradius)
        ft_error('sphereradius should be specified');
      end

      % compute the distance between voxels and each surface point
      dpos_fromsq = sum(pos_from.^2,2); % squared distance to origin
      dpos_tosq   = sum(pos_to.^2,2); % squared distance to origin
      maxnpnt = double(npos_to*ceil(4/3*pi*(sphereradius/max(dimres))^3)); % initial estimate of nonzero entries
      maxnpnt = min(maxnpnt, npos_to*npos_from);
      val   = nan(maxnpnt, 1);
      indx1 = nan(maxnpnt, 1);
      indx2 = nan(maxnpnt, 1);
      cnt = 1;
      for j = 1:npos_to

        % d   = dpos_tosq(j) + dpos_fromsq - 2 * pos_from * pos_to(j,:)';
        % sel = find(d<sphereradius.^2);
        % the following lines are equivalent to the previous 2 but uses fewer flops
        d    = sqrt(dpos_fromsq - 2 * pos_from * pos_to(j,:)' + dpos_tosq(j));
        sel  = find(d < sphereradius & inside);

        nsel = numel(sel);
        if nsel>0
          indx1(cnt:(cnt+nsel-1)) = j(ones(nsel,1));
          indx2(cnt:(cnt+nsel-1)) = sel(:);
          val(cnt:(cnt+nsel-1))   = d(sel) + eps('double');
          cnt = cnt + nsel;
        end
      end
      indx1(isnan(indx1)) = [];
      indx2(isnan(indx2)) = [];
      val(isnan(val))     = [];
      distmat = sparse(indx1, indx2, val, npos_to, npos_from);

    case 'smudge'
      if isempty(triout)
        ft_error('the ''smudge'' method needs a triangle definition');
      end

      [datin, loc] = ismember(pos_to, pos_from, 'rows');  % note that small numerical errors can cause them to be different
      [datout, S1] = smudge(datin, triout, 6);            % FIXME 6 is number of iterations, improve here

      sel = find(datin);
      S2  = sparse(sel(:), loc(datin), ones(npos_from,1), npos_to, npos_from);
      distmat = S1 * S2;

    otherwise
      ft_error('unsupported projection method');
  end % case projmethod
end % if isempty distmat

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% do something with the distance matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch projmethod
  case 'nearest'
    projmat         = distmat;

  case 'sphere_avg'
    projmat         = distmat;
    [ind1, ind2]    = find(projmat);
    nnz             = full(sum(spones(projmat),2));
    for k = 1:length(ind1)
      projmat(ind1(k),ind2(k)) = 1./nnz(ind1(k));
    end

  case 'sphere_weighteddistance'
    projmat         = distmat;
    [ind1, ind2, d] = find(projmat);
    projmat         = sparse(ind1, ind2, d.^-powerparam, npos_to, npos_from);
    [ind1, ind2, d] = find(projmat);
    normnz          = full(sum(projmat, 2));
    projmat         = sparse(ind1, ind2, d./normnz(ind1), npos_to, npos_from);

  case 'smudge'
    projmat = distmat;

  otherwise
    ft_error('unsupported projection method');
end  % case projmethod

if hasdat
  % return the interpolated values
  varargout{1} = projmat * dat;
else
  % return the interpolation and the distance matrix
  varargout{1} = projmat;
  varargout{2} = distmat;
end


function asa183_test03 ( )

%*****************************************************************************80
%
%% ASA183_TEST03 tests R8_RANDOM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ASA183_TEST03\n' );
  fprintf ( 1, '  Show how the seeds used by R8_RANDOM,\n' );
  fprintf ( 1, '  which change on each step, can be reset to\n' );
  fprintf ( 1, '  restore any part of the sequence.\n' );

  s1_save = 12345;
  s2_save = 34567;
  s3_save = 56789;

  s1 = s1_save;
  s2 = s2_save;
  s3 = s3_save;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Begin sequence with following seeds:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  S1 = %d\n', s1 );
  fprintf ( 1, '  S2 = %d\n', s2 );
  fprintf ( 1, '  S3 = %d\n', s3 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '         I      R                 S1        S2        S3\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ r, s1, s2, s3 ] = r8_random ( s1, s2, s3 );
    fprintf ( 1, '  %8d  %14f  %8d  %8d  %8d\n', i, r, s1, s2, s3 );

    if ( i == 5 )
      s1_save = s1;
      s2_save = s2;
      s3_save = s3;
    end

  end

  s1 = s1_save;
  s2 = s2_save;
  s3 = s3_save;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Restart the sequence, using the seeds\n' );
  fprintf ( 1, '  produced after step 5:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '         I      R                 S1        S2        S3\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ r, s1, s2, s3 ] = r8_random ( s1, s2, s3 );
    fprintf ( 1, '  %8d  %14f  %8d  %8d  %8d\n', i, r, s1, s2, s3 );

  end

  return
end


function MDP_DEM_Mixed_Models_Movement
% This demo illustrates a series of computational pathologies as elicited
% during a synthetic neurological examination. This focuses upon an
% examination of the biceps reflex, and a simple coordination task. Each of
% these are simulated for an arm that may move in three dimensions through
% internal and external rotation of the shoulder, flexion and extension of
% the shoulder, and flexion and extension of the elbow. These dynamics play
% out through an active Bayesian filtering scheme, where a series of
% attracting points draw the hand to different locations in 3D space. The
% selection of these attracting points involves a hierarhical Markov
% Decision Process, which identifies these sequences based upon the prior
% belief that (1) the sequence will minimise expected free energy and (2)
% the sequence is consistent with the trajectory predicted by the highest
% (contextual) level of the model.
%__________________________________________________________________________

rng default

% REFLEXES
%==========================================================================
% Demonstration 1 - Tendon reflexes (healthy, pyramidal lesion,
% cerebellar lesion)

% Specify generative model and process
%--------------------------------------------------------------------------

a = 2;                                      % length of upper arm
b = 1.5;                                    % length of forearm
c = [-1;-1;-1];                             % location of shoulder

DEM        = DEM_MDP_Movement_Disorders_GM; % Continuous state generative model
x = [0;-1;1;0;0;0];                         % Initial joint positions

DEM.G(1).g = @(x,v,a,P) Gg1Reflex(x,v,a,P); % Generative process allows for tendon tap
DEM.M(1).x = x;
DEM.G(1).x = x;
DEM.U = [MDP_DEM_MD_transform(x(1:3),a,b,c);0;0;0]*ones(1,64);
DEM.C = zeros(size(DEM.U));
DEM.C(3,10:12) = -0.25;                     % Transient stimulation of type II sensory afferents

% Solve model and plot healthy reflexes
%--------------------------------------------------------------------------
dem = spm_ADEM(DEM);

% Animate
spm_figure('GetWin','Figure 1'); clf
opengl('software')
for i = 1:size(dem.pU.x{1},2)
    MDP_DEM_MD_plot_arm(full(dem.pU.x{1}(1:3,i)),dem.M(1).pE.b1,dem.M(1).pE.b2,[-1;-1;-1]), hold on
    light
    view(10,0)
    zlim([-4 1])
    xlim([-1.2 2.1])
    drawnow
    hold off
    title('Normal')
end

clear dem

% Upper motor lesion
%--------------------------------------------------------------------------
DEM.M(1).V = exp(5);        % Overestimate precision

dem = spm_ADEM(DEM);

% Animate
spm_figure('GetWin','Figure 1'); clf
for i = 1:size(dem.pU.x{1},2)
    MDP_DEM_MD_plot_arm(full(dem.pU.x{1}(1:3,i)),dem.M(1).pE.b1,dem.M(1).pE.b2,[-1;-1;-1]), hold on
    light
    view(10,0)
    zlim([-4 1])
    xlim([-1.2 2.1])
    drawnow
    hold off
    title('Pyramidal')
end

clear dem

% Cerebellar
%--------------------------------------------------------------------------
DEM.M(1).V = exp(4);        % Restore sensory precision
DEM.M(1).E.s = 3/4;         % Overestimate smoothness

dem = spm_ADEM(DEM);

spm_figure('GetWin','Figure 1'); clf
for i = 1:size(dem.pU.x{1},2)
    MDP_DEM_MD_plot_arm(full(dem.pU.x{1}(1:3,i)),dem.M(1).pE.b1,dem.M(1).pE.b2,[-1;-1;-1]), hold on
    light
    view(10,0)
    zlim([-4 1])
    xlim([-1.2 2.1])
    drawnow
    hold off
    title('Cerebellar')
end

clear all

% COORDINATION
%==========================================================================
% Demonstration 2 - Mixed models (healthy)

% Specify discrete generatve model and mapping between continuous and
% discrete
%--------------------------------------------------------------------------

% Locations
%--------------------------------------------------------------------------

% 3D coordinates for three targets
L1  =   [   1;   -1; -3];
L2  =   [  -1;    1; -3];
L3  =   [   0;    0; -1];

% Intermediate (fictive) attracting points)
for i = 1:4
    L{i} = L1 + (i-1)*(L2 - L1)/4;
end
for i = 5:8
    L{i} = L2 + (i-5)*(L3 - L2)/4;
end
for i = 9:12
    L{i} = L3 + (i-9)*(L1 - L3)/4;
end

% MDP outcomes to DEM causes
%--------------------------------------------------------------------------
N  = 14;
nl = length(L);
nh = 3;

for i = 1:nh
    for j = 1:nl
        for k = 1:2
            c           = [L{j}; sparse(i,1,1,nh,1)];
            u           = [L{j}; sparse(i,1,1,nh,1)];
            demi.U{i,j,k} = u*ones(1,N);
            demi.C{i,j,k} = c*ones(1,N);
        end
    end
end

o     = [1 1 1];
O{1}  = spm_softmax(ones(nh,1));
O{2}  = spm_softmax(ones(nl,1));
O{3}  = spm_softmax(ones(2,1));

% generative model (Continuous)
%==========================================================================
DEM      = DEM_MDP_Movement_Disorders_GM;
DEM      = spm_MDP_DEM(DEM,demi,O,o);

% generative model (Discrete)
%==========================================================================
mdp      = MDP_DEM_Movement_Disorders_GM_1;
mdp.demi = demi;
mdp.DEM  = DEM;

mdp      = MDP_DEM_Movement_Disorders_GM_2(mdp);

% invert or solve
%--------------------------------------------------------------------------
rng default
MDP  = spm_MDP_check(mdp);
MDP  = spm_MDP_VB_X(MDP);

% illustrate belief updating - discrete
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2'); clf
spm_MDP_VB_LFP(MDP,[],2);

% illustrate belief updating - continuous (last movement)
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 3'); clf;
spm_DEM_qU(MDP.mdp(end).dem(end).qU)

% illustrate movement
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 4'); clf

[Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L);

spm_figure('GetWin','Figure 4'); clf
plot3(Q(1,:),Q(2,:),Q(3,:),'LineWidth',2,'Color','k'), hold on
for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        h = (m - 1)*length(MDP.mdp(m).dem) + j;
        H = length(MDP.mdp)*length(MDP.mdp(m).dem);
        surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,2),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'FaceAlpha',h/(1.5*H))
    end
end

light
axis equal
spm_figure('GetWin','Figure 5'); clf
subplot(5,1,1)
plot(0.05*(0:size(R,2)-1),R' - repmat(R(:,1)',size(R,2),1))
title('Normal')

% Demonstration 3 - Mixed models (Pyramidal versus extrapyramidal)
%--------------------------------------------------------------------------
% Pyramidal

mdp.MDP.DEM.M(1).V = exp(5); % Overestimate precision

% invert or solve
%--------------------------------------------------------------------------
rng default
MDP  = spm_MDP_check(mdp);
MDP  = spm_MDP_VB_X(MDP);

spm_figure('GetWin','Figure 6'); clf

[Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L);

spm_figure('GetWin','Figure 6'); clf
plot3(Q(1,:),Q(2,:),Q(3,:),'LineWidth',2,'Color','k'), hold on
for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        h = (m - 1)*length(MDP.mdp(m).dem) + j;
        H = length(MDP.mdp)*length(MDP.mdp(m).dem);
        surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,2),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'FaceAlpha',h/(1.5*H))
    end
end
light
axis equal

spm_figure('GetWin','Figure 5');
subplot(5,1,2)
plot(0.05*(0:size(R,2)-1),R' - repmat(R(:,1)',size(R,2),1))
title('Pyramidal')

% Extrapyramidal
%--------------------------------------------------------------------------
mdp.MDP.DEM.M(1).V = exp(4);
mdp.MDP.beta = 50;

% invert or solve
%--------------------------------------------------------------------------
rng default
MDP  = spm_MDP_check(mdp);
MDP  = spm_MDP_VB_X(MDP);

% illustrate movement
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 8'); clf

[Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L);

spm_figure('GetWin','Figure 8'); clf
plot3(Q(1,:),Q(2,:),Q(3,:),'LineWidth',2,'Color','k'), hold on
for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        h = (m - 1)*length(MDP.mdp(m).dem) + j;
        H = length(MDP.mdp)*length(MDP.mdp(m).dem);
        surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,2),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'FaceAlpha',h/(1.5*H))
    end
end
light
axis equal

spm_figure('GetWin','Figure 5');
subplot(5,1,3)
plot(0.05*(0:size(R,2)-1),R' - repmat(R(:,1)',size(R,2),1))
title('Extrapyramidal')

% Cerebellar
%--------------------------------------------------------------------------
mdp.MDP.beta = 16;
mdp.MDP.DEM.M(1).E.s = 3/4;

% invert or solve
%--------------------------------------------------------------------------
rng default
MDP  = spm_MDP_check(mdp);
MDP  = spm_MDP_VB_X(MDP);

% illustrate movement
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 9'); clf

[Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L);

spm_figure('GetWin','Figure 9'); clf
plot3(Q(1,:),Q(2,:),Q(3,:),'LineWidth',2,'Color','k'), hold on
for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        h = (m - 1)*length(MDP.mdp(m).dem) + j;
        H = length(MDP.mdp)*length(MDP.mdp(m).dem);
        surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,2),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'FaceAlpha',h/(1.5*H))
    end
end
light
axis equal

spm_figure('GetWin','Figure 5');
subplot(5,1,4)
plot(0.05*(0:size(R,2)-1),R' - repmat(R(:,1)',size(R,2),1))
title('Cerebellar')

% Executive
%--------------------------------------------------------------------------
mdp.MDP.DEM.M(1).E.s = 1/2;
mdp.A{3} = ones(size(mdp.A{3})); % Disconnect higher level

% invert or solve
%--------------------------------------------------------------------------
rng default
MDP  = spm_MDP_check(mdp);
MDP  = spm_MDP_VB_X(MDP);

% illustrate movement
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 10'); clf

[Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L);

spm_figure('GetWin','Figure 10'); clf
plot3(Q(1,:),Q(2,:),Q(3,:),'LineWidth',2,'Color','k'), hold on
for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        h = (m - 1)*length(MDP.mdp(m).dem) + j;
        H = length(MDP.mdp)*length(MDP.mdp(m).dem);
        surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,2),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'FaceAlpha',h/(1.5*H))
    end
end
light
axis equal

spm_figure('GetWin','Figure 5');
subplot(5,1,5)
plot(0.05*(0:size(R,2)-1),R' - repmat(R(:,1)',size(R,2),1))
title('Executive')

function MDP = MDP_DEM_Movement_Disorders_GM_1

% First level
%--------------------------------------------------------------------------

% Initial states
%--------------------------------------------------------------------------
D{1} = ones(3,1); % Location of visual cue
D{2} = zeros(12,1);  % Location of arm
D{2}(1) = 1;

% Likelihood
%--------------------------------------------------------------------------
for f1 = 1:numel(D{1})
    for f2 = 1:numel(D{2})
        A{1}(f1,f1,f2) = 1; % Vision (target)
        A{2}(f2,f1,f2) = 1; % Vision + Proprioception (arm)
        A{3}(2,f1,f2)  = (f1 == ((f2-1)/4)+1);  % At target
        A{3}(1,f1,f2)  = ~(f1 == ((f2-1)/4)+1); % Not at target
    end
end

% Transitions
%--------------------------------------------------------------------------
B{1} = eye(3);

for k = 1:length(D{2})
    for j = 1:length(D{2})
        if k == j
            B{2}(k,j,1) = 1; % Stay still
        elseif (k - j) == 1
            B{2}(k,j,2) = 1; % Move to next point
        elseif (j - k) == 1
            B{2}(k,j,3) = 1; % Move to previous point
        end
    end
end

B{2}(1,end,2) = 1;
B{2}(end,1,3) = 1;

% Preferences
%--------------------------------------------------------------------------
C{1} = zeros(3,1);
C{2} = zeros(12,1);
C{3} = [-3 3]';

% Policies
%--------------------------------------------------------------------------
V(:,:,1) = ones(8,3); % (time, policy, factor)
V(:,:,2) = [1 2 3;
    1 2 3;
    1 2 3;
    1 2 3;
    1 1 1;
    1 1 1;
    1 1 1;
    1 1 1];

E = [1 0.5 0.5]';

% First level MDP
%--------------------------------------------------------------------------
MDP.A = A;
MDP.B = B;
MDP.C = C;
MDP.D = D;
MDP.E = E;
MDP.V = V;
MDP.T = 9;
MDP.chi = -exp(64);
MDP.beta = 16;
% MDP.beta = 50;
MDP     = spm_MDP_check(MDP);

function MDP = MDP_DEM_Movement_Disorders_GM_2(mdp)

% First level
%--------------------------------------------------------------------------
MDP.MDP = mdp;

% Second level
%--------------------------------------------------------------------------

% Initial states
%--------------------------------------------------------------------------
D{1} = ones(3,1); % Target location
D{2} = zeros(9,1); % Initial locations x policies at lower level ({L1,P1},{L1,P2},{L1,P3},{L2,P1},...)
D{2}([1 2 3]) = 1;

% Likelihood
%--------------------------------------------------------------------------
for f1 = 1:length(D{1})
    for f2 = 1:length(D{2})
        A{1}(f1,f1,f2) = 1; % Target location 1st level

        if f2 < 4
            A{2}(11,f1,f2) = 0.03;  % Start location 1st level
            A{2}(12,f1,f2) = 0.12;
            A{2}(1,f1,f2)  = 0.7;
            A{2}(2,f1,f2)  = 0.12;
            A{2}(3,f1,f2)  = 0.03;

        elseif f2 <7
            A{2}(3,f1,f2) = 0.03;
            A{2}(4,f1,f2) = 0.12;
            A{2}(5,f1,f2)  = 0.7;
            A{2}(6,f1,f2)  = 0.12;
            A{2}(7,f1,f2)  = 0.03;
        else
            A{2}(7,f1,f2) = 0.03;
            A{2}(8,f1,f2) = 0.12;
            A{2}(9,f1,f2)  = 0.7;
            A{2}(10,f1,f2)  = 0.12;
            A{2}(11,f1,f2)  = 0.03;
        end

        A{3}(1,f1,f2)  = (rem(f2+2,3) == 0) + 0.5;  % Policy 1st level (slight bias towards 'stay still' policy)
        A{3}(2,f1,f2)  = (rem(f2+1,3) == 0);
        A{3}(3,f1,f2)  = (rem(f2,3) == 0);
    end
end

% Transitions
%--------------------------------------------------------------------------
B{1}(:,:,1) = ones(length(D{1}));
B{1}(:,:,2) = ones(length(D{1})); % Action that causes no change to prevent entering HMM mode
I = eye(3);
B{2} = [I, [I(3,:);I(1:2,:)],([I(3,:);I(1:2,:)])']/3;
B{2} = kron(B{2},ones(3,1));

% Preferences
%--------------------------------------------------------------------------
C{1} = zeros(3,1);
C{2} = zeros(12,1);
C{3} = zeros(3,1);

% Second level MDP
%--------------------------------------------------------------------------
MDP.A = A;
MDP.B = B;
MDP.C = C;
MDP.D = D;
MDP.T = 4;

MDP.link  = [1 0 0;
    0 1 0];
MDP.linkE = [0 0 1];

MDP     = spm_MDP_check(MDP);

function DEM = DEM_MDP_Movement_Disorders_GM
% hidden states
%--------------------------------------------------------------------------
x   = [0;-1;0.5;0;0;0]; % shoulder rotation, flexion, elbow flexion (position and velocity)

% and actions
%--------------------------------------------------------------------------
a   = [0;0;0];

% parameters
%--------------------------------------------------------------------------
P.kv = 2;              % damping
P.a = a;
P.b1 = 2;
P.b2 = 1.5;

M(1).pE = P;

% recognition model
%==========================================================================
M(1).E.s = 1/2;                               % smoothness: default 1/2 [3/4 for lesion]
M(1).E.n = 4;                                 % order of
M(1).E.d = 2;                                 % generalised motion

% level 1
%--------------------------------------------------------------------------
M(1).f  = @(x,v,P) Mf1(x,v,P);                % plant dynamics
M(1).g  = @(x,v,P) Mg1(x,v,P);                % prediction

M(1).x  = x;                                  % hidden states
M(1).V  = exp(4);                             % error precision (g)[4] [3/8 ?]
M(1).W  = exp(2);                             % error precision (f) [2]

% level 2:
%--------------------------------------------------------------------------
M(2).v  = [0;0;0;0;0;0];                      % priors (attracting point)
M(2).V  = exp(15);

% generative model
%==========================================================================

G(1).E.s = 1/2;                               % smoothness: default 1/2
G(1).E.n = 4;                                 % order of
G(1).E.d = 2;                                 % generalised motion

% first level
%--------------------------------------------------------------------------
G(1).f  = @(x,v,a,P) Gf1(x,v,a,P);
G(1).g  = @(x,v,a,P) Gg1(x,v,a,P);
G(1).x  = x;                                  % hidden states
G(1).V = exp(16);
% G(1).V  = exp([4*ones(1,6) 16*ones(1,6)]);    % error precision [16]
G(1).W  = exp(16);                            % error precision
G(1).pE = P;

% second level
%--------------------------------------------------------------------------
G(2).v  = [0;0;0;0;0;0];                      % exogenous forces
G(2).a  = spm_vec(a);                         % action forces
G(2).V  = exp(16);

DEM.G = G;
DEM.M = M;

% Equations for DEM model and process
%==========================================================================
function f = Mf1(x,v,P)
a = P.b1; % length of upper arm
b = P.b2; % length of forearm
c = [-1;-1;-1]; % location of shoulder

f = [x(4:6);MDP_DEM_MD_dthetadt(v(1:3),x,a,b,c) - P.kv*x(4:6)];

function g = Mg1(x,v,P)
a = P.b1; % length of upper arm
b = P.b2; % length of forearm
c = [-1;-1;-1]; % location of shoulder

g = [x;MDP_DEM_MD_transform(x(1:3),a,b,c);v(4:6)]; % proprioceptive, visual (hand), visual (target)

function f = Gf1(x,~,a,P)
f = [x(4:6);a  - P.kv*x(4:6)];

function g = Gg1(x,v,~,P)
a = P.b1; % length of upper arm
b = P.b2; % length of forearm
c = [-1;-1;-1]; % location of shoulder

g = [x;MDP_DEM_MD_transform(x(1:3),a,b,c);v(4:6)]; % proprioceptive, visual (hand), visual (target)

function g = Gg1Reflex(x,v,~,P)
a = P.b1; % length of upper arm
b = P.b2; % length of forearm
c = [-1;-1;-1]; % location of shoulder

g = [x + [zeros(3,1);v(1:3)];MDP_DEM_MD_transform(x(1:3),a,b,c);v(4:6)]; % proprioceptive, visual (hand), visual (target)

% Additional routines (plotting/coordinate transforms)
%==========================================================================
function y = MDP_DEM_MD_transform(x,a,b,c)
% converts angles to hand position in euclidean space
% x = angle of shoulder rotation, flexion, elbow flexion

y1 = c + [a*cos(x(2))*cos(x(1));
    a*cos(x(2))*sin(x(1));
    a*sin(x(2))];

y = y1 + [b*cos(x(3)+x(2))*cos(x(1));
    b*cos(x(3)+x(2))*sin(x(1));
    b*sin(x(3)+x(2))];

function MDP_DEM_MD_plot_arm(x,a,b,c)

y1 = c + [a*cos(x(2))*cos(x(1));
    a*cos(x(2))*sin(x(1));
    a*sin(x(2))];

y = y1 + [b*cos(x(3)+x(2))*cos(x(1));
    b*cos(x(3)+x(2))*sin(x(1));
    b*sin(x(3)+x(2))];

[S1, S2, S3] = sphere;

surf(S1*0.2+c(1),S2*0.2+c(2),S3*0.2+c(3),'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7), hold on
surf(S1*0.1+y1(1),S2*0.1+y1(2),S3*0.1+y1(3),'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7)
surf(S1*0.07+y(1),S2*0.07+y(2),S3*0.07+y(3),'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7)

plot3([c(1) y1(1)],[c(2) y1(2)],[c(3) y1(3)],'LineWidth',6,'Color',[0.1 0.1 0.8])
plot3([y1(1) y(1)],[y1(2) y(2)],[y1(3) y(3)],'LineWidth',5,'Color',[0.1 0.1 0.8])
plot3([y1(1) y(1)],[y1(2) y(2)],[y1(3)-0.1 y(3)-0.1],'LineWidth',4,'Color',[0.1 0.1 0.8])

MDP_DEM_Movements_Hand([pi,pi/2 + atan((y(3)-y1(3))/(sqrt((y(1) - y1(1))^2+(y(2) - y1(2))^2))),x(1)],0.25,y)
axis equal

function y = MDP_DEM_MD_dthetadt(v,x,a,b,c)

y1 = c + [a*cos(x(2))*cos(x(1));
    a*cos(x(2))*sin(x(1));
    a*sin(x(2))];

y2 = y1 + [b*cos(x(3)+x(2))*cos(x(1));
    b*cos(x(3)+x(2))*sin(x(1));
    b*sin(x(3)+x(2))];

y(1) = [v - y2]'*[-((a*cos(x(2)) + b*cos(x(3)+x(2))*sin(x(1))));
    ((a*cos(x(2)) + b*cos(x(3)+x(2))*cos(x(1))));
    0];

y(2) = [v - y2]'*[-((a*sin(x(2)) + b*sin(x(3) + x(2)))*cos(x(1)));
    -((a*sin(x(2)) + b*sin(x(3) + x(2))*sin(x(1))));
    (a*cos(x(2)) +(b*cos(x(2) + x(3))))];

y(3) = [v - y2]'*[-(a*sin(x(3) + x(2))*cos(x(1)));
    -(a*sin(x(3)+ x(2))*sin(x(1)));
    (b*cos(x(2) + x(3)))];

y = 0.05*y'; % 0.05

function MDP_DEM_Movements_Hand(a,s,l)
% angle    - a
% scale    - s
% location - l

% Carpals (as a single sphere)
%--------------------------------------------------------------------------
[X,Y,Z] = sphere;
Rx = makehgtform('xrotate',a(1));
Ry = makehgtform('yrotate',a(2));
Rz = makehgtform('zrotate',a(3));
T  = makehgtform('translate',l);
S  = makehgtform('scale',s);
q = hgtransform;

LW = 5;

% 2nd finger
%--------------------------------------------------------------------------

% MCP
%--------------------------------------------------------------------------
plot3([0 0],[0 0],[0 1.1],'b','LineWidth',LW,'Parent',q)
surf(0.1*X, 0.1*Y ,0.1*Z + 1.1,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% PIP
%--------------------------------------------------------------------------
plot3([0 0],[0 0.4],[1.1 1.4],'b','LineWidth',LW,'Parent',q)
surf(0.1*X, 0.1*Y + 0.4 ,0.1*Z + 1.4,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% MIP
%--------------------------------------------------------------------------
plot3([0 0],[0.4 0.7],[1.4 1.5],'b','LineWidth',LW,'Parent',q)
surf(0.1*X, 0.1*Y + 0.7 ,0.1*Z + 1.5,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% DIP
%--------------------------------------------------------------------------
plot3([0 0],[0.7 1],[1.5 1.6],'b','LineWidth',LW,'Parent',q)
surf(0.1*X, 0.1*Y + 1 ,0.1*Z + 1.6,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)

% 3rd finger
%--------------------------------------------------------------------------

% MCP
%--------------------------------------------------------------------------
plot3([-0.1 -0.2],[0 0],[0 1.05],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.2, 0.1*Y ,0.1*Z + 1.05,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% PIP
%--------------------------------------------------------------------------
plot3([-0.2 -0.2],[0 0.3],[1.05 1.4],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.2, 0.1*Y + 0.3 ,0.1*Z + 1.4,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% MIP
%--------------------------------------------------------------------------
plot3([-0.2 -0.2],[0.3 0.6],[1.4 1.5],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.2, 0.1*Y + 0.6 ,0.1*Z + 1.5,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% DIP
%--------------------------------------------------------------------------
plot3([-0.2 -0.2],[0.6 0.9],[1.5 1.6],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.2, 0.1*Y + 0.9 ,0.1*Z + 1.6,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)

% 4th finger
%--------------------------------------------------------------------------

% MCP
%--------------------------------------------------------------------------
plot3([-0.2 -0.4],[0 0],[0 1],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.4, 0.1*Y ,0.1*Z + 1,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% PIP
%--------------------------------------------------------------------------
plot3([-0.4 -0.4],[0 0.2],[1 1.4],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.4, 0.1*Y + 0.2 ,0.1*Z + 1.4,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% MIP
%--------------------------------------------------------------------------
plot3([-0.4 -0.4],[0.2 0.5],[1.4 1.5],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.4, 0.1*Y + 0.5 ,0.1*Z + 1.5,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% DIP
%--------------------------------------------------------------------------
plot3([-0.4 -0.4],[0.5 0.8],[1.5 1.6],'b','LineWidth',LW,'Parent',q)
surf(0.1*X - 0.4, 0.1*Y + 0.8 ,0.1*Z + 1.6,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)

% 1st finger
%--------------------------------------------------------------------------

% MCP
%--------------------------------------------------------------------------
plot3([0.1 0.2],[0 0],[0 1],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.2, 0.1*Y ,0.1*Z + 1,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% PIP
%--------------------------------------------------------------------------
plot3([0.2 0.2],[0 0.3],[1 1.4],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.2, 0.1*Y + 0.3 ,0.1*Z + 1.4,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% MIP
%--------------------------------------------------------------------------
plot3([0.2 0.2],[0.3 0.6],[1.4 1.5],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.2, 0.1*Y + 0.6 ,0.1*Z + 1.5,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% DIP
%--------------------------------------------------------------------------
plot3([0.2 0.2],[0.6 0.9],[1.5 1.6],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.2, 0.1*Y + 0.9 ,0.1*Z + 1.6,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)

% Thumb
%--------------------------------------------------------------------------

% MCP
%--------------------------------------------------------------------------
plot3([0.2 0.5],[0 0.05],[0 0.4],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.5, 0.1*Y +0.05,0.1*Z + 0.4,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% PIP
%--------------------------------------------------------------------------
plot3([0.5 0.5],[0.05 0.45],[0.4 0.6],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.5, 0.1*Y +0.45,0.1*Z + 0.6,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)
% DIP
%--------------------------------------------------------------------------
plot3([0.5 0.45],[0.45 0.6],[0.6 0.62],'b','LineWidth',LW,'Parent',q)
surf(0.1*X + 0.45, 0.1*Y +0.6,0.1*Z + 0.62,'Facecolor','b','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7,'Parent',q)

set(q,'Matrix',T*Rz*Rx*Ry*S);

function [Q, R, X, Y, Z] = MDP_DEM_Movement_Disorders_Animate(MDP,L)
opengl('software')
Q = [];
R = [];

for m = 1:length(MDP.mdp)
    for j = 1:length(MDP.mdp(m).dem)
        for i = 2:size(MDP.mdp(m).dem(j).pU.x{1},2)
            MDP_DEM_MD_plot_arm(full(MDP.mdp(m).dem(j).pU.x{1}(1:3,i)),MDP.mdp(m).dem(j).M(1).pE.b1,MDP.mdp(m).dem(j).M(1).pE.b2,[-1;-1;-1]), hold on
            [X,Y,Z] = sphere;
            surf(0.1*X + MDP.mdp(m).dem(j).qU.v{2}(1,i),0.1*Y + MDP.mdp(m).dem(j).qU.v{2}(2,i),0.1*Z + MDP.mdp(m).dem(j).qU.v{2}(3,i),'Facecolor','r','EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7)
            for k = 1:3
                surf(0.2*X + L{(k-1)*4+1}(1),0.2*Y + L{(k-1)*4+1}(2),0.2*Z + L{(k-1)*4+1}(3),'Facecolor',[1 1 1]-MDP.mdp(m).dem(j).Y(9+k,i),'EdgeColor','none','FaceLighting','gouraud','AmbientStrength',0.7)
            end
            axis equal
            light
            drawnow
            hold off

            Q(:,end+1) = MDP_DEM_MD_transform(full(MDP.mdp(m).dem(j).pU.x{1}(1:3,i)),MDP.mdp(m).dem(j).M(1).pE.b1,MDP.mdp(m).dem(j).M(1).pE.b2,[-1;-1;-1]);
        end
        R = [R full(MDP.mdp(m).dem(j).pU.x{1}(1:3,:))];
    end
end


%%*****************************************************************
%% linsysolvefun: Solve H*x = b
%%
%% x = linsysolvefun(L,b)
%% where L contains the triangular factors of H.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

  function x = linsysolvefun(L,b)

  x = zeros(size(b));
  for k=1:size(b,2)
     if strcmp(L.matfct_options,'chol')
        x(L.perm,k) = mextriang(L.R, mextriang(L.R,b(L.perm,k),2) ,1);
        %% x(L.perm,k) = L.R \ (b(L.perm,k)' / L.R)';
     elseif strcmp(L.matfct_options,'spchol')
        x(L.perm,k) = mextriangsp(L.Rt,mextriangsp(L.R,b(L.perm,k),2),1);
     elseif strcmp(L.matfct_options,'ldl')
	x(L.p,k) = ((L.D\ (L.L \ b(L.p,k)))' / L.L)';
     elseif strcmp(L.matfct_options,'spldl')
        btmp = b(:,k).*L.s;
        xtmp(L.p,1) = L.Lt\ (L.D\ (L.L \ btmp(L.p)));
	x(:,k) = xtmp.*L.s;
     elseif strcmp(L.matfct_options,'lu')
        x(:,k) = L.U \ (L.L \ b(L.p,k));
     elseif strcmp(L.matfct_options,'splu')
	btmp = b(:,k)./L.s;
        x(L.q,k) = L.U \ (L.L \ (btmp(L.p)));
     end
  end
%%*************************************************************************