function CPD = root_gaussian_CPD(bnet, self, mu, Sigma, mu0, n0, alpha0, beta0)
% ROOT_GAUSSIAN_CPD Make an unconditional Gaussian distrib.
%
% CPD = root_gaussian_CPD(bnet, self, mu, Sigma)
% This defines the distribution Y ~ N(mu, Sigma),
% Pass in [] to generate a default random value for a parameter.
%
% CPD = root_gaussian_CPD(bnet, self, [], [], mu0, n0, alpha0, beta0)
% defines a Normal-Wishart prior over the parameters:
%   P(mu | lambda) = N(mu | mu0, n0*lambda)
%   P(lambda) = Wishart(lambda | alpha0, beta0)
% where lambda = inv(Sigma) is the precision matrix of mu.
% n0 is a scale factor, beta0 is a precision matrix.
% Pass in [] to generate a default value for a hyperparameter.
% mu and Sigma will be set to their prior expected values.
% See "Bayesian Theory", Bernardo and Smith (2000), p441.

if nargin==0
  % This occurs if we are trying to load an object from a file.
  CPD = init_fields;
  CPD = class(CPD, 'root_gaussian_CPD', generic_CPD(0));
  return;
elseif isa(bnet, 'root_gaussian_CPD')
  % This might occur if we are copying an object.
  CPD = bnet;
  return;
end
CPD = init_fields;

ns = bnet.node_sizes;
d = ns(self);

if nargin < 5,
  prior = [];
  if isempty(mu), mu = randn(d, 1); end
  if isempty(Sigma), Sigma = eye(d); end
else
  if isempty(mu0), mu0 = zeros(d, 1); end
  if isempty(n0), n0 = 0.1; end
  if isempty(alpha0), alpha0 = (d-1)/2 + 1; end % Wishart requires 2 alpha > d-1
  if isempty(beta0), beta0 = eye(d); end

  prior.mu = mu0;
  prior.n = n0;
  prior.alpha = alpha0;
  prior.beta = beta0;

  % set params to their mean
  mu = prior.mu;
  Sigma = prior.beta/prior.alpha; % mean of Wishart is E[lambda] = alpha*inv(beta)
end

CPD.self = self;
CPD.mu = mu;
CPD.Sigma = Sigma;
CPD.prior = prior;

clamped = 0;
CPD = class(CPD, 'root_gaussian_CPD', generic_CPD(clamped));

%%%%%%%%%%%

function CPD = init_fields()
% This ensures we define the fields in the same order
% no matter whether we load an object from a file,
% or create it from scratch. (Matlab requires this.)

CPD.self = [];
CPD.mu = [];
CPD.Sigma = [];
CPD.prior = [];


function varargout = projPointOnPolygon(point, poly, varargin)
%PROJPOINTONPOLYGON  Compute position of a point projected on a polygon
%
%   POS = projPointOnPolygon(POINT, POLYGON)
%   Compute the position of the orthogonal projection of a point on a
%   polygon.
%   POINT is a 1-by-2 row vector containing point coordinates
%   POLYGON is a N-by-2 array containing coordinates of polygon vertices
%
%   When POINT is an array of points, returns a column vector with as many
%   rows as the number of points.
%
%   [POS, DIST] = projPointOnPolygon(...)
%   Also returns the distance between POINT and POLYGON. The distance is
%   negative if the point is located inside of the polygon.
%
%   Example
%     poly = [10 10; 20 10;20 20;10 20];
%     projPointOnPolygon([15 0], poly)
%     ans =
%         0.5000
%     projPointOnPolygon([0 16], poly)
%     ans =
%         3.4000
%
%   See also
%   points2d, polygons2d, polygonPoint, projPointOnPolyline
%   distancePointpolygon
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2009-04-30,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

% eventually copy first point at the end to ensure closed polygon
if sum(poly(end, :) == poly(1,:)) ~= 2
    poly = [poly; poly(1,:)];
end

% compute position wrt outline
[pos, minDist] = projPointOnPolyline(point, poly);

% process output arguments
if nargout <= 1
    varargout{1} = pos;
elseif nargout == 2
    varargout{1} = pos;
    if inpolygon(point(:,1), point(:,2), poly(:,1), poly(:,2))
        minDist = -minDist;
    end
    varargout{2} = minDist;
end


function point_num = sphere_llq_grid_point_count ( lat_num, long_num )

%*****************************************************************************80
%
%% SPHERE_LLQ_GRID_POINT_COUNT counts points for a SPHERE LLQ grid.
%
%  Discussion:
%
%    A SPHERE LLQ grid imposes a grid of quadrilaterals on a sphere,
%    using latitude and longitude lines.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LAT_NUM, LONG_NUM, the number of latitude
%    and longitude lines to draw.  The latitudes do not include the North and
%    South poles, which will be included automatically, so LAT_NUM = 5, for
%    instance, will result in points along 7 lines of latitude.
%
%    Output, integer POINT_NUM, the number of grid points.
%
  point_num = 2 + lat_num * long_num;

  return
end


%==============================================================================
% This code is part of the Finite Element Method app for the Matlab-based toolbox
%  FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR/FAIRFEM
%==============================================================================
%
% classdef TetraMesh1 < handle
%
% Finite Element Mesh based on tetrahedral subdivision of rectangular mesh.
%
% Each Cell is divided into 24 tetrahedra:
%
%
%  To construct an instance of this class type:
%
%  >> Mesh = TriMesh1(omega,m)
%
% Input:
% 	omega - description of spatial domain
%   m     - number of cells
%
% Properties:
%   xn     - node list
%   tri    - triangle list
%   dim    - space dimension
%   omega  - description of spatial domain
%   m      - number of cells
%   type   - type of partition
%   vol    - volume of triangles
%   nnodes - number of nodes
%   ntri   - number of triangles
%   dx1    - partial derivative operator
%   dx2    - partial derivative operator
%   dx3    - partial derivative operator
%   GRAD   - gradient operator
%   P1     - projection operator for node 1
%   P2     - projection operator for node 2
%   P3     - projection operator for node 3
%   P4     - projection operator for node 4
%   PC     - projection operator for Barycentrum
%   P      - prolongation operator
%   Pt     - restriction operator
%
%  Methods:
%   mfPu   - matrix free prolongation/restriction
%   mfPi   - matrix free edge projector
%   getP   - builds prolongation operator
%   tri2cc - averaging
%
%
% see also
% =========================================================================
classdef TetraMesh1 < handle

    properties
        % ===================================
        % node list
        % ===================================
        xn
        % ===================================
        % triangle list
        % ===================================
        tri
        % ===================================
        % space dimension
        % ===================================
        dim  = 3;
        % ===================================
        % description of computational domain
        % ===================================
        omega
        % ===================================
        % number of cells
        % ===================================
        m
        % ===================================
        % type of partition
        % ===================================
        type = 1;
        % ===================================
        % function handle to myself
        % ===================================
        me = @TetraMesh1;
        % ===================================
        % volume of triangles
        % ===================================
        vol
        % ===================================
        % number of nodes
        % ===================================
        nnodes
        % ===================================
        % number of triangles in mesh
        % ===================================
        ntri
    end

    properties (Access = public, Dependent) % These will be created when first callend and stores persistently
        % ===================================
        % dx1  - partial derivative operator
        % ===================================
        dx1
        % ===================================
        % dx2  - Partial derivative operator
        % ===================================
        dx2
        % ===================================
        % dx3  - Partial derivative operator
        % ===================================
        dx3
        % ===================================
        % GRAD - Gradient operator
        %
        %         | dx1 |
        %         |     |
        %  GRAD = | dx2 |
        %         |     |
        %         | dx3 |
        %
        % ===================================
        GRAD
        % ===================================
        % B - Vector gradient operator
        %
        %         | GRAD    0   0   |
        %         |                 |
        %   B =   |   0   GRAD  0   |
        %         |                 |
        %         |   0     0  GRAD |
        %
        % ===================================
        B
        % ===================================
        % P1 - Projection operator on Node 1
        % ===================================
        P1
        % ===================================
        % P2 - Projection operator on Node 2
        % ===================================
        P2
        % ===================================
        % P3 - Projection operator on Node 3
        % ===================================
        P3
        % ===================================
        % P4 - Projection operator on Node 4
        % ===================================
        P4
        % ===================================
        % PC - Projection operator on Barycenter
        % ===================================
        PC
        % ===================================
        % P  - Prolongation operator
        % ===================================
        P
        % ===================================
        % Pt - Restriction operator
        % ===================================
        Pt
        % ===================================
        % Boundary indices
        % ===================================
        boundaryIdx
        % ===================================
        % Boundary projector
        % ===================================
        boundaryProj
        mfdx1
        mfdx2
        mfdx3
        mfGRAD
    end

    properties (Access = private)
        % These are where the dependent data is actually stored
        dx1_
        dx2_
        dx3_
        GRAD_
        B_
        P1_
        P2_
        P3_
        P4_
        PC_
        P_
        Pt_
        mfdx1_
        mfdx2_
        mfdx3_
        mfGRAD_
        boundaryIdx_
        boundaryProj_
    end

    methods
        function this = TetraMesh1(omega,m)
            if nargin==0,
                help(mfilename);
                this.runMinimalExample;
                return;
            end
            h          = (omega(2:2:end)-omega(1:2:end))./m;
            this.omega = omega;
            this.m     = m;

            % get nodes (combine nodal, stg-1,stg-2,stg-3,cc grids)
            xc = @(i) linspace(omega(2*i-1)+h(i)/2,omega(2*i)-h(i)/2,m(i))'; % cell centers
            xn = @(i) linspace(omega(2*i-1),omega(2*i),m(i)+1)'; % nodes
            % add nodal grid
            nn = reshape(1:prod(m+1),m+1);
            nn = reshape(nn(1:end-1,1:end-1,1:end-1),1,[]); % get indices of bottom left vertices
            this.xn = reshape(getNodalGrid(omega,m),[],3);
            % add stg-1 grid
            ns  = prod(ones(length(m),1)*m+eye(length(m)),2); %staggered
            ns1 = reshape(size(this.xn,1)+(1:ns(1)),[m(1)+1 m(2) m(3)]);
            ns1 = reshape(ns1(1:end-1,:,:),1,[]); % get indices of bottom left vertices
            [x1,x2,x3] = ndgrid(xn(1),xc(2),xc(3));
            this.xn = [this.xn; x1(:), x2(:), x3(:)];
            % add stg-2 grid
            ns2 = reshape(size(this.xn,1)+(1:ns(2)),[m(1) m(2)+1 m(3)]);
            ns2 = reshape(ns2(:,1:end-1,:),1,[]); % get indices of bottom left vertices
            [x1,x2,x3] = ndgrid(xc(1),xn(2),xc(3));
            this.xn = [this.xn; x1(:), x2(:), x3(:)];
            % add stg-3 grid
            ns3 = reshape(size(this.xn,1)+(1:ns(3)),[m(1) m(2) m(3)+1]);
            ns3 = reshape(ns3(:,:,1:end-1),1,[]); % get indices of bottom left vertices
            [x1,x2,x3] = ndgrid(xc(1),xc(2),xn(3));
            this.xn = [this.xn; x1(:), x2(:), x3(:)];
            % add cc grid
            nc = reshape(size(this.xn,1)+(1:prod(m)),m);
            nc = reshape(nc,1,[]); % get indices of bottom left vertices
            this.xn = [this.xn; reshape(getCellCenteredGrid(omega,m),[],3)];

            % specify triangles
            iyn = m(1)+1; izn = prod(m(1:2)+1); izc = prod(m(1:2));
            this.tri   = [
                nn;           nn+1;         ns2;      nc;
                nn+1;         nn+izn+1;     ns2;      nc;
                nn+izn+1;     nn+izn;       ns2;      nc;
                nn+izn;       nn;           ns2;      nc; % 4
                nn+1;         nn+iyn+1;     ns1+1;    nc;
                nn+iyn+1;     nn+iyn+1+izn; ns1+1;    nc;
                nn+iyn+1+izn; nn+1+izn;     ns1+1;    nc;
                nn+1+izn;     nn+1;         ns1+1;    nc; % 8
                nn+1;         nn;           ns3;      nc;
                nn;           nn+iyn;       ns3;      nc;
                nn+iyn;       nn+1+iyn;     ns3;      nc;
                nn+1+iyn;     nn+1;         ns3;      nc; % 12

                nn+iyn;       nn;           ns1;      nc;
                nn;           nn+izn;       ns1;      nc;
                nn+izn;       nn+iyn+izn;   ns1;      nc;
                nn+iyn+izn;   nn+iyn;       ns1;      nc; % 16

                nn+iyn+1;     nn+iyn;       ns2+m(1); nc;
                nn+iyn;       nn+iyn+izn;   ns2+m(1); nc;
                nn+iyn+izn;   nn+iyn+1+izn; ns2+m(1); nc;
                nn+iyn+1+izn; nn+iyn+1;     ns2+m(1); nc; % 20
                nn+izn;       nn+izn+1;     ns3+izc;  nc;
                nn+izn+1;     nn+izn+1+iyn; ns3+izc;  nc;
                nn+iyn+1+izn; nn+iyn+izn;   ns3+izc;  nc;
                nn+iyn+izn;   nn+izn;       ns3+izc;  nc; % 24
                ];
            this.tri   = reshape(this.tri,4,[])';
            this.nnodes = size(this.xn,1);
            this.ntri  = size(this.tri,1);
            this.vol = prod((omega(2:2:end)-omega(1:2:end))./m)/24*ones(this.ntri,1);
        end

        function runMinimalExample(~)
            omega = [0 4 2 6 0 3]; m = [3 4 6];
            Mesh  = feval(mfilename,omega,m);
        end

        function x = mfPi(this,x,i)
            % =============================================================
            % function x = mfPi(this,x,i)
            %
            % matrix free edge projector
            % =============================================================
            switch i
                case 1
                    P = this.P1;
                case 2
                    P = this.P2;
                case 3
                    P = this.P3;
                case 4
                    P = this.P4;
                case 'C'
                    P = this.PC;
            end
            if size(x,1) == this.ntri,
                % ajoint
                x = P'*x;
            else
                x = P * x;
            end
        end

        function x = tri2cc(this,x)
            % =============================================================
            % function x = tri2cc(x)
            %
            % averaging or adjoint
            % =============================================================
            if numel(x)==this.ntri,
                x = mean(reshape(x,24,[]),1);
            else
                x = reshape(x,1,[]);
                x = (1/24)*repmat(x,[24 1]);
                x = x(:);
            end

        end

        function Pu = mfPuNodal(this,yn,m,flag)
            % =============================================================
            % function Pu = mfPuNodal(~,yn,m,flag)
            %
            % matrix free prolongation/restriction for nodal quantities
            % =============================================================
            yn = reshape(yn,[],this.dim);
            v = @(x) x(:);
            switch flag
                case 'Pu' % coarse --> fine
                    mf = 2*m;

                    % indices of coarse grid nodes (split by nodal, stg-i, c)
                    ns   = prod(ones(length(m),1)*m+eye(length(m)),2); %staggered
                    inc  = reshape(1:prod(m+1),m+1);
                    is1c = reshape(prod(m+1)+(1:ns(1)),[m(1)+1 m(2) m(3)]);
                    is2c = reshape(prod(m+1)+ns(1)+ (1:ns(2)),[m(1) m(2)+1 m(3)]);
                    is3c = reshape(prod(m+1)+ns(1)+ns(2)+(1:ns(3)),[m(1) m(2) m(3)+1]);
                    icc  = reshape(prod(m+1)+ns(1)+ns(2)+ns(3)+(1:prod(m)),m);
                    % indices of fine grid nodes
                    ns   = prod(ones(length(mf),1)*mf+eye(length(mf)),2); %staggered
                    inf  = reshape(1:prod(mf+1),mf+1);
                    is1f = reshape(prod(mf+1)+(1:ns(1)),[mf(1)+1 mf(2) mf(3)]);
                    is2f = reshape(prod(mf+1)+ns(1)+ (1:ns(2)),[mf(1) mf(2)+1 mf(3)]);
                    is3f = reshape(prod(mf+1)+ns(1)+ns(2)+(1:ns(3)),[mf(1) mf(2) mf(3)+1]);
                    icf  = reshape(prod(mf+1)+ns(1)+ns(2)+ns(3)+(1:prod(mf)),mf);

                    % allocate space
                    Pu = zeros(icf(end),this.dim);

                    % include existing nodes  (nodal-->nodal)
                    Pu(v(inf(1:2:end,1:2:end,1:2:end)),:)  = yn(inc(:),:);
                    % include existing nodes  (stg-1-->nodal)
                    Pu(v(inf(1:2:end,2:2:end,2:2:end)),:) = yn(is1c(:),:);
                    % include existing nodes  (stg-2-->nodal)
                    Pu(v(inf(2:2:end,1:2:end,2:2:end)),:) = yn(is2c(:),:);
                    % include existing nodes  (stg-3-->nodal)
                    Pu(v(inf(2:2:end,2:2:end,1:2:end)),:) = yn(is3c(:),:);
                    % include existing nodes  (cc-->nodal)
                    Pu(v(inf(2:2:end,2:2:end,2:2:end)),:) = yn(icc(:),:);

                    % average to get edge-stg-1
                    Pu(v(inf(2:2:end,1:2:end,1:2:end)),:) = ...
                        .5*(yn(v(inc(1:end-1,:,:)),:) + yn(v(inc(2:end,:,:)),:)) ;
                    % average to get edge-stg-2
                    Pu(v(inf(1:2:end,2:2:end,1:2:end)),:) = ...
                        .5*(yn(v(inc(:,1:end-1,:)),:) + yn(v(inc(:,2:end,:)),:)) ;
                    % average to get edge-stg-3
                    Pu(v(inf(1:2:end,1:2:end,2:2:end)),:) = ...
                        .5*(yn(v(inc(:,:,1:end-1)),:) + yn(v(inc(:,:,2:end)),:)) ;
                    % get face-stg-1
                    Pu(is1f(:),:) = .25*(   Pu(v(inf(1:end,1:end-1,1:end-1)),:) ...
                        + Pu(v(inf(1:end,2:end  ,1:end-1)),:) ...
                        + Pu(v(inf(1:end,2:end  ,2:end  )),:) ...
                        + Pu(v(inf(1:end,1:end-1,2:end  )),:));

                    % get face-stg-2
                    Pu(is2f(:),:) = .25*(   Pu(v(inf(1:end-1,1:end  ,1:end-1)),:) ...
                        + Pu(v(inf(2:end  ,1:end  ,1:end-1)),:) ...
                        + Pu(v(inf(2:end  ,1:end  ,2:end  )),:) ...
                        + Pu(v(inf(1:end-1,1:end  ,2:end  )),:));
                    % get face-stg-3
                    Pu(is3f(:),:) = .25*(   Pu(v(inf(1:end-1,1:end-1,1:end)),:) ...
                        + Pu(v(inf(2:end  ,1:end-1,1:end)),:) ...
                        + Pu(v(inf(2:end  ,2:end  ,1:end)),:) ...
                        + Pu(v(inf(1:end-1,2:end  ,1:end)),:));

                    % get new cell-centers
                    Pu(icf(:),:)  = .125*(   Pu(v(inf(1:end-1,1:end-1,1:end-1)),:) ...
                        + Pu(v(inf(2:end  ,1:end-1,1:end-1)),:) ...
                        + Pu(v(inf(2:end  ,2:end  ,1:end-1)),:) ...
                        + Pu(v(inf(1:end-1,2:end  ,1:end-1)),:) ...
                        + Pu(v(inf(1:end-1,1:end-1,2:end)),:) ...
                        + Pu(v(inf(2:end  ,1:end-1,2:end)),:) ...
                        + Pu(v(inf(2:end  ,2:end  ,2:end)),:) ...
                        + Pu(v(inf(1:end-1,2:end  ,2:end)),:));

                case 'PTu' % fine --> coarse
                    % include parent nodes
                    mf = m;
                    m  = m/2;
                    % indices of coarse grid nodes (split by nodal, stg-i, c)
                    ns   = prod(ones(length(m),1)*m+eye(length(m)),2); %staggered
                    inc  = reshape(1:prod(m+1),m+1);
                    is1c = reshape(prod(m+1)+(1:ns(1)),[m(1)+1 m(2) m(3)]);
                    is2c = reshape(prod(m+1)+ns(1)+ (1:ns(2)),[m(1) m(2)+1 m(3)]);
                    is3c = reshape(prod(m+1)+ns(1)+ns(2)+(1:ns(3)),[m(1) m(2) m(3)+1]);
                    icc  = reshape(prod(m+1)+ns(1)+ns(2)+ns(3)+(1:prod(m)),m);
                    % indices of fine grid nodes
                    ns   = prod(ones(length(mf),1)*mf+eye(length(mf)),2); %staggered
                    inf  = reshape(1:prod(mf+1),mf+1);
                    is1f = reshape(prod(mf+1)+(1:ns(1)),[mf(1)+1 mf(2) mf(3)]);
                    is2f = reshape(prod(mf+1)+ns(1)+ (1:ns(2)),[mf(1) mf(2)+1 mf(3)]);
                    is3f = reshape(prod(mf+1)+ns(1)+ns(2)+(1:ns(3)),[mf(1) mf(2) mf(3)+1]);
                    icf  = reshape(prod(mf+1)+ns(1)+ns(2)+ns(3)+(1:prod(mf)),mf);

                    % allocate space
                    Pu = zeros(icc(end),this.dim);

                    % push weights from cell-centers
                    yn(v(inf(1:end-1,1:end-1,1:end-1)),:)  = yn(v(inf(1:end-1,1:end-1,1:end-1)),:) + .125* yn(icf(:),:);
                    yn(v(inf(2:end  ,1:end-1,1:end-1)),:)  = yn(v(inf(2:end  ,1:end-1,1:end-1)),:) + .125* yn(icf(:),:);
                    yn(v(inf(2:end  ,2:end  ,1:end-1)),:)  = yn(v(inf(2:end  ,2:end  ,1:end-1)),:) + .125* yn(icf(:),:);
                    yn(v(inf(1:end-1,2:end  ,1:end-1)),:)  = yn(v(inf(1:end-1,2:end  ,1:end-1)),:) + .125* yn(icf(:),:);
                    yn(v(inf(1:end-1,1:end-1,2:end)),:)    = yn(v(inf(1:end-1,1:end-1,2:end)),:)   + .125* yn(icf(:),:);
                    yn(v(inf(2:end  ,1:end-1,2:end)),:)    = yn(v(inf(2:end  ,1:end-1,2:end)),:)   + .125* yn(icf(:),:);
                    yn(v(inf(2:end  ,2:end  ,2:end)),:)    = yn(v(inf(2:end  ,2:end  ,2:end)),:)   + .125* yn(icf(:),:);
                    yn(v(inf(1:end-1,2:end  ,2:end)),:)    = yn(v(inf(1:end-1,2:end  ,2:end)),:)   + .125* yn(icf(:),:);
                    % push weights from face-stg-1
                    yn(v(inf(1:end,1:end-1,1:end-1)),:)   = yn(v(inf(1:end,1:end-1,1:end-1)),:) + .25 * yn(is1f(:),:);
                    yn(v(inf(1:end,2:end  ,1:end-1)),:)   = yn(v(inf(1:end,2:end  ,1:end-1)),:) + .25 * yn(is1f(:),:);
                    yn(v(inf(1:end,2:end  ,2:end  )),:)   = yn(v(inf(1:end,2:end  ,2:end  )),:) + .25 * yn(is1f(:),:);
                    yn(v(inf(1:end,1:end-1,2:end  )),:)   = yn(v(inf(1:end,1:end-1,2:end  )),:) + .25 * yn(is1f(:),:);
                    % push weights from face-stg-2
                    yn(v(inf(1:end-1,1:end  ,1:end-1)),:) = yn(v(inf(1:end-1,1:end  ,1:end-1)),:) + .25 * yn(is2f(:),:);
                    yn(v(inf(2:end  ,1:end  ,1:end-1)),:) = yn(v(inf(2:end  ,1:end  ,1:end-1)),:) + .25 * yn(is2f(:),:);
                    yn(v(inf(2:end  ,1:end  ,2:end  )),:) = yn(v(inf(2:end  ,1:end  ,2:end  )),:) + .25 * yn(is2f(:),:);
                    yn(v(inf(1:end-1,1:end  ,2:end  )),:) = yn(v(inf(1:end-1,1:end  ,2:end  )),:) + .25 * yn(is2f(:),:);
                    % push weights from face-stg-3
                    yn(v(inf(1:end-1,1:end-1,1:end)),:)   = yn(v(inf(1:end-1,1:end-1,1:end)),:)  + .25 *  yn(is3f(:),:);
                    yn(v(inf(2:end  ,1:end-1,1:end)),:)   = yn(v(inf(2:end  ,1:end-1,1:end)),:)  + .25 *  yn(is3f(:),:);
                    yn(v(inf(2:end  ,2:end  ,1:end)),:)   = yn(v(inf(2:end  ,2:end  ,1:end)),:)  + .25 *  yn(is3f(:),:);
                    yn(v(inf(1:end-1,2:end  ,1:end)),:)   = yn(v(inf(1:end-1,2:end  ,1:end)),:)  + .25 *  yn(is3f(:),:);

                    % include existing nodes  (nodal-->nodal)
                    Pu(inc(:),:)  = yn(v(inf(1:2:end,1:2:end,1:2:end)),:) ;
                    % include existing nodes  (stg-1-->nodal)
                    Pu(is1c(:),:) = yn(v(inf(1:2:end,2:2:end,2:2:end)),:);
                    % include existing nodes  (stg-2-->nodal)
                    Pu(is2c(:),:) = yn(v(inf(2:2:end,1:2:end,2:2:end)),:);
                    % include existing nodes  (stg-3-->nodal)
                    Pu(is3c(:),:) = yn(v(inf(2:2:end,2:2:end,1:2:end)),:);
                    % include existing nodes  (cc-->nodal)
                    Pu(icc(:),:)  = yn(v(inf(2:2:end,2:2:end,2:2:end)),:);

                    % average to get edge-stg-1
                    Pu(v(inc(1:end-1,:,:)),:) = Pu(v(inc(1:end-1,:,:)),:) + .5 * yn(v(inf(2:2:end,1:2:end,1:2:end)),:);
                    Pu(v(inc(2:end,:,:)),:)   = Pu(v(inc(2:end,:,:)),:)   + .5 * yn(v(inf(2:2:end,1:2:end,1:2:end)),:);

                    % average to get edge-stg-2
                    Pu(v(inc(:,1:end-1,:)),:) = Pu(v(inc(:,1:end-1,:)),:) + .5 * yn(v(inf(1:2:end,2:2:end,1:2:end)),:);
                    Pu(v(inc(:,2:end  ,:)),:) = Pu(v(inc(:,2:end  ,:)),:) + .5 * yn(v(inf(1:2:end,2:2:end,1:2:end)),:);

                    % average to get edge-stg-3
                    Pu(v(inc(:,:,1:end-1)),:) = Pu(v(inc(:,:,1:end-1)),:) + .5 * yn(v(inf(1:2:end,1:2:end,2:2:end)),:);
                    Pu(v(inc(:,:,2:end  )),:) = Pu(v(inc(:,:,2:end  )),:) + .5 * yn(v(inf(1:2:end,1:2:end,2:2:end)),:);

            end
            Pu = Pu(:);

        end
        function Pu = mfInterNodal(this,yn,m)
            % =============================================================
            % function Pu = mfInterNodal(~,yn,m,)
            %
            % interpolates to nodal grid of a finer resolution.
            % =============================================================
            yn = reshape(yn,[],this.dim);
            v = @(x) x(:);
            mf = 2*m;

            % indices of coarse grid nodes (split by nodal, stg-i, c)
            ns   = prod(ones(length(m),1)*m+eye(length(m)),2); %staggered
            inc  = reshape(1:prod(m+1),m+1);
            is1c = reshape(prod(m+1)+(1:ns(1)),[m(1)+1 m(2) m(3)]);
            is2c = reshape(prod(m+1)+ns(1)+ (1:ns(2)),[m(1) m(2)+1 m(3)]);
            is3c = reshape(prod(m+1)+ns(1)+ns(2)+(1:ns(3)),[m(1) m(2) m(3)+1]);
            icc  = reshape(prod(m+1)+ns(1)+ns(2)+ns(3)+(1:prod(m)),m);
            % indices of fine grid nodes
            inf  = reshape(1:prod(mf+1),mf+1);

            % allocate space
            Pu = zeros(inf(end),this.dim);

            % include existing nodes  (nodal-->nodal)
            Pu(v(inf(1:2:end,1:2:end,1:2:end)),:)  = yn(inc(:),:);
            % include existing nodes  (stg-1-->nodal)
            Pu(v(inf(1:2:end,2:2:end,2:2:end)),:) = yn(is1c(:),:);
            % include existing nodes  (stg-2-->nodal)
            Pu(v(inf(2:2:end,1:2:end,2:2:end)),:) = yn(is2c(:),:);
            % include existing nodes  (stg-3-->nodal)
            Pu(v(inf(2:2:end,2:2:end,1:2:end)),:) = yn(is3c(:),:);
            % include existing nodes  (cc-->nodal)
            Pu(v(inf(2:2:end,2:2:end,2:2:end)),:) = yn(icc(:),:);

            % average to get edge-stg-1
            Pu(v(inf(2:2:end,1:2:end,1:2:end)),:) = ...
                .5*(yn(v(inc(1:end-1,:,:)),:) + yn(v(inc(2:end,:,:)),:)) ;
            % average to get edge-stg-2
            Pu(v(inf(1:2:end,2:2:end,1:2:end)),:) = ...
                .5*(yn(v(inc(:,1:end-1,:)),:) + yn(v(inc(:,2:end,:)),:)) ;
            % average to get edge-stg-3
            Pu(v(inf(1:2:end,1:2:end,2:2:end)),:) = ...
                .5*(yn(v(inc(:,:,1:end-1)),:) + yn(v(inc(:,:,2:end)),:)) ;

            Pu = Pu(:);

        end

        function P = getPuNodal(~,m)
            % =============================================================
            % function P = getPuNodal(~,m)
            %
            % returns prolongation operator for input of cell-width m
            % =============================================================

            v = @(x) x(:);

            mf = 2*m;
            % indices of coarse grid nodes (split by nodal, stg-i, c)
            ns   = prod(ones(length(m),1)*m+eye(length(m)),2); %staggered
            inc  = reshape(1:prod(m+1),m+1);
            is1c = reshape(prod(m+1)+(1:ns(1)),[m(1)+1 m(2) m(3)]);
            is2c = reshape(prod(m+1)+ns(1)+ (1:ns(2)),[m(1) m(2)+1 m(3)]);
            is3c = reshape(prod(m+1)+ns(1)+ns(2)+(1:ns(3)),[m(1) m(2) m(3)+1]);
            icc  = reshape(prod(m+1)+ns(1)+ns(2)+ns(3)+(1:prod(m)),m);
            % indices of fine grid nodes
            ns   = prod(ones(length(mf),1)*mf+eye(length(mf)),2); %staggered
            inf  = reshape(1:prod(mf+1),mf+1);
            is1f = reshape(prod(mf+1)+(1:ns(1)),[mf(1)+1 mf(2) mf(3)]);
            is2f = reshape(prod(mf+1)+ns(1)+ (1:ns(2)),[mf(1) mf(2)+1 mf(3)]);
            is3f = reshape(prod(mf+1)+ns(1)+ns(2)+(1:ns(3)),[mf(1) mf(2) mf(3)+1]);
            icf  = reshape(prod(mf+1)+ns(1)+ns(2)+ns(3)+(1:prod(mf)),mf);

            % allocate space
            I = []; J = []; W = [];

            % include existing nodes  (nodal-->nodal)
            ii = inf(1:2:end,1:2:end,1:2:end);
            jj = inc;
            ww = ones(size(jj));
            I = [I; ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];
            % include existing nodes  (stg-1-->nodal)
            ii = inf(1:2:end,2:2:end,2:2:end);
            jj = is1c;
            ww = ones(size(jj));
            I = [I; ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];
            % include existing nodes  (stg-2-->nodal)
            ii = inf(2:2:end,1:2:end,2:2:end);
            jj = is2c;
            ww = ones(size(jj));
            I = [I; ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];
            % include existing nodes  (stg-3-->nodal)
            ii = inf(2:2:end,2:2:end,1:2:end);
            jj = is3c;
            ww = ones(size(jj));
            I = [I; ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];
            % include existing nodes  (cc-->nodal)
            ii = inf(2:2:end,2:2:end,2:2:end);
            jj = icc;
            ww = ones(size(jj));
            I = [I; ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];

            % average to get edge-stg-1
            ii   = inf(2:2:end,1:2:end,1:2:end);
            jj   = [reshape(inc(1:end-1,:,:),[],1); reshape(inc(2:end,:,:),[],1)];
            ww   = .5*ones(size(jj));
            I    = [I; ii(:);ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];
            % average to get edge-stg-2
            ii   = inf(1:2:end,2:2:end,1:2:end);
            jj   = [reshape(inc(:,1:end-1,:),[],1); reshape(inc(:,2:end,:),[],1)];
            ww   = .5*ones(size(jj));
            I    = [I; ii(:);ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];
            % average to get edge-stg-3
            ii   = inf(1:2:end,1:2:end,2:2:end);
            jj   = [reshape(inc(:,:,1:end-1),[],1); reshape(inc(:,:,2:end),[],1)];
            ww   = .5*ones(size(jj));
            I    = [I; ii(:);ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];

            % coarse --> fine(nodal only)
            P1  = sparse(I,J,W,prod(mf+1),icc(end));
            % allocate space
            I = []; J = []; W = [];

            % get face-stg-1
            ii  = is1f;
            jj  = [
                v(inf(1:end,1:end-1,1:end-1));
                v(inf(1:end,2:end  ,1:end-1));
                v(inf(1:end,2:end  ,2:end  ));
                v(inf(1:end,1:end-1,2:end  ));
                ];
            ww   = .25*ones(size(jj));
            I    = [I; ii(:);ii(:);ii(:);ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];
            % get face-stg-2
            ii  = is2f;
            jj  = [
                v(inf(1:end-1,1:end  ,1:end-1));
                v(inf(2:end  ,1:end  ,1:end-1));
                v(inf(2:end  ,1:end  ,2:end  ));
                v(inf(1:end-1,1:end  ,2:end  ));
                ];
            ww   = .25*ones(size(jj));
            I    = [I; ii(:);ii(:);ii(:);ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];
            % get face-stg-3
            ii  = is3f;
            jj  = [
                v(inf(1:end-1,1:end-1,1:end));
                v(inf(2:end  ,1:end-1,1:end));
                v(inf(2:end  ,2:end  ,1:end));
                v(inf(1:end-1,2:end  ,1:end));
                ];
            ww   = .25*ones(size(jj));
            I    = [I; ii(:);ii(:);ii(:);ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];

            % get new cell-centers
            ii  = icf;
            jj  = [
                v(inf(1:end-1,1:end-1,1:end-1));
                v(inf(2:end  ,1:end-1,1:end-1));
                v(inf(2:end  ,2:end  ,1:end-1));
                v(inf(1:end-1,2:end  ,1:end-1));
                v(inf(1:end-1,1:end-1,2:end));
                v(inf(2:end  ,1:end-1,2:end));
                v(inf(2:end  ,2:end  ,2:end));
                v(inf(1:end-1,2:end  ,2:end));
                ];
            ww   = .125*ones(size(jj));
            I    = [I; repmat(ii(:),[8,1])]; J = [J; jj(:)]; W = [W; ww(:)];

            P2   = sparse(I-prod(mf+1),J,W,ns(1)+ns(2)+ns(3)+prod(mf),prod(mf+1));

            P    = [P1; P2*P1];
        end

        % ========== get methods ========================================
        function dx1 = get.dx1(this)
            if isempty(this.dx1_),
                [this.dx1_, this.dx2_, this.dx3_] = getGradientMatrixFEM(this,0);
            end
            dx1 = this.dx1_;
        end

        function dx2 = get.dx2(this)
            if isempty(this.dx2_),
                [this.dx1_, this.dx2_, this.dx3_] = getGradientMatrixFEM(this,0);
            end
            dx2 = this.dx2_;
        end

        function dx3 = get.dx3(this)
            if isempty(this.dx3_),
                [this.dx1_, this.dx2_, this.dx3_] = getGradientMatrixFEM(this,0);
            end
            dx3 = this.dx3_;
        end

        function GRAD = get.GRAD(this)
            if isempty(this.GRAD_),
                this.GRAD_ = [this.dx1;this.dx2;this.dx3];
            end
            GRAD = this.GRAD_;
        end
        function B = get.B(this)
            if isempty(this.B_),
                this.B_ = blkdiag(this.GRAD,this.GRAD,this.GRAD);
            end
            B = this.B_;
        end

        function P1 = get.P1(this)
            if isempty(this.P1_),
                A = speye(this.nnodes);
                this.P1_ = A(this.tri(:,1),:);
            end
            P1 = this.P1_;
        end
        function P2 = get.P2(this)
            if isempty(this.P2_),
                A = speye(this.nnodes);
                this.P2_ = A(this.tri(:,2),:);
            end
            P2 = this.P2_;
        end

        function P3 = get.P3(this)
            if isempty(this.P3_),
                A = speye(this.nnodes);
                this.P3_ = A(this.tri(:,3),:);
            end
            P3 = this.P3_;
        end

        function P4 = get.P4(this)
            if isempty(this.P4_),
                A = speye(this.nnodes);
                this.P4_ = A(this.tri(:,4),:);
            end
            P4 = this.P4_;
        end
        function PC = get.PC(this)
            if isempty(this.PC_),
                this.PC_ = (this.P1+this.P2+this.P3+this.P4)/4;
            end
            PC = this.PC_;
        end
        function P = get.P(this)
            if isempty(this.P_),
                this.P_ = this.getPuNodal(this.m);
            end
            P = this.P_;
        end
        function Pt = get.Pt(this)
            if isempty(this.Pt_),
                this.Pt_ = this.getPuNodal(this.m/2);
            end
            Pt = this.Pt_;
        end

        function mfdx1 = get.mfdx1(this)
            if isempty(this.mfdx1_),
                [this.mfdx1_, this.mfdx2_, this.mfdx3_] = getGradientMatrixFEM(this,1);
            end
            mfdx1 = this.mfdx1_;
        end

        function mfdx2 = get.mfdx2(this)
            if isempty(this.mfdx2_),
                [this.mfdx1_, this.mfdx2_, this.mfdx3_] = getGradientMatrixFEM(this,1);
            end
            mfdx2 = this.mfdx2_;
        end

        function mfdx3 = get.mfdx3(this)
            if isempty(this.mfdx3_),
                [this.mfdx1_, this.mfdx2_, this.mfdx3_] = getGradientMatrixFEM(this,1);
            end
            mfdx3 = this.mfdx3_;
        end

        function mfGRAD = get.mfGRAD(this)
            if isempty(this.mfGRAD_),
                this.mfGRAD_ = getGradientMatrixFEM(this,1);
            end
            mfGRAD = this.mfGRAD_;
        end

        function idx = get.boundaryIdx(this)
            if isempty(this.boundaryIdx_),
                mkvc = @(v) v(:);
                ns   = prod(ones(length(this.m),1)*this.m+eye(length(this.m)),2); %staggered

                % nodal points
                id = reshape(1:prod(this.m+1),this.m+1);
                idx = [ ...
                            mkvc(id([1,end],:,:));
                            mkvc(id(:,[1,end],:));
                            mkvc(id(:,:,[1,end]));
                            ];
                % stg-1 points
                id = prod(this.m+1) + reshape(1:prod(this.m+[1,0,0]),this.m+[1,0,0]);
                idx = [idx; mkvc(id([1,end],:,:)) ];
                % stg-2 points
                id = prod(this.m+1) +ns(1) + reshape(1:prod(this.m+[0,1,0]),this.m+[0,1,0]);
                idx = [idx; mkvc(id(:,[1,end],:)) ];
                % stg-3 points
                id = prod(this.m+1) +ns(1) + ns(2) + reshape(1:prod(this.m+[0,0,1]),this.m+[0,0,1]);
                idx = [idx; mkvc(id(:,:,[1,end])) ];

                this.boundaryIdx_ = unique( idx);
            end
            idx = this.boundaryIdx_;
        end

        function idx = get.boundaryProj(this)
            if isempty(this.boundaryProj_),
                idx = this.boundaryIdx;

                P = speye(size(this.xn,1));
                P = P(idx,:);
                P = kron(speye(3),P);

                this.boundaryProj_ = P;
            end
            idx = this.boundaryProj_;
        end

    end
end


function [sigma,u,AD] = HodgeLaplacianE(node,elem,pde,bdFlag,option)
%% HODEGELAPLACIANE Hodge Laplacian of edge element

if ~exist('option','var'), option = []; end
if ~exist('bdFlag','var'), bdFlag = []; end

%% Data structure
% elemold = elem;
[elem,bdFlag] = sortelem(elem,bdFlag);  % ascend ordering
[elem2edge,edge] = dofedge(elem);
[Dlambda,area,elemSign] = gradbasis(node,elem);
locEdge = [2 3; 1 3; 1 2];
N = size(node,1); NT = size(elem,1); NE = size(edge,1);
Nsigma = N; Nu = NE; Ndof = Nsigma + Nu;

%% Assemble matrix
% Mass matrices
Mv = accumarray([elem(:,1);elem(:,2);elem(:,3)],[area;area;area]/3,[N,1]);
Mv = spdiags(Mv,0,N,N);
Me = getmassmatvec(elem2edge,area,Dlambda,'ND1');
Mt = spdiags(1./area,0,NT,NT);
% G. gradient operator: P1 -> (ND1)'
G = Me*icdmat(double(edge),[-1 1]);  % gradient matrix
% R. rotation operator
R = icdmat(double(elem2edge),[1 -1 1]);
C = R'*Mt*R;
A = [-Mv G'; G C];

%% Assemble right hand side
f = zeros(Ndof,1);
if ~isfield(pde,'f') || (isfield(pde,'f') && isreal(pde.f) && all(pde.f==0))
    pde.f = [];
end
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order is 3
end
if isfield(pde,'f') && ~isempty(pde.f)
    [lambda,w] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
    bt = zeros(NT,3);
    for p = 1:nQuad
        % quadrature points in the x-y-z coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        fp = pde.f(pxy);
        for k = 1:3
            i = locEdge(k,1); j = locEdge(k,2);
            % phi_k = lambda_iDlambda_j - lambda_jDlambda_i;
            phi_k = lambda(p,i)*Dlambda(:,:,j)-lambda(p,j)*Dlambda(:,:,i);
            rhs = dot(phi_k,fp,2);
            bt(:,k) = bt(:,k) + w(p)*rhs;
        end
    end
    bt = bt.*repmat(area,1,3);
    f = accumarray(N+elem2edge(:),bt(:),[Ndof 1]);
end
clear pxy fp bt rhs phi_k Dlambda

%% Boundary Conditions
[AD,f,u,sigma,freeDof,isPureNeumann] = getbdHodgeLapE(f);

%% Solve the linear system
temp = zeros(Ndof,1);
temp(freeDof) = A(freeDof,freeDof)\f(freeDof);
sigma(freeNode) = temp(freeNode);
u(freeEdge) = temp(freeEdge+N);

if isPureNeumann
   u = u - mean(u); % normalize u
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbdHodgeLapE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,f,u,sigma,freeDof,isPureNeumann] = getbdHodgeLapE(f)

    u = zeros(NE,1); sigma = zeros(N,1);
    % Find boundary edges: Neumann
    isNeumann(elem2edge(bdFlag(:)==2)) = true;
    Neumann = edge(isNeumann,:);
    % Neumann boundary condition: modify nodal dof values
    if isnumeric(pde.gun) && all(pde.gun == 0)
        pde.gun = [];
    end
    if ~isempty(Neumann) && ~isempty(pde.gun)
        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 2;   % default order exact for linear gN
        end
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
        phigN = lambdagN;                 % linear bases
        nQuadgN = size(lambdagN,1);
        ge = zeros(size(Neumann,1),2);
        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNp = pde.gun(ppxy);
            for igN = 1:2
                ge(:,igN) = ge(:,igN) + weightgN(pp)*phigN(pp,igN)*gNp;
            end
        end
        ge = ge.*repmat(el,1,2);
        f = f + accumarray(Neumann(:), ge(:),[Ndof,1]);
    end
    % Neumann boundary condition: modify edge dof values
    edgeSign = ones(NE,1);
    idx = (bdFlag(:,1) ~= 0) & (elemSign == -1);% first edge is on boundary
    edgeSign(elem2edge(idx,1)) = -1;
    idx = (bdFlag(:,2) ~= 0) & (elemSign == 1); % second edge is on boundary
    edgeSign(elem2edge(idx,2)) = -1;
    idx = (bdFlag(:,3) ~= 0) & (elemSign == -1);% first edge is on boundary
    edgeSign(elem2edge(idx,3)) = -1;
    if isnumeric(pde.grotu) && all(pde.grotu==0)
        pde.grotu = [];
    end
    if ~isempty(pde.grotu) && ~isempty(Neumann)
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 2;   % default order exact for linear gN
        end
        [lambda,weight] = quadpts1(option.gNquadorder);
        nQuad = size(lambda,1);
        Neumannidx = find(isNeumann) + N;
        for ip = 1:nQuad
        	pxy = lambda(ip,1)*node(Neumann(:,1),:)+...
                  lambda(ip,2)*node(Neumann(:,2),:);
            f(Neumannidx) = f(Neumannidx) + weight(ip)*pde.grotu(pxy);
        end
        f(Neumannidx) = f(Neumannidx).*edgeSign(isNeumann);
        % no edge length since the basis of edge element contains it.
    end

    % Find Dirichlet boundary nodes and edges
    isDirichlet = false(NE,1);
    isDirichlet(elem2edge(bdFlag(:)==1)) = true;
    Dirichlet = edge(isDirichlet,:);
    isfixedNode = false(N,1);
    isfixedNode(Dirichlet(:)) = true;
    fixedNode = find(isfixedNode);
    fixedDof = [fixedNode; N + find(isDirichlet)];
    freeNode = find(~isfixedNode);
    freeEdge = find(~isDirichlet);
    freeDof = [freeNode; N + freeEdge];
    isPureNeumann = false;
    if isempty(fixedNode) % pure Neumann boundary condition
        isPureNeumann = true;
        fixedDof = Ndof;
        freeDof = (1:Ndof-1)';    % eliminate the kernel
    end

    % Modify right hand side to include Dirichlet boundary condition
    if isnumeric(pde.gu) && all(pde.gu == 0)   % zero gu
        pde.gu = [];
    end
    if isnumeric(pde.gsigma) && all(pde.gsigma == 0)   % zero gsigma
        pde.gsigma = [];
    end
    if ~isPureNeumann && ~isempty(fixedNode) && (~isempty(pde.gu) || ~isempty(pde.gsigma))
        sigma = zeros(N,1);
        sigma(fixedNode) = pde.gsigma(node(fixedNode,:));
        u = zeros(NE,1);
        u(isDirichlet) = edgeinterpolate(pde.gu,node,Dirichlet);
        f = f - A*[sigma; u];
        f(fixedDof) = [sigma(fixedNode); u(isDirichlet)];
    end

    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    if ~isempty(fixedDof)
        bdidx = zeros(Ndof,1);
        bdidx(fixedDof) = 1;
        Tbd = spdiags(bdidx,0,Ndof,Ndof);
        T = spdiags(1-bdidx,0,Ndof,Ndof);
        AD = T*A*T + Tbd;
    else
        AD = A;
    end

    % Pure Neumann boundary condition
    if isPureNeumann
        f(N+1:Ndof) = f(N+1:Ndof) - mean(f(N+1:Ndof));   % compatilbe condition: sum(b) = 0
    end
    end
end


function [U,XI,YI] = takeo_asap(V,F,b,bc)
  % TAKEO_ASAP Solve "As-Similar-As-Possible" according to "As-Rigid-As-Possible
  % Shape Manipulation" by Igarashi et al., this is their "First Step". This is
  % equivalent up to factor of 2.5 to lscm
  %
  % U = takeo_arap(V,F,b,bc)
  %
  % Inputs:
  %   V  #V by 2 list of rest domain positions
  %   F  #F by 3 list of triangle indices into V
  %   b  #b list of indices of constraint (boundary) vertices
  %   bc  #b by 2 list of constraint positions for b
  % Outputs:
  %   U  #V by 2 list of new positions
  %   X1  #F*3 by 1 list of coefficents, so that V(I,:) can be described in
  %     terms of V(J,:) and V(K,:), where I,J,K are the indices of each
  %     triangle
  %   Y1  #F*3 by 1 list of coefficents, see above
  %
  % Note about X1,Y1:
  % V(I,:) =
  %   (V(J,:) + [XI XI].* (V(K,:) - V(J,:)) + [YI -YI].* fliplr(V(K,:)-V(J,:)))
  %
  % See also: arap, takeo_arap, lscm
  %

  % We will build the quadratic system matrix per triangle
  % First we need for each triangle to write each vertex in terms of the other
  % two vertices.

  [XI,YI] = relative_coordinates(V,F);
  % reshape coordinate into single tall column
  XI = reshape(XI,size(F,1)*3,1);
  YI = reshape(YI,size(F,1)*3,1);

  % number of vertices
  n = size(V,1);
  % only works in 2D
  assert(size(V,2) == 2)
  % number of triangles
  nt = size(F,1);

  % Indices of each triangle vertex, I, and its corresponding two neighbors, J
  % and K
  I = [F(:,1);F(:,2);F(:,3)];
  J = [F(:,2);F(:,3);F(:,1)];
  K = [F(:,3);F(:,1);F(:,2)];
  % rename indices to so that I1 and I2 and so on index into vertex positions
  % as single column. Namely V(I,1) = V(I1) and V(I,2) = V(I2) etc.
  I1 = I;
  I2 = I+n;
  J1 = J;
  J2 = J+n;
  K1 = K;
  K2 = K+n;

  % Construct quadratic system matrix each triplet from II,JJ,VV contains an
  % set of elements per vertex per triangle in the quadratic energy summation
  II = [ ...
    J1
    J2
    J1
    K1
    J1
    K2
    J2
    K1
    J2
    K2
    K1
    K2
    I1
    J1
    I1
    J2
    I2
    J1
    I2
    J2
    I1
    I2
    I1
    K1
    I1
    K2
    I2
    K1
    I2
    K2
    ];
  JJ = [
    J1
    J2
    K1
    J1
    K2
    J1
    K1
    J2
    K2
    J2
    K1
    K2
    J1
    I1
    J2
    I1
    J1
    I2
    J2
    I2
    I1
    I2
    K1
    I1
    K2
    I1
    K1
    I2
    K2
    I2
    ];
  VV = [ ...
    (1-2*XI+XI.^2+YI.^2)
    (1-2*XI+XI.^2+YI.^2)
    (XI-XI.^2-YI.^2)
    (XI-XI.^2-YI.^2)
    (YI)
    (YI)
    (-YI)
    (-YI)
    (XI-XI.^2-YI.^2)
    (XI-XI.^2-YI.^2)
    (XI.^2+YI.^2)
    (XI.^2+YI.^2)
    (-1+XI)
    (-1+XI)
    (YI)
    (YI)
    (-YI)
    (-YI)
    (-1+XI)
    (-1+XI)
    ones(nt*3,1)
    ones(nt*3,1)
    (-XI)
    (-XI)
    (-YI)
    (-YI)
    (YI)
    (YI)
    (-XI)
    (-XI)
    ];
  % Assembly matrix
  A = sparse(II,JJ,VV,2*n,2*n);

  % solve
  U = min_quad_with_fixed(A,zeros(2*n,1),[b b+n],bc(:));
  % reshape into columns
  U = reshape(U,n,2);

end

%E = ...
%  sum(sum(((U(J,:) + [XI XI].* (U(K,:) - U(J,:)) + [YI -YI].* fliplr(U(K,:)-U(J,:))) - U(I,:)).^2,2));
%  E
%
%
%E = ...
%  sum( ...
%      (U(J,1) + XI.* U(K,1) - XI.*U(J,1) + YI.* U(K,2) - YI.*U(J,2) - U(I,1)).^2 + ...
%      (U(J,2) + XI.* U(K,2) - XI.*U(J,2) - YI.* U(K,1) + YI.*U(J,1) - U(I,2)).^2 ...
%  );
%
%E = ...
%  sum( ...
%      U(J,1).^2 + ...
%      2.*XI.*U(J,1).*U(K,1) - 2.*XI.*U(J,1).^2 + 2.*YI.*U(J,1).*U(K,2) - 2.*YI.*U(J,1).*U(J,2) - 2.*U(J,1).*U(I,1) + ...
%      (XI.* U(K,1) - XI.*U(J,1) + YI.* U(K,2) - YI.*U(J,2) - U(I,1)).^2 + ...
%      (U(J,2) + XI.* U(K,2) - XI.*U(J,2) - YI.* U(K,1) + YI.*U(J,1) - U(I,2)).^2 ...
%  );
%
%E = ...
%  sum( ...
%      U(J,1).^2 + ...
%      2.*XI.*U(J,1).*U(K,1) - 2.*XI.*U(J,1).^2 + 2.*YI.*U(J,1).*U(K,2) - 2.*YI.*U(J,1).*U(J,2) - 2.*U(J,1).*U(I,1) + ...
%      XI.^2.*U(K,1).^2 + ...
%      -2.*XI.^2.*U(J,1).*U(K,1) + 2.*XI.*YI.*U(K,1).*U(K,2) - 2.*XI.*YI.*U(K,1).*U(J,2) - 2.*XI.*U(K,1).*U(I,1) + ...
%      (- XI.*U(J,1) + YI.* U(K,2) - YI.*U(J,2) - U(I,1)).^2 + ...
%      (U(J,2) + XI.* U(K,2) - XI.*U(J,2) - YI.* U(K,1) + YI.*U(J,1) - U(I,2)).^2 ...
%  );
%
%E = ...
%  sum( ...
%      U(J,1).^2 + ...
%      2.*XI.*U(J,1).*U(K,1) - 2.*XI.*U(J,1).^2 + 2.*YI.*U(J,1).*U(K,2) - 2.*YI.*U(J,1).*U(J,2) - 2.*U(J,1).*U(I,1) + ...
%      XI.^2.*U(K,1).^2 + ...
%      -2.*XI.^2.*U(J,1).*U(K,1) + 2.*XI.*YI.*U(K,1).*U(K,2) - 2.*XI.*YI.*U(K,1).*U(J,2) - 2.*XI.*U(K,1).*U(I,1) + ...
%      XI.^2.*U(J,1).^2 + ...
%      -2.*XI.*U(J,1).*YI.* U(K,2) + 2.*XI.*U(J,1).*YI.*U(J,2) + 2.*XI.*U(J,1).*U(I,1) + ...
%      (YI.* U(K,2) - YI.*U(J,2) - U(I,1)).^2 + ...
%      (U(J,2) + XI.* U(K,2) - XI.*U(J,2) - YI.* U(K,1) + YI.*U(J,1) - U(I,2)).^2 ...
%  );
%
%E = ...
%  sum( ...
%      U(J,1).^2 + ...
%      2.*XI.*U(J,1).*U(K,1) - 2.*XI.*U(J,1).^2 + 2.*YI.*U(J,1).*U(K,2) - 2.*YI.*U(J,1).*U(J,2) - 2.*U(J,1).*U(I,1) + ...
%      XI.^2.*U(K,1).^2 + ...
%      -2.*XI.^2.*U(J,1).*U(K,1) + 2.*XI.*YI.*U(K,1).*U(K,2) - 2.*XI.*YI.*U(K,1).*U(J,2) - 2.*XI.*U(K,1).*U(I,1) + ...
%      XI.^2.*U(J,1).^2 + ...
%      -2.*XI.*U(J,1).*YI.* U(K,2) + 2.*XI.*U(J,1).*YI.*U(J,2) + 2.*XI.*U(J,1).*U(I,1) + ...
%      YI.^2.*U(K,2).^2 + ...
%      - 2.*YI.^2.*U(J,2).*U(K,2) - 2.*YI.*U(I,1).*U(K,2) + ...
%      (- YI.*U(J,2) - U(I,1)).^2 + ...
%      (U(J,2) + XI.* U(K,2) - XI.*U(J,2) - YI.* U(K,1) + YI.*U(J,1) - U(I,2)).^2 ...
%  );
%
%E = ...
%  sum( ...
%      U(J,1).^2 + ...
%      2.*XI.*U(J,1).*U(K,1) - 2.*XI.*U(J,1).^2 + 2.*YI.*U(J,1).*U(K,2) - 2.*YI.*U(J,1).*U(J,2) - 2.*U(J,1).*U(I,1) + ...
%      XI.^2.*U(K,1).^2 + ...
%      -2.*XI.^2.*U(J,1).*U(K,1) + 2.*XI.*YI.*U(K,1).*U(K,2) - 2.*XI.*YI.*U(K,1).*U(J,2) - 2.*XI.*U(K,1).*U(I,1) + ...
%      XI.^2.*U(J,1).^2 + ...
%      -2.*XI.*U(J,1).*YI.* U(K,2) + 2.*XI.*U(J,1).*YI.*U(J,2) + 2.*XI.*U(J,1).*U(I,1) + ...
%      YI.^2.*U(K,2).^2 + ...
%      - 2.*YI.^2.*U(J,2).*U(K,2) - 2.*YI.*U(I,1).*U(K,2) + ...
%      YI.^2.*U(J,2).^2 + ...
%      + 2.*YI.*U(I,1).*U(J,2) + ...
%      U(I,1).^2+...
%      (U(J,2) + XI.* U(K,2) - XI.*U(J,2) - YI.* U(K,1) + YI.*U(J,1) - U(I,2)).^2 ...
%  );
%
%E = ...
%  sum( ...
%      U(J,1).^2 + ...
%      - 2.*XI.*U(J,1).^2 + ...
%      XI.^2.*U(J,1).^2 + ...
%      YI.^2.*U(J,1).^2 + ...
%      ...
%      U(J,2).^2 + ...
%      YI.^2.*U(J,2).^2 + ...
%      - 2.*XI.*U(J,2).^2 + ...
%      XI.^2.*U(J,2).^2 + ...
%      ...
%      - 2.*YI.*U(J,1).*U(J,2) +...
%      2.*XI.*YI.*U(J,1).*U(J,2) + ...
%      2.*YI.*U(J,1).*U(J,2) + ...
%      - 2.*XI.*YI.*U(J,1).*U(J,2) + ...
%      ...
%      2.*XI.*U(J,1).*U(K,1) + ...
%      -2.*XI.^2.*U(J,1).*U(K,1) + ...
%      - 2.*YI.^2.*U(J,1).*U(K,1) + ...
%      ...
%      2.*YI.*U(J,1).*U(K,2) + ...
%      -2.*XI.*YI.*U(J,1).* U(K,2) + ...
%      2.*XI.*YI.*U(J,1).*U(K,2) + ...
%      ...
%      - 2.*XI.*YI.*U(J,2).*U(K,1) + ...
%      - 2.*YI.*U(J,2).*U(K,1) + ...
%      2.*XI.*U(J,2).*YI.* U(K,1) + ...
%      ...
%      - 2.*YI.^2.*U(J,2).*U(K,2) + ...
%      2.*XI.*U(J,2).*U(K,2) + ...
%      -2.*XI.^2.*U(J,2).*U(K,2) + ...
%      ...
%      - 2.*U(I,1).*U(J,1) + ...
%      2.*XI.*U(I,1).*U(J,1) + ...
%      - 2.*YI.*U(I,2).*U(J,1) + ...
%      ...
%      2.*YI.*U(I,1).*U(J,2) + ...
%      ...
%      - 2.*U(I,2).*U(J,2) + ...
%      2.*XI.*U(I,2).*U(J,2) + ...
%      ...
%      XI.^2.*U(K,1).^2 + ...
%      YI.^2.*U(K,1).^2 + ...
%      ...
%      YI.^2.*U(K,2).^2 + ...
%      XI.^2.*U(K,2).^2 + ...
%      ...
%      U(I,1).^2+...
%      ...
%      U(I,2).^2 + ...
%      ...
%      2.*XI.*YI.*U(K,1).*U(K,2) + ...
%      - 2.*XI.*YI.*U(K,1).*U(K,2) + ...
%      ...
%      - 2.*XI.*U(I,1).*U(K,1) + ...
%      ...
%      - 2.*YI.*U(I,1).*U(K,2) + ...
%      ...
%      2.*YI.*U(I,2).*U(K,1) + ...
%      ...
%      - 2.*XI.*U(I,2).*U(K,2) + ...
%  0 ...
%  );
%
%
%E = ...
%  sum( ...
%      (1-2*XI+XI.^2+YI.^2).*U(J,1).^2 + ...
%      ...
%      (1-2*XI+XI.^2+YI.^2).*U(J,2).^2 + ...
%      ...
%      (2*XI-2*XI.^2-2*YI.^2).*U(J,1).*U(K,1) + ...
%      ...
%      (2.*YI).*U(J,1).*U(K,2) + ...
%      ...
%      (-2*YI).*U(J,2).*U(K,1) + ...
%      ...
%      (2*XI-2*XI.^2-2*YI.^2).*U(J,2).*U(K,2) + ...
%      ...
%      (-2+2*XI).*U(I,1).*U(J,1) + ...
%      ...
%      (2.*YI).*U(I,1).*U(J,2) + ...
%      ...
%      (-2*YI).*U(I,2).*U(J,1) + ...
%      ...
%      (-2+2*XI).*U(I,2).*U(J,2) + ...
%      ...
%      (XI.^2+YI.^2).*U(K,1).^2 + ...
%      ...
%      (XI.^2+YI.^2).*U(K,2).^2 + ...
%      ...
%      (1).*U(I,1).^2+...
%      ...
%      (1).*U(I,2).^2 + ...
%      ...
%      (-2*XI).*U(I,1).*U(K,1) + ...
%      ...
%      (-2*YI).*U(I,1).*U(K,2) + ...
%      ...
%      (2*YI).*U(I,2).*U(K,1) + ...
%      ...
%      (-2*XI).*U(I,2).*U(K,2) + ...
%  0 ...
%  );
%
%E = ...
%  sum( ...
%      (1-2*XI+XI.^2+YI.^2).*U(J,1).^2 + ...
%      ...
%      (1-2*XI+XI.^2+YI.^2).*U(J,2).^2 + ...
%      ...
%      2*(XI-XI.^2-YI.^2).*U(J,1).*U(K,1) + ...
%      ...
%      2*(YI).*U(J,1).*U(K,2) + ...
%      ...
%      2*(-YI).*U(J,2).*U(K,1) + ...
%      ...
%      (2*XI-2*XI.^2-2*YI.^2).*U(J,2).*U(K,2) + ...
%      ...
%      2*(-1+XI).*U(I,1).*U(J,1) + ...
%      ...
%      2*(YI).*U(I,1).*U(J,2) + ...
%      ...
%      2*(-YI).*U(I,2).*U(J,1) + ...
%      ...
%      2*(-1+XI).*U(I,2).*U(J,2) + ...
%      ...
%      (XI.^2+YI.^2).*U(K,1).^2 + ...
%      ...
%      (XI.^2+YI.^2).*U(K,2).^2 + ...
%      ...
%      (1).*U(I,1).^2+...
%      ...
%      (1).*U(I,2).^2 + ...
%      ...
%      2*(-XI).*U(I,1).*U(K,1) + ...
%      ...
%      2*(-YI).*U(I,1).*U(K,2) + ...
%      ...
%      2*(YI).*U(I,2).*U(K,1) + ...
%      ...
%      2*(-XI).*U(I,2).*U(K,2) + ...
%  0 ...
%  );


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

% Calibrating kl, and ku for a sabr model for Kienitz extrapolation
% the prices are calculated from known sabr parameters

f = 0.03; t =1;                         % forward and time
a = 0.25; b = 0.5; r = -.5;  n = 0.2;   % sabr parameters
mu = 1.5; nu = 3;                       % tail decay

k = 0.001:0.0001:1;                         % strike range

call = sprice(a, b, r, n, f, k, t,1);   % call prices (standard sabr)
put = sprice(a, b, r, n, f, k, t,0);    % put prices (standard sabr)
put(1) = 0;
call(1) = f;                            % assures forward is matched

Nparam = 2;                             % number of calibrated params

% objective function
of = @(x) of_sabr(a, b, r, n, f, k, t, mu, nu, x(1), x(2), call, put);

x0 = [.25*f; 25.5*f];                   % starting values

A = zeros(Nparam,Nparam); bc= zeros(Nparam,1);
Aeq = A; beq = bc;
lb = [.25*f; f];                        % lower bound
ub = [f; 30*f];                         % upper bound

y = fmincon(of,x0,A,bc,Aeq,beq,lb,ub);  % optimization

% verification of results
xval = 0:0.001:.25;                     % x-values
[cl, bl, al, cu, bu, au] = ...
    psabr_param_3(a, b, r, n, f, t,mu,nu,y(1),y(2));
yval = psabr_5(a, b, r, n, f, xval, t, y(1), y(2), ...
           mu, cl, bl,al, nu, cu,bu,au);% y-values calculated

plot(xval,yval);                        % plot the results
Factor = 1000000;                       % used for plotting

yval_call = sprice_5(a, b, r, n, f, xval, t, y(1), y(2), ...
    mu, cl, bl,al, nu, cu,bu,au, 1);    % SABR Call prices
figure; hold on;
    plot(xval,Factor*yval_call,'r');
    plot(k,Factor*call,'g');
hold off;

yval_put = sprice_5(a, b, r, n, f, xval, t, y(1), y(2), ...
    mu, cl, bl,al, nu, cu,bu,au, 0);    % SABR Put prices
figure; hold on;
    plot(xval,Factor*yval_put,'r');
    plot(k,Factor*put,'g');
hold off;

fval = sprice_5(a, b, r, n, f, 0, t, y(1), y(2), ...
    mu, cl, bl,al, nu, cu,bu,au, 1);    % calculate forward value
y                                       % calibrated values
f - fval                                % display difference to forward


function [Bt]=GetCoriolisMatrix1(T,Pcii,Icii,mcii,dq)
%% About the function: this is a function that is used to calculate
% Coriolis matrix of the serially linked manipulator, the return value of
% the function is (nxn) Coriolis matrix.

%% Arreguemnts:
% T is (4x4xn) transformation matrix of the serially linked robot, each
% (4x4) matrix represents the transform for each link in the base frame.
% Pcii is 3Xn matrix while each column represents the local coordinates
% of the center of mass of each link.
% Icii is (3x3xn) matrix, each 3x3 matrix of which represnets the
% associated link inertial tensor represented in its local inertial frame
% mcii is (1xn) vector, each element of which specifies a mass of one of
% the links

% Copyright Mohammad SAFEEA

n=max(size(mcii));
%% Initialization of Ai and Bi.
Bi=zeros(3,n,n);
Di=zeros(3,n,n);
L=zeros(3,3);
Lj_1=zeros(3,1);
wj=zeros(3,n);
half_wj=zeros(3,n);
%% Calculate === some auxuliary variables
Pcii_A=zeros(3,n);
w=zeros(3,n);
vci=zeros(3,n);
vi=zeros(3,n);
mcii_Pcii_A=zeros(3,n);
Pcii_A(:,1)=T(1:3,1:3,1)*Pcii(:,1);
w(:,1)=T(1:3,3,1)*dq(1);
wj(:,1)=w(:,1);
half_wj(:,1)=0.5*wj(:,1);
mcii_Pcii_A(:,1)=mcii(1)*Pcii_A(:,1);
double_kj=zeros(3,n);
double_kj(:,1)=2*T(1:3,3,1);
for i=2:n
        Pcii_A(:,i)=T(1:3,1:3,i)*Pcii(:,i);
        wj(:,i)=T(1:3,3,i)*dq(i);
        half_wj(:,i)=0.5*wj(:,i);
        w(:,i)=w(:,i-1)+wj(:,i);
        double_kj(:,i)=2*T(1:3,3,i);
        mcii_Pcii_A(:,i)=mcii(i)*Pcii_A(:,i);
end
%% calculating the links model, Mci and ddPci
for i=1:n
    %% calculating the Mci term
    Pci=Pcii_A(:,i)+T(1:3,4,i);
    L=T(1:3,1:3,i)*(trace(Icii(:,:,i))*eye(3)-2*Icii(:,:,i))*T(1:3,1:3,i)';
    Lj_1=L*T(1:3,3,i);
    for j=i:-1:2
    Bi(:,j,i)=Bi(:,j,i)+cross(Lj_1,half_wj(:,j));
    Lj_1=L*T(1:3,3,j-1);
    Bi(:,j-1,i)=Bi(:,j-1,i)+cross(Lj_1,w(:,i)-w(:,j-1));
    end
    j=1;
    %Bi(:,j,i)=Bi(:,j,i)+0.5*cross(Lj_1,wj(:,j));
    Bi(:,j,i)=Bi(:,j,i)+cross(Lj_1,half_wj(:,j));
    %% calculating the ddPci term
    vr=zeros(3,1);
    for j=i:-1:2
        Pcij=Pci-T(1:3,4,j);
        %Di(:,j,i)=Di(:,j,i)+(dq(j))*cross(T(1:3,3,j),cross(T(1:3,3,j),Pcij
        %));
        Di(:,j,i)=Di(:,j,i)+(dq(j))*(T(1:3,3,j)*(T(1:3,3,j)'*Pcij)-Pcij);
        vr=vr+cross(wj(:,j),Pcij);
        Di(:,j-1,i)=Di(:,j-1,i)+cross(double_kj(:,j-1),vr);
    end
    j=1;
    Pcij=Pci-T(1:3,4,j);
    Di(:,j,i)=Di(:,j,i)+(dq(j))*(T(1:3,3,j)*(T(1:3,3,j)'*Pcij)-Pcij);
end
Bt=zeros(n,n);
Fac_D=zeros(3,n);
Mac_B=zeros(3,n);
Pjp1_j=zeros(3,1);
%% calculating Mac for all of the links, then calculating two by filling At
%% and Bt

start=n-1;
j=n;
%% recursive rprocedure on moments and forces
        for k=1:n %% iterate through the matrix
            Mac_B(:,k)=Bi(:,k,j)+cross(mcii_Pcii_A(:,j),Di(:,k,j));
        end
        %% on forces
        Fac_D=mcii(j)*Di(:,:,j);
        Bt(j,:)=T(1:3,3,j)'*Mac_B;

for j=start:-1:1 %% iterate through the joints
    Pjp1_j=T(1:3,4,j+1)-T(1:3,4,j);
    %% recursive rprocedure on moments and forces
        for k=1:j %% iterate through the matrix
            Mac_B(:,k)=Mac_B(:,k)+Bi(:,k,j)+cross(Pjp1_j,Fac_D(:,k))+cross(mcii_Pcii_A(:,j),Di(:,k,j));
        end
        for k=j+1:n
            Mac_B(:,k)=Mac_B(:,k)+cross(Pjp1_j,Fac_D(:,k));
        end
    %% on forces
    Fac_D(:,1:j)=Fac_D(:,1:j)+mcii(j)*Di(:,1:j,j);
    Bt(j,:)=T(1:3,3,j)'*Mac_B;

end
%% close the function
end


function [X,err,iter] = lrtcR_snn(M,omega,alpha,opts)

% Solve the Noisy Low-Rank Tensor Completion (LRTC) based on Sum of Nuclear Norm (SNN) problem by M-ADMM
%
% min_{X,E} \sum_i \alpha_i*||X_{i(i)}||_* + loss(E),
% s.t. P_Omega(X) + E = M.
% loss(E) = ||E||_1 or 0.5*||E||_F^2
%
% ---------------------------------------------
% Input:
%       M       -    d1*d2*...dk tensor
%       omega   -    index of the observed entries
%       alpha   -    k*1 vector, parameters
%       opts    -    Structure value in Matlab. The fields are
%           opts.loss       -   'l1' (default): loss(E) = ||E||_1
%                               'l2': loss(E) = 0.5*||E||_F^2
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       X       -    d1*d2*...*dk tensor
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 24/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;
loss = 'l1';

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'loss');        loss = opts.loss;            end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

dim = size(M);
k = length(dim);

omegac = setdiff(1:prod(dim),omega);

X = zeros(dim);
Y = cell(k,1);
Z = Y;
E = X;
Y2 = E;
for i = 1 : k
    Y{i} = X;
    Z{i} = X;
end

iter = 0;
for iter = 1 : max_iter
    Xk = X;
    Ek = E;
    Zk = Z;
    % first super block {Z_i,E}
    sumtemp = zeros(dim);
    for i = 1 : k
        Z{i} = Fold(prox_nuclear(Unfold(X+Y{i}/mu,dim,i), alpha(i)/mu),dim,i);
        sumtemp = sumtemp + Z{i} - Y{i}/mu;
    end
    if strcmp(loss,'l1')
        E = prox_l1(-X+M-Y2/mu,1/mu);
    elseif strcmp(loss,'l2')
        E = (-X+M-Y2/mu)*(mu/(1+mu));
    else
        error('not supported loss function');
    end
    % second super block {X}
    X(omega) = (sumtemp(omega)-Y2(omega)/mu-E(omega)+M(omega))/(k+1);
    X(omegac) = sumtemp(omegac)/k;

    chg = max([max(abs(Xk(:)-X(:))), max(abs(Ek(:)-E(:))) ]);
    err = 0;
    for i = 1 : k
        dY = X-Z{i};
        err = err+norm(dY(:))^2;
        Y{i} = Y{i}+mu*dY;
        chg = max([chg,max(abs(dY(:))), max(abs((Zk{i}(:)-Z{i}(:))))]);
    end
    dY = E-M;
    dY(omega) = dY(omega)+X(omega);
    chg = max(chg,max(abs(dY(:))));
    Y2 = Y2 + mu*dY;
    err = sqrt(err+norm(dY(:))^2);

    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', err=' num2str(chg)]);
        end
    end
    if chg < tol
        break;
    end
    mu = min(rho*mu,max_mu);
end


function [tt]=tt_qshift_p(d,m)

% for integer m
% returns the periodic downward m-position shift matrix of size 2^d x 2^d
% in the QTT format
% The shift is (-m)-position upward for m < 0
% m=0 corresponds to the identity matrix
% m=2^d corresponds to the identity matrix
% m=-2^d corresponds to the identity matrix
%
% January 28, 2011
% Vladimir Kazeev
% vladimir.kazeev@gmail.com
% INM RAS
% Moscow, Russia
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% For details please see the preprint
% http://www.mis.mpg.de/publications/preprints/2011/prepr2011-36.html
% Vladimir A. Kazeev, Boris N. Khoromskij and Eugene E. Tyrtyshnikov
% Multilevel Toeplitz matrices generated by QTT tensor-structured vectors and convolution with logarithmic complexity
% January 12, 2012
% Vladimir Kazeev,
% Seminar for Applied Mathematics, ETH Zurich
% vladimir.kazeev@sam.math.ethz.ch
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

I=eye(2);
J=[0,1;0,0];
O=zeros(2);
P=J+J';

m=mod(m,2^d);
m=dec2bin(m,d);

tt=cell(d,1);

if (m(1) == '1')
	tt{1}=[P,I];
else
	tt{1}=[I,P];
end
tt{1}=reshape(tt{1},[2,2,2]);

for k=2:d-1
	if (m(k) == '1')
		tt{k}=[J',O;J,I];
	else
		tt{k}=[I,J';O,J];
	end
	tt{k}=permute(reshape(tt{k},[2,2,2,2]),[1,3,2,4]);
end

if (m(d) == '1')
	tt{d}=[J',J];
else
	tt{d}=[I,O];
end
tt{d}=reshape(tt{d},[2,2,2]);

tt=tt_reverse(tt,2);

return
end


%% kronecker
% http://www.mathworks.com/matlabcentral/fileexchange/24499-kronecker
%
function K = kronecker(A,B)
%KRONECKER   Kronecker tensor product.
%   KRONECKER(X,Y) is the Kronecker tensor product of X and Y.
%   The result is a large matrix formed by taking all possible
%   products between the elements of X and those of Y. For
%   example, if X is 2 by 3, then KRONECKER(X,Y) is
%
%      [ X(1,1)*Y  X(1,2)*Y  X(1,3)*Y
%        X(2,1)*Y  X(2,2)*Y  X(2,3)*Y ]
%
%   If either X or Y is sparse, only nonzero elements are multiplied
%   in the computation, and the result is sparse.
%
%   Class support for inputs X,Y:
%      float: double, single
%
%   NOTE: This function does exactly what Matlab KRON does, but for large
%      full matrices, the engine uses BSXFUN to accelerate the calculation.
%      Another advantage is no intermediates large matrices is generated
%      (four temporary arrays in case of KRON)
%
%   Benchmark on Intel Core2 Duo T7250 @2GHz and 2Go RAM
%   Size A/B  Speed gain
%      10       1.17
%      20       3.48
%      30       3.78
%      40       3.73
%      50       3.68
%      60       4.22
%      70       3.81
%
%   Restriction: MATLAB 2007A or later is required
%
%   See also: KRON
%
%   Author: Bruno Luong <brunoluong@yahoo.com>
%   History:
%       Original 21-Jun-2009

if ~issparse(A) && ~issparse(B)
    if ndims(A) > 2 || ndims(B) > 2
        error('kronecker:TwoDInput','Inputs must be 2-D.');
    end
    % Both inputs are full, result is full. This is faster than
    % MATLAB stock kron (indexing based)
    [ma na] = size(A);
    [mb nb] = size(B);
    A = reshape(A,[1 ma 1 na]);
    B = reshape(B,[mb 1 nb 1]);
    K = bsxfun(@times,A,B);
    K = reshape(K,[ma*mb na*nb]);

else % One of the input matrix is sparse

    % Call MATLAB stock KRON
    K = kron(A,B);

end


function [ J, grad, u ] = optimal_control ( x, e_conn, q, y1, y2, Md, Nd, L )

%*****************************************************************************80
%
%% OPTIMAL_CONTROL is a script to solve the optimal control problem.
%
%  Discussion:
%
%    The differential equation being solved has the form:
%
%      ( q(x) u_x(x) )_x = f(x)
%
%    The solution is approxiamted using 1D linear finite elements.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Parameters:
%
%    Input, X
%
%    Input, E_CONN
%
%    Input, Q
%
%    Input, Y1
%
%    Input, Y2
%
%    Input, MD
%
%    Input, ND
%
%    Input, L
%
%    Output, J
%
%    Output, GRAD
%
%    Output, U
%
  alpha = 0.000003;

  [n_nodes   , n_dimensions] = size(x     );
  [n_elements, nel_dof     ] = size(e_conn);

  n_equations      = n_nodes-2;
  ide(1)           = -1;
  ide(2:n_nodes-1) = 1:n_equations;
  ide(n_nodes)     = -2;

  dir              = zeros(2,1);

  n_gauss          = 3;  % number of points used in Gaussian integration

% q = x./(1-3*x.^2);
% q = x.^3;
% q = x.*(1-x);
%---------------------------------------------------------------------
%  Build the finite element matrices
%---------------------------------------------------------------------

  M = sparse(n_nodes    ,n_nodes    );
  K = sparse(n_equations,n_equations);
  b = zeros (n_equations,1          );
  u = zeros (n_nodes    ,1          );

  [r,w] = oned_gauss(n_gauss);

  o_g   = ones(size(r));

  for n_el=1:n_elements

    % compute value of each test function and their spatial derivaties
    % at the integration points
    nodes_local       = e_conn(n_el,:);
    x_local           = x(nodes_local,:);
    [x_g,w_g,phi,p_x] = oned_shape(x_local,r,w);

    q_local           = q(nodes_local);
    q_g               = phi*q_local;

    % compute the value of functions at the Gauss points
    f_g   = f_function(x_g, y1, y2, Md, Nd, L);

    %--------------------------------------------------------------------
    %  Integrate the weak form of the equations (element contributions)
    %--------------------------------------------------------------------
    M_loc = oned_bilinear(o_g, phi, phi, w_g);
    K_loc =-oned_bilinear(q_g, p_x, p_x, w_g);
    b_loc = oned_f_int   (f_g, phi,      w_g);

    %-----------------------------------------------------------------
    % Assemble contributions into the global system matrix
    %-----------------------------------------------------------------

    M(nodes_local,nodes_local) = M(nodes_local,nodes_local) + M_loc;

    for n_t=1:nel_dof
      n_test = ide(nodes_local(n_t));
      if (n_test > 0)  % this is an unknown, fill the row
        for n_u=1:nel_dof
          n_unk = ide(nodes_local(n_u));
          if (n_unk > 0)
            K(n_test,n_unk) = K(n_test,n_unk) + K_loc(n_t,n_u);
          end
        end
        b(n_test) = b(n_test) + b_loc(n_t);
      end
    end

  end

  %-----------------------------------------------------------------------
  %  Perform Implicit Solve
  %-----------------------------------------------------------------------
%  figure(5); spy(A);  % view the sparsity pattern in A

  du = K\b;

  %-----------------------------------------------------------------------
  %  Construct the Solution (here, we simply apply the Dirichlet bc's)
  %-----------------------------------------------------------------------
  for n=1:n_nodes
    i = ide(n);
    if (i>0)
      %  get the nodal value out of the linear system solve
      u(n) = du(i);
    else
      %  get the nodal value from the specified Dirichlet bc
      u(n) = dir(-i);
    end
  end

%  figure(12)
%  plot(x,u)
%  hold on
%  up = u_hat(x, y);
%  plot(x,up,'r+')

  lam = zeros(n_nodes  ,1);
  c = zeros(n_equations,1);
  %   Compute the right hand side of the adjoint equation
  for n_el=1:n_elements

    nodes_local       = e_conn(n_el,:);
    x_local           = x(nodes_local,:);
    [x_g,w_g,phi,p_x] = oned_shape(x_local,r,w);

    u_local           = u(nodes_local);
    u_g               = phi*u_local;

    % compute the value of functions at the Gauss points
    uhat_g   = u_hat(x_g, y1, Md, L);

    %--------------------------------------------------------------------
    %  Integrate the weak form of the equations (element contributions)
    %--------------------------------------------------------------------
    c_loc = oned_f_int   ( uhat_g-u_g, phi,      w_g);

    %-----------------------------------------------------------------
    % Assemble contributions into the global system matrix
    %-----------------------------------------------------------------
    for n_t=1:nel_dof
      n_test = ide(nodes_local(n_t));
      if (n_test > 0)  % this is an unknown, fill the row
        c(n_test) = c(n_test) + c_loc(n_t);
      end
    end

  end

  J = 0.5*sqrt(c'*M(2:end-1,2:end-1)*c);
  dlam = K'\c;

  for n=1:n_nodes
    i = ide(n);
    if (i>0)
      lam(n) = dlam(i);
    else
      lam(n) = dir(-i);
    end
  end

%  figure
%  plot(x,lam)

  grad = zeros(n_nodes,1);

  % Compute the gradient
  for n_el=1:n_elements

    nodes_local       = e_conn(n_el,:);
    x_local           = x(nodes_local,:);
    [x_g,w_g,phi,p_x] = oned_shape(x_local,r,w);

    u_local           = u(nodes_local);
    u_g               = phi*u_local;
    ux_g              = p_x*u_local;

    q_local           = q(nodes_local);
    q_g               = phi*q_local;
    qx_g              = p_x*q_local;

    lam_local         = lam(nodes_local);
    lam_g             = phi*lam_local;
    lamx_g            = p_x*lam_local;

%     grad_loc = alpha*oned_f_int( q_g, phi, w_g) ...
%               - oned_f_int( lamx_g.*ux_g, phi, w_g);
    grad_loc = alpha*oned_f_int( qx_g, p_x, w_g) ...
              - oned_f_int( lamx_g.*ux_g, phi, w_g);

    for n_t=1:nel_dof
      n_test = nodes_local(n_t);
      grad(n_test) = grad(n_test) + grad_loc(n_t);
    end

  end

%  figure
%  plot(x,grad)
%  title('gradient')

  return
end


function x = mg_ns_iter(mgdata,x0,f,level,npre,npost,sweeps)
%mg_ns_iter    performs one MG iteration (Navier-Stokes)
%   x = mg_ns_iter(mgdata,x0,f,level,npre,npost,sweeps)
%   input
%          mgdata       structure containing matrices, grid transfer
%                       operators and smoothing operators
%          x0           initial iterate
%          f            right-hand side
%          level        grid level
%          npre         number of presmoothing steps
%          npost        number of postsmoothing steps
%          sweeps       type of sweeping strategy used for Gauss-Seidel
%                       smoothing
%   output
%          x            result of one MG iteration
%
%   IFISS function: HCE; 18 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

A = mgdata(level).matrix;
P = mgdata(level).prolong;
for i=2:level, smoother(i) = mgdata(i).smoother; end

if level==2,
   n = length(A);
   [U,Sig,V] = svd(full(A));
   s = diag(Sig);
   if abs(s(n))<1.d-14, si = [1./s(1:n-1);0];
   else                 si = 1./s(1:n);
   end
   Sigi = diag(si);
   x = V*(Sigi*(U'*f));
else

   % presmooth
   x = mg_pre(A,x0, f,npre,smoother,level,sweeps);
   % Restrict residual
   r = f - A*x;
   rc = P'*r;
   % coarse grid correction
   cc = mg_ns_iter(mgdata,zeros(size(rc)),rc,level-1,npre,npost,sweeps);
   % add this line for W-cycle
%  cc = mg_ns_iter(mgdata,cc,rc,level-1,npre,npost,sweeps);
   x = x + P*cc;
   % postsmooth
   x = mg_post(A,x,f,npost,smoother,level,sweeps);
end


function [aic, sbic] = aicsbic(errors,constant,p,q,X)
% Computes the Akaike and Schwartz/Bayes Information Criteria for an ARMA(P,Q) as parameterized in
% ARMAXFILTER
%
% USAGE:
%   [AIC] = aicsbic(ERRORS,CONSTANT,P,Q)
%   [AIC,SBIC] = aicsbic(ERRORS,CONSTANT,P,Q,X)
%
% INPUTS:
%   ERRORS   - A T by 1 length vector of errors from the regression
%   CONSTANT - Scalar variable: 1 to include a constant, 0 to exclude
%   P        - Non-negative integer vector representing the AR orders to include in the model.
%   Q        - Non-negative integer vector representing the MA orders to include in the model.
%   X        - [OPTIONAL]  a T by K  matrix of exogenous variables.
%
% OUTPUTS:
%   AIC       - The Akaike Information Criteria
%   SBIC      - The Schwartz/Bayes Information Criteria
%
% COMMENTS:
%   This is a helper for ARMAXFILTER and uses the same inputs, CONSTANT, P, Q and X.  ERRORS should
%   be the errors returned from a call to ARMAXFILTER with the same values of P, Q, etc.
%
% EXAMPLES:
%   Compute AIC and SBIC from an ARMA
%       [parameters, LL, errors] = armaxfilter(y, constant, p, q);
%       [aic,sbic] = aicsbic(errors,constant,p,q)
%
%  See also ARMAXFILTER, HETEROGENEOUSAR

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 10/19/2009

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<3 || nargin>5
    error('3 to 5 inputs required')
end
if nargin==3
    q = [];
    X=[];
elseif nargin==4
    X=[];
end
%%%%%%%%%%%%%%%
% y
%%%%%%%%%%%%%%%
if size(errors,2) > 1 || length(errors)==1
    error('ERRORS series must be a column vector.')
elseif isempty(errors)
    error('ERRORS is empty.')
end

%%%%%%%%%%%%%%%
% P
%%%%%%%%%%%%%%%
if size(p,2)>size(p,1)
    p=p';
end
if isempty(p)
    p=0;
end
if min(size(p))~=1
    error('P must be a column vector of included lags')
end
if  any(p<0) || any(floor(p)~=p)
    error('P must contain non-negative integers only')
end
if max(p)>=(length(errors)-max(p))
    error('Too many lags in the AR.  max(P)<T/2')
end
maxP=max(p);
if size(p,1)==1 && p==0
    p=[];
end
if length(unique(q))~=length(q)
    error('P must contain at most one of each lag')
end
%%%%%%%%%%%%%%%
% Q
%%%%%%%%%%%%%%%
if size(q,2)>size(q,1)
    q=q';
end
if isempty(q)
    q=0;
end
if min(size(q))~=1
    error('Q must be a column vector of included lags')
end
if  any(q<0) || any(floor(q)~=q)
    error('Q must contain non-negative integers only')
end
if max(q)>=length(errors)
    error('Too many lags in the AR.  max(Q)<T')
end
maxq=max(q);
if size(q,1)==1 && q==0
    q=[];
end
if length(unique(q))~=length(q)
    error('Q must contain at most one of each lag')
end
%%%%%%%%%%%%%%%
% Constant
%%%%%%%%%%%%%%%
if ~ismember(constant,[0 1])
    error('CONSTANT must be 0 or 1')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T = length(errors);
seregression=sqrt(errors'*errors/T);

lp = length(unique(p));
lq = length(unique(q));
K=constant+lp+lq+size(X,2);

aic = log(seregression^2) + 2*K/T;
sbic = log(seregression^2) + log(T)*K/T;


function net = dsift_net(binSize, varargin)
% Define a CNN equivalent to dense SIFT

opts.numOrientations = 4 ;
opts = vl_argparse(opts, varargin) ;
opts.binSize = binSize ;

% Spatial derivatives along NO directions
NO = opts.numOrientations ;
dx = [0 0 0 ; -1 0 1 ; 0  0 0]/2 ;
dy = dx' ;
for i=0:2*NO-1
  t = (2*pi)/(2*NO)*i ;
  spatialDer{i+1} = cos(t)*dx+sin(t)*dy;
end
spatialDer = single(cat(4, spatialDer{:})) ;

% Spatial bilienar binning
a = 1 - abs(((1:2*opts.binSize) - (2*opts.binSize+1)/2)/opts.binSize);
bilinearFilter = repmat(single(a'*a), [1, 1, 1, 2*NO]) ;

% Stacking of SIFT cells into 4x4 blocks.

sigma = 1.5 ;
mask = {} ;
t = 0 ;
for i=1:4
  for j=1:4
    for o=1:2*NO
      t=t+1 ;
      mask{t} = zeros(4,4,2*NO) ;
      mask{t}(i,j,o) = exp(-0.5*((i-2.5).^2 + (j-2.5).^2) / sigma^2) ;
    end
  end
end
mask = single(cat(4, mask{:})) ;

net.layers = {} ;
net.layers{end+1} = struct('type','conv', ...
  'filters', spatialDer, ...
  'biases', zeros(size(spatialDer,4),1,'single'), ...
  'stride', 1, 'pad', 0) ;
if 0
  net.layers{end+1} = struct('type','noffset', 'param', [.5*cos(2*pi/8), .5]) ;
  net.layers{end+1} = struct('type','relu') ;
else
  net.layers{end+1} = get_hog_binning_layer(opts) ;
end
net.layers{end+1} = struct('type','conv', ...
  'filters', bilinearFilter, ...
  'biases', [], ...
  'stride', binSize, 'pad', 0) ;
net.layers{end+1} = struct('type','conv', ...
  'filters', mask, ...
  'biases', [], ...
  'stride', 3, 'pad', 0) ;
net.layers{end+1} = struct('type', 'normalize', 'param', [128*2, 0.000001, 1, .5]) ;
net.layers{end+1} = get_hog_clamp_layer(opts) ;

% -------------------------------------------------------------------------
function l = get_hog_clamp_layer(opts)
% -------------------------------------------------------------------------
l.type = 'custom' ;
l.forward = @hog_clamp_forward ;
l.backward = @hog_clamp_backward ;

function res_ = hog_clamp_forward(ly,res,res_)
res_.x = min(res.x,single(0.2)) ;
%res_.x = res.x ;

function res = hog_clamp_backward(ly,res,res_)
res.dzdx = res_.dzdx .* (res.x <= 0.2) ;

% -------------------------------------------------------------------------
function l = get_hog_binning_layer(opts)
% -------------------------------------------------------------------------
l.type = 'custom' ;
l.NO = opts.numOrientations ;
l.forward = @hog_binning_forward ;
l.backward = @hog_binning_backward ;

function res_ = hog_binning_forward(ly,res,res_)
x = res.x ;
n2 = sum(x.^2,3)/ly.NO ;
n = sqrt(n2) ;
cs = bsxfun(@rdivide, x, max(n, 1e-10)) ;
cs = max(min(cs,1),-1) ;
delta = 1 - (2*ly.NO)/(2*pi)*acos(cs) ;
w = max(0, delta) ;
res_.x = bsxfun(@times, n, w) ;

function res = hog_binning_backward(ly,res,res_)
% forward computations
x = res.x ;
n2 = sum(x.^2,3)/ly.NO ;
n = sqrt(n2) ;
cs = bsxfun(@rdivide, x, max(n, 1e-10)) ;
cs = max(min(cs,1),-1) ;
delta = 1 - (2*ly.NO)/(2*pi)*acos(cs) ;
w = max(0, delta) ;

dn = bsxfun(@rdivide, x/ly.NO, max(n, 1e-10)) ;
dwdcs = ((2*ly.NO)/(2*pi) ./ sqrt(1.0000001 - cs.^2)) .* (delta > 0) .* res_.dzdx ;
dw = ...
  + dwdcs .* repmat(1./n, [1 1 2*ly.NO]) ...
  - bsxfun(@times, sum(bsxfun(@rdivide, dwdcs.*x/ly.NO, n.*n2),3), x) ;
res.dzdx = bsxfun(@times, sum(res_.dzdx .* w, 3), dn) +  bsxfun(@times, n, dw) ;


function C = ecov_music_1d(design, wavelength, doas, P, noise_var, snapshot_count)
%ECOV_MUSIC_1D Asymptotic covariance matrix of the estimation errors
%of the classical MUSIC algorithm.
%Syntax:
%   C = ECOV_MUSIC_1D(design, wavelength, doas, P, noise_var[, snapshot_count])
%Inputs:
%   design - Array design.
%   wavelength - Wavelength.
%   doas - DOA vector in radians.
%   P - Source covariance matrix. If all sources are uncorrelated and
%       shares the same power, you can just pass in a scalar. If all
%       sources are uncorrelated but have different powers, you can just
%       pass in a vector.
%   noise_var - Noise power.
%   snapshot_count - (Optional) number of snapshots. Default is one.
%Output:
%   C - Asymptotic error covariance matrix.
%Reference:
%   [1] P. Stoica and A. Nehorai, "MUSIC, maximum likelihood, and
%       Cramer-Rao bound: further results and comparisons," IEEE
%       Transactions on Acoustics, Speech and Signal Processing, vol. 38,
%       no. 12, pp. 2140-2150, Dec. 1990.
%   [2] P. Stoica and A. Nehorai, "MUSIC, maximum likelihood, and
%       Cramer-Rao bound," IEEE Transactions on Acoustics, Speech and
%       Signal Processing, vol. 37, no. 5, pp. 720-741, May 1989.
if design.dim ~= 1
    error('1D array expected.');
end
if nargin <= 5
    snapshot_count = 1;
end
m = design.element_count;
k = length(doas);
P = unify_source_power_matrix(P, k);
[A, D] = steering_matrix(design, wavelength, doas);
H = D'*(eye(m) - A/(A'*A)*A')*D;
B = P\(P + noise_var*eye(k)/(A'*A))/P;
h = real(1 ./ diag(H));
C = (noise_var/2/snapshot_count) * (real(H .* B.') .* (h*h'));
end


function N = tangentspherefactory(M, x)
% Returns a manifold struct. for the sphere on the tangent space to M at x.
%
% N = tangentspherefactory(M, x)
%
% N defines a manifold that is the unit sphere on the tangent space to M
% at x. Points are represented as tangent vectors of unit norm. Tangent
% vectors are represented as tangent vectors orthogonal to the root point,
% with respect to the Riemannian metric on the tangent space.
%
% This is chiefly useful to solve optimization problems involving unit norm
% tangent vectors to M at x, which notably comes up when looking for
% extreme eigenvectors of the Hessian of a cost function on M at x, for
% example. The Riemannian structure on this sphere is that of a Riemannian
% submanifold of the (Euclidean) tangent space, equipped with the
% Riemannian metric of M at that point.
%
% See also: hessianextreme

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, March 16, 2015.
% Contributors:
% Change log:
%
%   Nov 27, 2015 (NB):
%       Extra projection added in the retraction, to prevent numerical
%       drift.
%
%   Jun 23, 2021 (QR):
%       Extra projection in retraction changed to M.tangent.
%
%   Jun 23, 2021 (NB):
%       Several fixes to the logic of this factory that should help for
%       more sophisticated manifolds where representations of points,
%       tangent vectors and ambient vectors are not straightforward.

    % N is the manifold we build.
    % y is a point on N, thus also a tangent vector to M at x.
    % This is a typical Riemannian submanifold of a Euclidean space,
    % hence it is easy to describe in terms of the tools available for M.
    N = struct();
    N.name = @() sprintf('Sphere in a tangent space of [%s]', M.name());

    % u, u1 and u2 are tangent vectors to N at y.
    % The tangent space to N at y is a subspace of the tangent space to M
    % at x, thus u, u1 and u2 are also tangent vectors to M at x.

    N.dim   = @() M.dim() - 1;
    N.inner = @(y, u1, u2) M.inner(x, u1, u2);
    N.norm  = @(y, u)      M.norm(x, u);
    N.proj  = @(y, v) M.lincomb(x, 1, v, -M.inner(x, v, y), y);
    N.typicaldist = @() 1;
    N.tangent = N.proj;
    N.egrad2rgrad = N.proj;
    N.retr = @retraction;
    N.exp = N.retr;
    function yy = retraction(y, u, t)
        if nargin == 2
            t = 1;
        end
        y_plus_tu = M.lincomb(x, 1, y, t, u);
        % Mathematically, y_plus_tu is exactly in the tangent space to M at
        % x. However, numerically, it may 'leave' the tangent space
        % slightly. The extra 'projection' on the next line is not required
        % mathematically but helps prevent numerical issues sometimes.
        % If this proves to be a huge slow down, one could consider adding
        % a type of counter that only executes this extra step every so
        % often, instead of at every call.
        y_plus_tu = M.tangent(x, y_plus_tu);
        nrm = M.norm(x, y_plus_tu);
        yy = M.lincomb(x, 1/nrm, y_plus_tu);
    end
    N.rand = @random;
    function y = random()
        y = M.randvec(x);
        nrm = M.norm(x, y);
        y = M.lincomb(x, 1/nrm, y);
    end
    N.randvec = @randvec;
    function u = randvec(y)
        u = N.proj(y, M.randvec(x));
        nrm = N.norm(y, u);
        u = M.lincomb(x, 1/nrm, u);
    end
    N.zerovec = @(y) M.zerovec(x);
    N.transp = @(y1, y2, u) N.proj(y2, u);
    N.hash = @(y) ['z' hashmd5(M.vec(x, y))];

    N.lincomb = @Nlincomb;
    function v = Nlincomb(y, a1, d1, a2, d2) %#ok<INUSL>
        if nargin == 3
            v = M.lincomb(x, a1, d1);
        elseif nargin == 5
            v = M.lincomb(x, a1, d1, a2, d2);
        else
            error('lincomb takes either 3 or 5 inputs.');
        end
    end

end


function Xtensor = tucker2multiarray(X)
% Converts a 3d Tucker form tensor to a multiarray.
%
% function Xtensor = tucker2multiarray(X)
%
% X has fields U1, U2, U3, and G.
%
% The matrices U1 (n1-by-r1), U2 (n2-by-r2) and U3 (n3-by-r3) are
% orthogonal matrices.
% G (r1-by-r2-by-r3) is a multidimensional array.
%
% See also: fixedrankfactory_tucker_preconditioned

% This file is part of Manopt: www.manopt.org.
% Original authors: Hiroyuki Kasai and Bamdev Mishra, June 05, 2015.
% Contributors:
% Change log:

    U1 = X.U1;
    U2 = X.U2;
    U3 = X.U3;
    G = X.G;

    % Tensor size
    n1 = size(U1, 1);
    n2 = size(U2, 1);
    n3 = size(U3, 1);

    % Core size
    [r1, r2, r3] = size(G);

    % Multplication by U1
    G1 = reshape(G, r1, r2*r3);
    GU1 = reshape(U1*G1, n1, r2, r3);

    % Further multplication by U2
    G2 = reshape(permute(GU1, [2 1 3]), r2, n1*r3);
    GU1U2 = permute(reshape(U2*G2, n2, n1, r3), [2 1 3]);

    % Further multplication by U3
    G3 = reshape(permute(GU1U2, [3 1 2]), r3, n1*n2);
    GU1U2U3 = permute(reshape(U3*G3, n3, n1, n2), [2 3 1]);

    Xtensor = GU1U2U3;% Full tensor

end


function rval=rtest_1()

n = 49;
[A,b] = testmat(n,2);
x0 = [1:n]'/(n+1);
y0 = [1:n]'/(n+1);
x = repmat(x0,1,n);
y = repmat(y0',n,1);
xy = [x(:),y(:)];

rval = 0;
try
    T = mst(A);
    T = T + diag(diag(A));
    rval = 1;
catch
    lasterr
end;

try
    A(1,2)= -1;
    A(2,1)= -1;
    T = prim_mst(A);
    rval = 0;
catch
    rval = 1;
end;

function [A,b] = testmat( n,stencil )
    h = 1/(n+1);

    % initialization
    x = [1:n]'/(n+1);
    y = [1:n]'/(n+1);
    u = zeros(n,n);

    % exact solution
    u0 = repmat(x.^4,1,n) + repmat(12*y'.^2,n,1);

    % matices
    T0 = -ones(n);
    T0 = sparse( triu(tril(T0,-1),-1) + triu(tril(T0,1),1) );
    I  = speye(n);
    if stencil == 1
        T = (-8*I - T0) / 3;
        B = (I - T0) / 3;
    else
        T = (-20*I - 4*T0) / 6;
        B = (4*I - T0) / 6;
    end

    A = kron(I,T) - kron(T0,B);

    % boundary
    b = zeros(n);
    if stencil == 1
        b(1,:) = b(1,:) + 12*y'.^2 + 12*(y-h)'.^2 + 12*(y+h)'.^2;
        b(n,:) = b(n,:) + 3 + 12*y'.^2 + 12*(y-h)'.^2 + 12*(y+h)'.^2;
        b(:,1) = b(:,1) + x.^2 + (x-h).^2 + (x+h).^2;
        b(:,n) = b(:,n) + x.^2 + (x-h).^2 + (x+h).^2 + 12*3;
    else
        b(1,:) = b(1,:) + 4* 12*y'.^2 + 12*(y-h)'.^2 + 12*(y+h)'.^2;
        b(n,:) = b(n,:) + 6 + 4* 12*y'.^2 + 12*(y-h)'.^2 + 12*(y+h)'.^2;
        b(:,1) = b(:,1) + 4* x.^2 + (x-h).^2 + (x+h).^2;
        b(:,n) = b(:,n) + 4* x.^2 + (x-h).^2 + (x+h).^2 + 12*6;
    end
    % fix corners
    b(1,n) = b(1,n) - 12;
    b(n,1) = b(n,1) - 1;
    b(n,n) = b(n,n) - 13;
    % normalize
    if stencil == 1
        b = - b / 3;
    else
        b = - b / 6;
    end
    % add source
    f = 12*x.^2 + 24;
    f = repmat(f,1,n);
    b = b + f * h^2;

    b = b(:);
return


function [ h,mycolormap ] = PlotVorticity( STATE,varargin )

mycolormap = VorticityColormap( 64 );
        h=0;

if nargout>1
    return;
end
% cavity grid and PDE operators

N = sqrt(size(STATE,1))-1;

[Grid]=CollocationGrid_q(N);    % the computational grid
[~,Grid.wc] = clencurt(N);Grid.W = kron(Grid.wc,Grid.wc);   % integration weights
 disp('grid created')

[ Operators ] = CreateOperators_psi( Grid.D,eye(N+1));
disp('operators for psi created')

% grid for the plot
[xx2,yy2] = meshgrid(Grid.x,Grid.x);
[xxx,yyy] = meshgrid(-1:.005:1,-1:.005:1);
% vorticity
w = - Operators.del2*STATE;
ww = reshape(real(w),N+1,N+1);
www = interp2(xx2,yy2,ww,xxx,yyy,'spline');

a = 2;
if ~isempty(varargin)
    a = varargin{1};
end

www(www>a)=a;www(www<-a)=-a;

%         figure(100+randi(100))
        contourf(xxx,yyy,www,100,'LineStyle','None') ;    % vorticty
        axis square;
        box on;
        colormap(mycolormap);
        caxis([-a,a]);
        ax = gca;
        ax.XTick = [-1  1];
        ax.YTick = [-1  1];
end

function [ cmap ] = VorticityColormap( ncol )
%VORTICITYCOLORMAP Summary of this function goes here
%   Detailed explanation goes here
cmap=jet(ncol);
for i=ncol/8+1:7*ncol/16
    cmap(i,:)=[(i-ncol/8)/(5*ncol/16),(i-ncol/8)/(5*ncol/16),1];    % blue fading to white
end

    cmap(7*ncol/16+1:9*ncol/16,:)=1;  % white

for i=9*ncol/16+1:7*ncol/8
    cmap(i,:)=[1,1-(i-9*ncol/16)/(5*ncol/16),1-(i-9*ncol/16)/(5*ncol/16)];    % white turning red
end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Setting up advection-diffusion solver
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [C,laplacian_C] = please_Update_Adv_Diff_Concentration_Flux_Limiter_FV(C,dt,dx,dy,uX,uY,k)

% C:     concentration
% dt:    time-step
% dx,dy: spatial steps in x and y, respectively
% uX:    x-Component of Velocity
% uY:    y-Component of Velocity
% k:     diffusion coefficient

% Compute Fluxes (Note: these calculations could be parallalized)
selection = 'superbee';
Fx = give_Necessary_Fluxes(C,dx,uX,'x',selection,dt); % Fluxes in x
Fy = give_Necessary_Fluxes(C,dy,uY,'y',selection,dt); % Fluxes in y

% "forward difference of fluxes in x"
F2x = [Fx(:,2:end) Fx(:,1)];
F1x = [Fx(:,end) Fx(:,1:end-1)];
diffX = 0.5/dx*( F2x - F1x );

% "forward differences of fluxes in y"
F2y = [Fy(2:end,:); Fy(1,:)];
F1y = [Fy(end,:); Fy(1:end-1,:)];
diffY = 0.5/dy*( F2y - F1y );

% Compute 2nd Derivative Terms
Cxx = DD(C,dx,'x');
Cyy = DD(C,dy,'y');

% Forms Laplacian
laplacian_C = Cxx+Cyy;

% UPWIND
C = C - dt * ( diffX + diffY ) + dt*( k*laplacian_C );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Computes derivative based on sign of Velocity, u, using UPWIND
% approach
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C_z = give_Necessary_Fluxes(C,dz,uZ,string,selection,dt)

C_z = zeros(size(C));
len = length(uZ(:,1));
signs = sign(uZ);

if strcmp(string,'x')

    %For periodicity on ends w/ UPWIND
    for i=1:len

        %left side of grid
        if signs(i,1) <= 0
            r = ( C(i,2) - C(i,1) ) / ( C(i,1) - C(i,len) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,1) =  uZ(i,1)*C(i,2)   + 0.5*abs( uZ(i,1) )*( 1 - abs( 0.5*dt*uZ(i,1)/dz ) )*phi*( C(i,2) - C(i,len) );
        else
            r = ( C(i,1) - C(i,len) ) / ( C(i,2) - C(i,1) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,1) =  uZ(i,1)*C(i,len) + 0.5*abs( uZ(i,1) )*( 1 - abs( 0.5*dt*uZ(i,1)/dz ) )*phi*( C(i,2) - C(i,len) );
        end

        %right side of grid
        if signs(len,1) <= 0
            r = ( C(i,1) - C(i,len) ) / ( C(i,len) - C(i,len-1) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,len) =  uZ(i,len)*C(i,1)     + 0.5*abs( uZ(i,len) )*( 1 - 0.5*abs( dt*uZ(i,len)/dz ) )*phi*( C(i,1) - C(i,len-1) );
        else
            r = ( C(i,len) - C(i,len-1) ) / ( C(i,1) - C(i,len) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(i,len) =  uZ(i,len)*C(i,len-1) + 0.5*abs( uZ(i,len) )*( 1 - 0.5*abs( dt*uZ(i,len)/dz ) )*phi*( C(i,1) - C(i,len-1) );
        end

    end
    %Standard Upwind
    for i=1:len
        for j=2:len-1
            if signs(i,j) <= 0
                r = ( C(i,j+1) - C(i,j) ) / ( C(i,j) - C(i,j-1) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(i,j) = uZ(i,j)*C(i,j+1) + 0.5*abs( uZ(i,j) )*( 1 - 0.5*abs( dt*uZ(i,j)/dz ) )*phi*( C(i,j+1) - C(i,j-1) );
            else
                r = ( C(i,j) - C(i,j-1) ) / ( C(i,j+1) - C(i,j) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(i,j) = uZ(i,j)*C(i,j-1) + 0.5*abs( uZ(i,j) )*( 1 - 0.5*abs( dt*uZ(i,j)/dz ) )*phi*( C(i,j+1) - C(i,j-1) );
            end
        end
    end

    % Ends x-Direction calculation %

elseif strcmp(string,'y')

    %For periodicity on ends w/ UPWIND
    for i=1:len

        %bottom of grid
        if signs(1,i) <= 0
            r = ( C(2,i) - C(1,i) ) / ( C(1,i) - C(len,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(1,i) =  uZ(1,i)*C(2,i)   + 0.5*abs( uZ(1,i) )*( 1 - abs( 0.5*dt*uZ(1,i)/dz ) )*phi*( C(2,i) - C(len,i) );
        else
            r = ( C(1,i) - C(len,i) ) / ( C(2,i) - C(1,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(1,i) =  uZ(1,i)*C(len,i) + 0.5*abs( uZ(1,i) )*( 1 - abs( 0.5*dt*uZ(1,i)/dz ) )*phi*( C(2,i) - C(len,i) );
        end

        %top of grid
        if signs(len,1) <= 0
            r = ( C(1,i) - C(len,i) ) / ( C(len,i) - C(len-1,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(len,i) =  uZ(len,i)*C(1,i)       + 0.5*abs( uZ(len,i) )*( 1 - abs( 0.5*dt*uZ(len,i)/dz ) )*phi*( C(1,i) - C(len-1,i) );
        else
            r = ( C(len,i) - C(len-1,i) ) / ( C(1,i) - C(len,i) );
            phi = please_Give_Flux_Limiter(r,selection);
            C_z(len,i) =  uZ(len,i)*C(len-1,i)   + 0.5*abs( uZ(len,i) )*( 1 - abs( 0.5*dt*uZ(len,i)/dz ) )*phi*( C(1,i) - C(len-1,i) );
        end

    end

    %Standard Upwind
    for i=1:len
        for j=2:len-1
            if signs(j,i) <= 0
                r = ( C(j+1,i) - C(j,i) ) / ( C(j,i) - C(j-1,i) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(j,i) = uZ(j,i)*C(j+1,i) + 0.5*abs( uZ(j,i) )*( 1 - abs( 0.5*dt*uZ(j,i)/dz ) )*phi*( C(j+1,i) - C(j-1,i) );
            else
                r = ( C(j,i) - C(j-1,i) ) / ( C(j+1,i) - C(j,i) );
                phi = please_Give_Flux_Limiter(r,selection);
                C_z(j,i) = uZ(j,i)*C(j-1,i) + 0.5*abs( uZ(j,i) )*( 1 - abs( 0.5*dt*uZ(j,i)/dz ) )*phi*( C(j+1,i) - C(j-1,i) );
            end
        end
    end

    % Ends y-Direction calculation %

else

    fprintf('\n\n\n ERROR IN FUNCTION FOR COMPUTING FLUX LIMITERS\n');

end

clear signs; clear len;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes flux limiter with choice of which one
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function phi = please_Give_Flux_Limiter(r,selection)

if strcmp(selection,'superbee')
    max1 = max( min(1,2*r),min(2,r) );
    phi = max(0,max1);
else
    fprintf('\n\n');
    error('NEED TO CHOOSE AN APPROPRIATE FLUX LIMITER');
end