function [g1, g2] = simwhiteXrbfinfwhiteKernGradient(simKern, rbfKern, t1, varargin)

% SIMWHITEXRBFINFWHITEKERNGRADIENT Compute a cross gradient between a
% SIM-WHITE kernel and an RBF-WHITE kernel (with integration limits between
% minus infinity and infinity).
% FORMAT
% DESC computes cross gradient of parameters of a cross kernel between a
% SIM-WHITE and an RBF-WHITE kernels for the multiple output kernel.
% ARG simKern : the kernel structure associated with the SIM-WHITE kernel.
% ARG rbfKern : the kernel structure associated with the RBF-WHITE kernel.
% ARG t1 : inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the SIM-WHITE kernel, for
% ordering see simwhiteKernExtractParam.
% RETURN g2 : gradient of the parameters of the RBF-WHITE kernel, for
% ordering see rbfwhiteKernExtractParam.
%
% FORMAT
% DESC computes cross kernel terms between a SIM-WHITE kernel and an
% RBF-WHITE kernel (with integration limits between minus infinity and
% infinity) for the multiple output kernel.
% ARG simKern : the kernel structure associated with the SIM-WHITE kernel.
% ARG rbfKern : the kernel structure associated with the RBF-WHITE kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the SIM-WHITE kernel, for
% ordering see simwhiteKernExtractParam.
% RETURN g2 : gradient of the parameters of the RBF-WHITE kernel, for
% ordering see rbfwhiteKernExtractParam.
%
% SEEALSO : multiKernParamInit, multiKernCompute, simwhiteKernParamInit,
% rbfinfwhiteKernParamInit, simwhiteKernExtractParam, rbfinfwhiteKernExtractParam
%
% COPYRIGHT : David Luengo, 2009

% KERN

if nargin < 5
    t2 = t1;
else
    t2 = varargin{1};
end
covGrad = varargin{end};

if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end
if simKern.variance ~= rbfKern.variance
  error('Kernels cannot be cross combined if they have different variances.')
end

g1 = zeros(1, simKern.nParams);
g2 = zeros(1, rbfKern.nParams);

T1 = repmat(t1, 1, size(t2, 1));
T2 = repmat(t2.', size(t1, 1), 1);
deltaT = T1 - T2;

% Parameters required for further computations
isStationary = simKern.isStationary;
variance = simKern.variance;
decay = simKern.decay;
sensitivity = simKern.sensitivity;
invWidth = rbfKern.inverseWidth;

% Setting normalised parameters for computation of K
simKern.variance = 1;
rbfKern.variance = 1;
simKern.sensitivity = 1;
K = simwhiteXrbfinfwhiteKernCompute(simKern, rbfKern, t1, t2);

% Gradient w.r.t. the decay (simKern)
g1(1) = variance * sensitivity * sum(sum( ...
    ((decay/invWidth-deltaT) .* K  + 1/sqrt(2*pi*invWidth) ...
        * (exp(-(decay*T1+0.5*invWidth*(T2.^2))) * (~isStationary) ...
            - exp(-0.5*invWidth*(deltaT.^2)))) ...
    .* covGrad));
%       * exp(0.5*(decay^2)/invWidth-decay*deltaT) ...
%         .* (exp(-0.5*invWidth*((T2+decay/invWidth).^2)) * (~isStationary) ...
%           - exp(-0.5*invWidth*((deltaT-decay/invWidth).^2)))) ...

% Gradient w.r.t. the inverse width (rbfKern)
g2(1) = 0.5 * variance * sensitivity * sum(sum( ...
    ((-(decay/invWidth)^2) * K  + 1/sqrt(2*pi*invWidth) ...
        * ((T2-decay/invWidth) .* exp(-(decay*T1+0.5*invWidth*(T2.^2))) * (~isStationary) ...
            + (deltaT+decay/invWidth) .* exp(-0.5*invWidth*(deltaT.^2)))) ...
    .* covGrad));
%       * exp(0.5*(decay^2)/invWidth - decay*deltaT) ...
%         .* ((T2-decay/invWidth) .* exp(-0.5*invWidth*((T2+decay/invWidth).^2)) * (~isStationary) ...
%           + (deltaT+decay/invWidth) .* exp(-0.5*invWidth*((deltaT-decay/invWidth).^2)))) ...

% Gradient w.r.t. sigma_r^2
g1(2) = sensitivity * sum(sum(K .* covGrad));
g2(2) = 0; % Otherwise it is counted twice

% Gradient w.r.t. sensitivity (only simKern)
g1(3) = variance * sum(sum(K .* covGrad));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

domain = [-1,1,-1,1,-1,1];
bc = [1,1,1,1,1,1];
nx = 10;
ny = nx;
nz = nx;
r1 = 1; r2 = 0; r3 = 0; x0 = pi/10^4; y0 = 0; z0 = 0;
bm = 1; bp = 1; am = 1; ap = 1;
pde = ConstFun(am,ap,bm,bp,r1,r2,r3,x0,y0,z0);
mesh = genMesh3D(domain, nx, ny, nz);
mesh = enrichMesh3D(mesh,2); % Mesh detail level = 1 (for IFE).
mesh = genIntfMesh3D(mesh,pde.intf);
fem = genNedFEM3D(mesh,bc);

%% 1. use vectorization to generate face and face2elem data structure

% reoder the nodes such that the nodes of interface elements appear at the
% end

IntMeshNode = unique(reshape(mesh.t(mesh.tLoc<0,:),[],1));
MeshNodeID = true(size(mesh.p,1),1);
MeshNodeID(IntMeshNode) = false;
NodeOrderNew = zeros(size(mesh.p,1),1);
NodeOrderNew(MeshNodeID) = 1:sum(MeshNodeID);
NodeOrderNew(~MeshNodeID) = sum(MeshNodeID)+1:size(mesh.p,1);

mesh.t = NodeOrderNew(mesh.t);
mesh.e = NodeOrderNew(mesh.e);
[~,NodeOrderNewSortID] = sort(NodeOrderNew);
mesh.p = mesh.p(NodeOrderNewSortID,:);
mesh.pLoc = mesh.pLoc(NodeOrderNewSortID);

tic

Np = size(mesh.p,1);
node = [mesh.p;mesh.eIntP];
allCutElem = mesh.t(mesh.tLoc<0,:);
isCutElem = (mesh.tLoc<0);
vSign = [mesh.pLoc;zeros(size(mesh.eIntP,1),1)];
isInterfaceNode = false(size(node,1), 1);
isInterfaceNode(allCutElem(:)) = true; % include the vertices of allCutElem
numCutElemV1 = sum(isInterfaceNode);
isInterfaceNode(Np+1:end) = true;    % and the cut points and the aux points
allCutElemReoderTmp = zeros(size(node,1),1);
numCutElemV2 = sum(isInterfaceNode);
allCutElemReoderTmp(isInterfaceNode) = 1:numCutElemV2;
allCutElemReoder = allCutElemReoderTmp(allCutElem);
interfaceNode = node(isInterfaceNode,:);
ntI = -min(mesh.tLoc);
intID = find(mesh.tLoc<0);

tetElem = zeros(12*ntI,4);
tetElemLoc = zeros(12*ntI,1);
idx2cube = zeros(12*ntI,1);
tetcount = 0;

for i = 1:ntI
    tID = intID(i);
    t_e = mesh.t_e(tID,:);
    pLocK = mesh.pLoc(mesh.t(tID,:));
    vert0 = mesh.p(mesh.t(tID,:),:);
    nodeid = [allCutElemReoder(i,:),numCutElemV1-mesh.eLoc(t_e(mesh.eLoc(t_e)<0))'];
    vert2 = vert0(pLocK>0,:); % plus domain
    vert1 = vert0(pLocK<0,:); % minus domain
    intpt0 = mesh.eIntP(-mesh.eLoc(t_e(mesh.eLoc(t_e)<0)),:);
    id1 = [find(pLocK<0)', 5:4+size(intpt0,1)];
    id2 = [find(pLocK>0)', 5:4+size(intpt0,1)];
    p1 = [vert1;intpt0]; DT = delaunayTriangulation(p1); t1 = DT.ConnectivityList;
    p2 = [vert2;intpt0]; DT = delaunayTriangulation(p2); t2 = DT.ConnectivityList;
    tetElem(tetcount+1:tetcount+size(t1,1),:) = nodeid(id1(t1));
    idx2cube(tetcount+1:tetcount+size(t1,1)) = tID;
    tetElemLoc(tetcount+1:tetcount+size(t1,1)) = 1; % inside subdomain
    tetcount = tetcount+size(t1,1);
    tetElem(tetcount+1:tetcount+size(t2,1),:) = nodeid(id2(t2));
    idx2cube(tetcount+1:tetcount+size(t2,1)) = tID;
    tetElemLoc(tetcount+1:tetcount+size(t2,1)) = 2; % outside subdomain
    tetcount = tetcount+size(t2,1);
end
tetElem(tetcount+1:end,:) = [];
tetElemLoc(tetcount+1:end,:) = [];
idx2cube(tetcount+1:end,:) = [];
[tetElem, ortidx, volume] = fixorder3(interfaceNode, tetElem);
% there are some tets which are coplane, need to get rid of them
idPlane = (volume<=10^(-12));
tetElem(idPlane,:) = [];
volume(idPlane,:) = [];
tetElemLoc(idPlane,:) = [];
idx2cube(idPlane,:) = [];
localidx2globalidx = find(isInterfaceNode); % tetElem points to interfaceNode
tetElem = localidx2globalidx(tetElem);
% reorder tetElem such that the index of its nodes are local, i.e.,
% starting from 1,2,...

% there are some tets which are on the interface, but contained in the surrounding tets
% need to get rid of them (but the following algorithm may not be robust)
tetSign = vSign(tetElem);
idSliver = (sum(abs(tetSign),2)==0);
tetElem(idSliver,:) = [];
volume(idSliver,:) = [];
tetElemLoc(idSliver,:) = [];
idx2cube(idSliver,:) = [];
PolyVolume = accumarray([idx2cube,tetElemLoc],volume);
PolyVolume = PolyVolume(mesh.tLoc<0,:);
h = (PolyVolume(:,1) + PolyVolume(:,2)).^(1/3);
% the first component contains the volume of the polyhedron on the
% subdomain 1, the first component contains the volume of the polyhedron
% on the subdomain 2

toc

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Get triangular faces for interrior elements and interface

tic
localFace = [2 3 4; 1 4 3; 1 2 4; 1 3 2];
NT = size(tetElem,1);
tface = zeros(4*NT, 3);
tface2elem = zeros(4*NT, 1);
iface = zeros(4*NT, 3);
iface2elem = zeros(4*NT, 1);
% find the interior tet elements
isIntTet = min(vSign(tetElem),[], 2) == -1; % can not be == -1 as there is sliver
intTet = tetElem(isIntTet,:);
% find the corresponding cube indices
intIdx2cube = idx2cube(isIntTet);
% find triangular interface
T = auxstructure3(intTet);
neighbor = T.neighbor; % if a face is on the boundary, then the neighbor element index is itself
clear T;
tmp = (1:size(intTet, 1))';
ct = 0;
ci = 0;
for i = 1:4
    face = intTet(:, localFace(i,:));
    % find the triangle faces of polyhedron
    % 1. face and its neighbor associated to different cubes, and
    % 2. face is not on the boundary of all cut elements (which are squares)
    isPolyTriFace = ((neighbor(:, i) == tmp) | (intIdx2cube ~= intIdx2cube(neighbor(:,i)))) &...
        (sum(abs(vSign(face)), 2) > 0);% & (sum(abs(vSign(face)), 2) > 0);
    c2 = ct + sum(isPolyTriFace);
    tface((ct+1):c2,:) = face(isPolyTriFace,:);
    tface2elem((ct+1):c2,:) = intIdx2cube(isPolyTriFace); % the indices of the polyhedron
    ct = c2;
    % note that only interior elements are treated

    % find the triangle faces on interface with normal points to exterior
    % 1. face is on the boundary and
    % 2. all vertices are on the interface
    isInterface = (sum(abs(vSign(face)), 2) == 0);

    % add to interface
    c4 = ci + sum(isInterface);
    iface((ci+1):c4,:) = face(isInterface,:); % the interface tri faces.
    iface2elem((ci+1):c4,:) = intIdx2cube(isInterface); % the indices of the polyhedron
    ci = c4;
end
iface((ci+1):end,:) = [];
iface2elem((ci+1):end,:) = [];
c2old = c2;
% plot to check
%trisurf(tface(1:c2,:),node(:,1),node(:,2),node(:,3))
%trisurf(iface(1:ci,:),node(:,1),node(:,2),node(:,3))

% Find the triangular faces for exterior elements
extTet = tetElem(~isIntTet, :);
extIdx2cube = idx2cube(~isIntTet);

T = auxstructure3(extTet);
neighbor = T.neighbor;
clear T;
tmp = (1:size(extTet,1))';
for i = 1:4
    face = extTet(:, localFace(i,:));
    % find the triangle faces of polyhedron
    % 1. face and its neighbor associated to different cubes, and
    % 2. face is not on the boundary of all cut elements (which are squares)
    isPolyTriFace = ((neighbor(:, i) == tmp) | (extIdx2cube ~= extIdx2cube(neighbor(:,i)))) &...
        (sum(abs(vSign(face)), 2) > 0);
    c2 = ct + sum(isPolyTriFace);
    tface((ct+1):c2,:) = face(isPolyTriFace,:);
    tface2elem((ct+1):c2,:) = extIdx2cube(isPolyTriFace);
    % index of exterior polyhedron is append to the end of elem
    ct = c2;
end
toc

tface((ct+1):end,:) = [];    % remove empty meomory
tface2elem((ct+1):end) = [];
face2elemLoc = [ones(c2old,1);2*ones(c2-c2old,1)];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% generate edge structure of elements
% reorder the DoFs: put the DoFs of the edges of non-interface elements (sans those of interface elements)
% at the begining, then cancate the new edges including interface edges and
% non-interface edges

IntNodeID = [unique(reshape(mesh.t(mesh.tLoc<0,:),[],1));...
    (size(mesh.p,1)+1:size(mesh.p,1) + size(mesh.eIntP,1))'];
AllNodeID = zeros(size(node,1),1);
AllNodeID(IntNodeID) = 1:length(IntNodeID);
tfaceNew = AllNodeID(tface);
[face2EDoF,Newedge,face2EDoFSign] = dof3edge(tfaceNew);
% IMPORTANT: the Newedge contains those inside elements (4 intersection points)
% the edge orientation of dof3edge is from 1->2, 2->3, 3->1 of nodes of tfacenew
% face2EDoFSign assigns a sign such that orientation is from small to large
Newedge = IntNodeID(Newedge); % transfer the node index of edges to the global ones
% the new index of non-interface edges of interface elements (starting from 1)
NewedgeNintID = find(abs(sum(vSign(Newedge),2))==2);

OldedgeID = unique(reshape(mesh.t_e(mesh.tLoc<0,:),[],1));
Oldedge = mesh.e(OldedgeID,:);
OldEtmp = abs(sum(vSign(Oldedge),2))==2;
OldedgeNintID = OldedgeID(OldEtmp);
% the old index of non-interface edges of interface elements
OldedgeNint = Oldedge(OldEtmp,:);
[OldedgeNintSort,OldedgeNintSortID] = sortrows(OldedgeNint);
% ~ should be exactly the same as Newedge(NewedgeNintID)
% therefore OldedgeNintSortID should be the new index of all the
% non-interface edges of interface elements
BasicEdge = ones(size(mesh.e,1),1); BasicEdge(OldedgeID) = false;
BasicEdge = logical(BasicEdge);
% basically the non-interface edges sans edges of interface elements
BasicEdgeNum = sum(BasicEdge);
TotalOldEdge = zeros(size(mesh.e,1),1);
TotalOldEdge(BasicEdge) = 1:BasicEdgeNum;
TotalOldEdge(OldedgeNintID(OldedgeNintSortID)) = BasicEdgeNum + NewedgeNintID;
% map OldedgeNintSortID to global ones and put them at the location of
% the original non-interface edges
% NOTE: at the location of interface edges, TotalOldEdge still contains zero

% generate new global edge DoF structure
NumEdge = size(mesh.e,1) - size(Oldedge,1) + size(Newedge,1);
% NumEdge should equal size(mesh.e,1) + sum(mesh.eLoc<0) + 2*sum(mesh.fLoc<0)
gdof = zeros(NumEdge,2);
gdof(1:BasicEdgeNum,:) = mesh.e(BasicEdge,:);
gdof(BasicEdgeNum+1:end,:) = Newedge;
NintElem = mesh.tLoc>0;
g2ldofNint = TotalOldEdge(mesh.t_e(NintElem,:));
face2EDoF = face2EDoF + BasicEdgeNum;
% each row contains the DoFs of each element

% generate orientation structure for each edge
t_e_orit = zeros(size(g2ldofNint));
e_ind = [1,2; 1,3; 1,4; 2,3; 2,4; 3,4];
for i = 1:6
    d = mesh.p(mesh.t(NintElem,e_ind(i,2)),:) - mesh.p(mesh.t(NintElem,e_ind(i,1)),:);
    %d_crect = mesh.p(mesh.e(mesh.t_e(NintElem,i),2),:) - mesh.p(mesh.e(mesh.t_e(NintElem,i),1),:);
    d_crect = node(gdof(g2ldofNint(:,i),2),:) - node(gdof(g2ldofNint(:,i),1),:);
    t_e_orit(:,i) = sign(sum(d.*d_crect,2));
end

meshI.tface = tface;
meshI.tface2elem = tface2elem;
meshI.iface = iface;
meshI.iface2elem = iface2elem;
meshI.face2elemLoc = face2elemLoc;
meshI.PolyVolume = PolyVolume;
% meshI.vSign = vSign;
meshI.node = node;
meshI.tetElem = tetElem;
meshI.tetElemLoc = tetElemLoc;
meshI.idx2cube = idx2cube;
meshI.tetVolume = volume;
meshI.face2EDoF = face2EDoF;
meshI.face2EDoFSign = face2EDoFSign;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 2.use face, face2elem and face2EDoF data structure to compute ife functions

face2elem = meshI.tface2elem;
tface = meshI.tface;
N = size(node, 1); NEdof = NumEdge;
NF = size(tface,1); NC = size(mesh.t, 1);

NP = max(face2elem);
isCutPoly = false(NP, 1);
isCutPoly(face2elem) = true;
% this is the same as:
% isCutPoly = (mesh.tLoc<0);
% isCutPoly = isCutPoly(1:max(face2elem));
cutPolyIdx = zeros(NP, 1);
isExtPoly = false(NP,1);
isExtPoly(NC+1:NP) = true;

NP = sum(isCutPoly); % this new NP is max(-mesh.tLoc(mesh.tLoc<0));
cutPolyIdx(isCutPoly) = 1:NP;
% this is the same as:
% cutPolyIdx = zeros(size(mesh.tLoc,1), 1);
% cutPolyIdx(mesh.tLoc<0) = -mesh.tLoc(mesh.tLoc<0);
% cutPolyIdx = cutPolyIdxNew(1:max(face2elem));
face2elem = cutPolyIdx(face2elem);
% the old face2elem is the global index of interface elements
% the new one is only the index of interface elements
isExtPoly = isExtPoly(isCutPoly);
%%%clear isCutPoly cutPolyIdx

% prepare new data structure to compute projections
poly2node = sparse(face2elem(:)*ones(1,3), tface(:), 1, NP,N);
poly2node = (poly2node > 0);
NV = poly2node*ones(N, 1);

poly2edge = sparse(face2elem(:)*ones(1,3), face2EDoF(:), 1, NP,NEdof);
poly2edge = (poly2edge > 0);
NE = poly2edge*ones(NEdof, 1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% generate local IFE basis functions which are not associated with any DoFs
% IFEbasis(:,i,:,:) for minus subdomain (i=1) or plus domain (i=2)
iface = meshI.iface;
iface2elem = meshI.iface2elem;
[iface2elemReduce, uniID] = unique(iface2elem);
% length(uniID) should be NP
ifaceReduce = iface(uniID,:);
[IntFnormal,IntFarea,unitNormal,unittgt1,unittgt2] = facenormaltgt(node,ifaceReduce);
[Fnormal,Farea,FunitNormal,Funittgt1,Funittgt2] = facenormaltgt(node,tface);
% IFEbasis contains the associated IFE function on each face
% IFEbasisA is for curl term and IFEbasisB is for u term
IFEbasisA = zeros(size(face2elem,1),3,3);
% for two pieces of an ife function, only the normal vector is different
% on the minus subdomain (1) ife = n; on the plus subdomain (2) ife = n*bm/bp
IFEbasisA(:,1,:) = unitNormal(face2elem,:);
IFEbasisA(:,2,:) = unittgt1(face2elem,:);
IFEbasisA(:,3,:) = unittgt2(face2elem,:);
IFEbasisB = IFEbasisA;
piece1tmp = (face2elemLoc==1);
IFEbasisA(~piece1tmp,2,:) = IFEbasisA(~piece1tmp,2,:)*am/ap;
IFEbasisA(~piece1tmp,3,:) = IFEbasisA(~piece1tmp,3,:)*am/ap;
IFEbasisB(~piece1tmp,1,:) = IFEbasisB(~piece1tmp,1,:)*bm/bp;
% Xmf the centroid of a triangular face on the interface of each interface
% element but assigned to each face
% Xf the centroid of each triangular face
Xmf = (node(ifaceReduce(face2elem,1),:) + node(ifaceReduce(face2elem,2),:) +...
    node(ifaceReduce(face2elem,3),:))/3; % the Xm point on the approximate interface plane
Xf = (node(tface(:,1),:) + node(tface(:,2),:) +...
    node(tface(:,3),:))/3;

%% generate boundary integral for computing projections
% for the curl term, need to compute surface curl (rot) on each face
BIFEA =  zeros(length(face2elem),3);
BIFEB =  zeros(length(face2elem),3,3);
% BIFEA contains  (alpha*(grad vi).(xf -xm))for i=1,2,3 (vi is the test function)
BIFEA(:,1) = sum(squeeze(IFEbasisA(:,1,:)).*(Xf - Xmf),2);
BIFEA(:,2) = sum(squeeze(IFEbasisA(:,2,:)).*(Xf - Xmf),2);
BIFEA(:,3) = sum(squeeze(IFEbasisA(:,3,:)).*(Xf - Xmf),2);
% no need multiply area for this case as computing curl generates |F|^(-1)
BIFEA(piece1tmp,:) = am*BIFEA(piece1tmp,:);
BIFEA(~piece1tmp,:) = ap*BIFEA(~piece1tmp,:);
% BIFEB contains  wh=(beta*(curl vi)times(xf -xm))times n for i=1,2,3
% position to match the structure of mytimes
BIFEB(:,:,1) = -cross(cross(squeeze(IFEbasisB(:,1,:)),(Xf - Xmf)),FunitNormal)/2;
BIFEB(:,:,2) = -cross(cross(squeeze(IFEbasisB(:,2,:)),(Xf - Xmf)),FunitNormal)/2;
BIFEB(:,:,3) = -cross(cross(squeeze(IFEbasisB(:,3,:)),(Xf - Xmf)),FunitNormal)/2;
% devided by 2 is because curl(c times x) = 2c
BIFEB(piece1tmp,:) = bm*BIFEB(piece1tmp,:);
BIFEB(~piece1tmp,:) = bp*BIFEB(~piece1tmp,:);
% project the 3D vector onto each face
FNT = zeros(length(face2elem),3,3);
FNT(:,1,:) = Funittgt1;
FNT(:,2,:) = Funittgt2;
FNT(:,3,:) = FunitNormal;
BIFEB = mytimes(FNT,BIFEB);
BIFEB = BIFEB(:,1:2,:);
%%%%%%%%%%%%%%%%
% mutiply the inverse the matrix on the left hand side of each interface
% element for computing projection
% this is a blocal diagonal matrix
% for BIFEA (curl):
VdiagTmp1 = (am*PolyVolume(:,1) + ap*PolyVolume(:,2)).^(-1);
VdiagTmp2 = (am*PolyVolume(:,1) + am^2/ap*PolyVolume(:,2)).^(-1);
VdiagTmp3 = (am*PolyVolume(:,1) + am^2/ap*PolyVolume(:,2)).^(-1);
Vdiag1 = VdiagTmp1(face2elem);
Vdiag2 = VdiagTmp2(face2elem);
Vdiag3 = VdiagTmp3(face2elem);
Mdiag = zeros(size(Vdiag1,1),3,3);
Mdiag(:,1,1) = Vdiag1; Mdiag(:,2,2) = Vdiag2; Mdiag(:,3,3) = Vdiag3;
BIFEA = mytimes(Mdiag,BIFEA);
%%%%%%%%%%%%%%%%
% for BIFEB (u):
VdiagTmp1 = (bm*PolyVolume(:,1) + bm^2/bp*PolyVolume(:,2)).^(-1);
VdiagTmp2 = (bm*PolyVolume(:,1) + bp*PolyVolume(:,2)).^(-1);
VdiagTmp3 = (bm*PolyVolume(:,1) + bp*PolyVolume(:,2)).^(-1);
Vdiag1 = VdiagTmp1(face2elem);
Vdiag2 = VdiagTmp2(face2elem);
Vdiag3 = VdiagTmp3(face2elem);
Mdiag = zeros(size(Vdiag1,1),3,3);
Mdiag(:,1,1) = Vdiag1; Mdiag(:,2,2) = Vdiag2; Mdiag(:,3,3) = Vdiag3;
%%%%%%%%%%%%%%%%
% compute [-(ym-y1),(xm-x1)]
VecXm2Pt = (Xf - node(tface(:,1),:))/2;
VecXm2Pt = mytimes(FNT,VecXm2Pt);
VecXm2Pt = VecXm2Pt(:,[2,1]); VecXm2Pt(:,1) = -VecXm2Pt(:,1);
% compute [-(ym-y1),(xm-x1)].wh
BIFEB1 = zeros(size(VecXm2Pt));
for i = 1:3
    BIFEB1(:,i) = sum(VecXm2Pt.*squeeze(BIFEB(:,:,i)),2);
end
BIFEB1 = mytimes(Mdiag,BIFEB1);
% compute inv([t1;t2]) where t1 and t2 are for the 1st and 3rd edges
MatT = zeros(length(face2elem),3,2);
MatT(:,:,1) = node(tface(:,2),:) - node(tface(:,1),:);
MatT(:,:,2) = node(tface(:,1),:) - node(tface(:,3),:);
MatT = mytimes(FNT,MatT);
MatT = MatT(:,1:2,:); MatTInv = zeros(size(MatT));
MatTInv(:,1,1) = MatT(:,2,2)./(-2*Farea); % generate inverse matrix
MatTInv(:,2,2) = MatT(:,1,1)./(-2*Farea);
MatTInv(:,1,2) = -MatT(:,1,2)./(-2*Farea);
MatTInv(:,2,1) = -MatT(:,2,1)./(-2*Farea);
clear MatT
BIFEB2 = mytimes(MatTInv,BIFEB,[2,2]);  % T^(-1)w_h
BIFEB2 = mytimes(Mdiag,permute(BIFEB2,[1,3,2]));
BIFEB2 = permute(BIFEB2,[1,3,2]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute projections to IFE spaces and form the matrices
face2EDoF = meshI.face2EDoF;
face2EDoFSign = meshI.face2EDoFSign;
IFNT = zeros(length(ifaceReduce),3,3);
IFNT(:,:,1) = unitNormal;
IFNT(:,:,2) = unittgt1;
IFNT(:,:,3) = unittgt2;

nnz = sum(NE.^2);
iiP = zeros(nnz, 1);
jjP = zeros(nnz, 1);
ssKC = zeros(nnz, 1);
ssMU = zeros(nnz, 1);
indexP = 0;
iiF = zeros(nnz, 1);
jjF = zeros(nnz, 1);
ssSC = zeros(nnz, 1);
ssSU = zeros(nnz, 1);
indexF = 0;
une = unique(NE);
hF = h(face2elem);
b = zeros(NEdof, 1);

for kk = 1:length(une) % group polys according to their # of nodes

    tic
    %% generate IFE functions including the IFE basis functions on each interface
    %% elements which are not associated with any DoFs, and projections of gradients
    %% and the IFE functions with matching face average
    ne = une(kk);
    % some data related to element
    isCurrentFace = (NE(face2elem) == ne);
    CurrentFace = tface(isCurrentFace,:);
    CurrentFaceE = face2EDoF(isCurrentFace,:);
    CurrentFaceS = face2EDoFSign(isCurrentFace,:);
    CNF = size(CurrentFaceE,1);
    Currentfaceloc = face2elemLoc(isCurrentFace);
    Currentface2elem = face2elem(isCurrentFace);
    CurrentFnormal = FunitNormal(isCurrentFace,:);
    FareaCurrent = Farea(isCurrentFace);
    % some data related to face
    isCurrentElem = false(NP, 1);
    isCurrentElem(face2elem(isCurrentFace)) = true;
    CurrentPolyVolume = PolyVolume(isCurrentElem,:);
    IFNTCurrent = IFNT(isCurrentElem,:,:);
    % Current elem to node matrix
    currentPoly2edge = poly2edge(isCurrentElem, :);
    CNP = size(currentPoly2edge, 1);
    currentPolyLocalIdx = zeros(NP, 1);
    currentPolyLocalIdx(isCurrentElem) = 1:CNP;

    % currentElem(i,:) contains the edge index of the (current) i-th element
    [I, ~] = find(currentPoly2edge');
    currentElem = reshape(I, ne, [])';
    % localIdx(i,j) contains the (current) i-th element having the
    % node(dof=1,2,3,4) on the j-th location (j is the global node index)
    localIdx = sparse(repmat((1:CNP)', 1, ne), currentElem, ones(CNP, 1)*(1:ne), CNP, NEdof);
    clear currentPoly2edge

    %% Deal with current triangle face cases
    %% (1) for projection of curl u
    subs1 = currentPolyLocalIdx(face2elem(isCurrentFace));
    subs2 = [ones(CNF, 1); 2*ones(CNF, 1); 3*ones(CNF, 1)];
    val = BIFEA(isCurrentFace,:);

    % compute the projection of gradients
    GPA1 = zeros(CNP,3,ne);
    % coefficient of n t1 and t2 for the subelement on the subdomain 1
    for m = 1:3
        subs3 = full(localIdx((CurrentFaceE(:, m) - 1)*CNP + subs1));
        SignCurrentE = repmat(CurrentFaceS(:,m),3,1);
        % subs1: new order (range in 1:CNP) of the element index
        % subs2: three components in the vector
        % subs3: the local index (DoF) (w.r.t. element) of the m-th edge of this face
        GPA1 = GPA1 + accumarray([repmat(subs1,3,1), subs2, repmat(subs3, 3, 1)],...
            SignCurrentE.*val(:), [CNP, 3, ne]);
    end
    GPA2 = GPA1; GPA2(:,2,:) = am/ap*GPA2(:,2,:); GPA2(:,3,:) = am/ap*GPA2(:,3,:);
    GPA1 = mytimes(IFNTCurrent,GPA1);
    GPA2 = mytimes(IFNTCurrent,GPA2);
    % the updated GP contains three component values of the projection vector
    % generate IFE vectors on each face
    % clear currentPolyLocalIdx localIdx
    % clear subs1 subs2 subs3

    % generate the data for computing stabilization matrices
    CurrentFace2DoF = zeros(NP,ne);
    CurrentFace2DoF(isCurrentElem,:) = currentElem;
    CurrentFace2DoF = CurrentFace2DoF(Currentface2elem,:);
    % CurrentFace2DoF contains the DoFs of the element associated with each face

    % compute curl uh.n on each face (uh is an IFE function)
    GPA1tmp = zeros(NP,3,ne); GPA2tmp = zeros(NP,3,ne);
    GPA1tmp(isCurrentElem,:,:) = GPA1; GPA2tmp(isCurrentElem,:,:) = GPA2;
    GAface = zeros(CNF,3,ne);
    GAface(Currentfaceloc==1,:,:) = GPA1tmp(Currentface2elem(Currentfaceloc==1),:,:);
    GAface(Currentfaceloc==2,:,:) = GPA2tmp(Currentface2elem(Currentfaceloc==2),:,:);
    clear GPA1tmp GPA2tmp
    GAfaceP = zeros(size(GAface,1),ne);
    for n = 1:ne
        GAfaceP(:,n) =  sum(squeeze(GAface(:,:,n)).*CurrentFnormal,2);
    end

    % compute rot_F uh on each face according to DoFs
    TrueCurlTmp = CurrentFaceS./FareaCurrent;
    TrueCurl = zeros(CNF,ne);
    for i = 1:ne
        for j = 1:3
            IDij = (CurrentFaceE(:,j) == CurrentFace2DoF(:,i));
            TrueCurl(IDij,i) = TrueCurlTmp(IDij,j);
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% (2) for projection of u
    val = BIFEB1(isCurrentFace,:);
    GPB1 = zeros(CNP,3,ne);
    % coefficient of n t1 and t2 for the subelement on the subdomain 1
    for m = 1:3
        subs3 = full(localIdx((CurrentFaceE(:, m) - 1)*CNP + subs1));
        SignCurrentE = repmat(CurrentFaceS(:,m),3,1);
        % subs1: new order (range in 1:CNP) of the element index
        % subs2: three components in the vector
        % subs3: the local index (DoF) (w.r.t. element) of the m-th edge of this face
        GPB1 = GPB1 + accumarray([repmat(subs1,3,1), subs2, repmat(subs3, 3, 1)],...
            SignCurrentE.*val(:), [CNP, 3, ne]);
    end

    edgetmp = [1,3];
    for mm = 1:2
        val = squeeze(BIFEB2(isCurrentFace,mm,:));
        m = edgetmp(mm);
        subs3 = full(localIdx((CurrentFaceE(:, m) - 1)*CNP + subs1));
        SignCurrentE = repmat(CurrentFaceS(:,m),3,1);
        Areatmp = repmat(FareaCurrent,3,1);
        % subs1: new order (range in 1:CNP) of the element index
        % subs2: three components in the vector
        % subs3: the local index (DoF) (w.r.t. element) of the m-th edge of this face
        GPB1 = GPB1 + accumarray([repmat(subs1,3,1), subs2, repmat(subs3, 3, 1)],...
            SignCurrentE.*val(:).*Areatmp, [CNP, 3, ne]);
    end
    GPB2 = GPB1; GPB2(:,2,:) = am/ap*GPB2(:,2,:); GPB2(:,3,:) = am/ap*GPB2(:,3,:);
    GPB1 = mytimes(IFNTCurrent,GPB1);
    GPB2 = mytimes(IFNTCurrent,GPB2);
    % the updated GP contains three component values of the projection vector
    % generate IFE vectors on each face
    clear currentPolyLocalIdx localIdx
    clear subs1 subs2 subs3

    % compute uh^(\tau) (projection onto each face)
    GPB1tmp = zeros(NP,3,ne); GPB2tmp = zeros(NP,3,ne);
    GPB1tmp(isCurrentElem,:,:) = GPB1; GPB2tmp(isCurrentElem,:,:) = GPB2;
    GBface = zeros(CNF,3,ne);
    GBface(Currentfaceloc==1,:,:) = GPB1tmp(Currentface2elem(Currentfaceloc==1),:,:);
    GBface(Currentfaceloc==2,:,:) = GPB2tmp(Currentface2elem(Currentfaceloc==2),:,:);
    clear GPA1tmp GPA2tmp
    GBfaceP = zeros(size(GBface));
    for n = 1:ne
        GBfaceP(:,:,n) = GBface(:,:,n) - sum(GBface(:,:,n).*CurrentFnormal,2).*CurrentFnormal;
    end
    clear GBface
    GBfaceP = mytimes(FNT(isCurrentFace,:,:),GBfaceP);
    GBfaceP = GBfaceP(:,1:2,:);

    % compute u^(tau)(xm) uh on each face according to DoFs
    TrueUtTmp = zeros(CNF,2,ne);
    TrueUt = zeros(CNF,2,ne);
    MatTInvCurrent = MatTInv(isCurrentFace,:,:);
    for i = 1:ne
        TrueUt(:,:,i) = TrueUt(:,:,i) + TrueCurl(:,i).*VecXm2Pt(isCurrentFace,:);
        for j = 1:2
            jj = edgetmp(j);
            IDij = (CurrentFaceE(:,jj) == CurrentFace2DoF(:,i));
            TrueUtTmp(IDij,:,i) = MatTInvCurrent(IDij,j,:).*CurrentFaceS(IDij,jj);
        end
    end
    TrueUt = TrueUt + TrueUtTmp;
    clear TrueUtTmp

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% generate stiffness and mass matrices
    for n = 1:ne
        for m = 1:ne
            iiP(indexP+1:indexP + CNP) = currentElem(:, n);
            jjP(indexP+1:indexP + CNP) = currentElem(:, m);
            ssKC(indexP+1:indexP + CNP) = am*dot(GPA1(:,:,n), GPA1(:,:,m),2).*CurrentPolyVolume(:,1)+...
                ap*dot(GPA2(:,:,n), GPA2(:,:,m),2).*CurrentPolyVolume(:,2);
            ssMU(indexP+1:indexP + CNP) = bm*dot(GPB1(:,:,n), GPB1(:,:,m),2).*CurrentPolyVolume(:,1)+...
                bp*dot(GPB2(:,:,n), GPB2(:,:,m),2).*CurrentPolyVolume(:,2);
            indexP = indexP + CNP;

            iiF(indexF+1:indexF + CNF) = CurrentFace2DoF(:, n);
            jjF(indexF+1:indexF + CNF) = CurrentFace2DoF(:, m);
            ssSC(indexF+1:indexF + CNF) = dot((TrueCurl(:,m) - GAfaceP(:,m)),...
                (TrueCurl(:,n) - GAfaceP(:,n)),2).*...
                Farea(isCurrentFace).*hF(isCurrentFace);
            ssSU(indexF+1:indexF + CNF) = ( dot((TrueUt(:,1,m) - GBfaceP(:,1,m)),...
                (TrueUt(:,1,n) - GBfaceP(:,1,n)),2) + dot((TrueUt(:,2,m) - GBfaceP(:,2,m)),...
                (TrueUt(:,2,n) - GBfaceP(:,2,n)),2) ).*Farea(isCurrentFace).*hF(isCurrentFace);
            indexF = indexF + CNF;
        end
    end

    %% form the right-hand side vector
    idx2cubeNew = -mesh.tLoc(idx2cube);
    TetID = isCurrentElem(idx2cubeNew);
    currentvolume = volume(TetID);
    %%% DoFs of elements for tet
    currentTetDoF = zeros(size(isCurrentElem,1),ne);
    currentTetDoF(isCurrentElem,:) = currentElem;
    currentTetDoF = currentTetDoF(idx2cubeNew(TetID),:);
    %%% quadrature points
    X1 = node(tetElem(TetID,1),:);
    X2 = node(tetElem(TetID,2),:);
    X3 = node(tetElem(TetID,3),:);
    X4 = node(tetElem(TetID,4),:);
    ng = 1;
    [gx, gy, gz] = gaussPtetra(X1, X2, X3, X4, ng);
    gw = gaussWtetra(ng);
    Xm = (node(ifaceReduce(isCurrentElem,1),:) + node(ifaceReduce(isCurrentElem,2),:) +...
        node(ifaceReduce(isCurrentElem,3),:))/3;
    Xmtet = zeros(size(isCurrentElem,1),3);
    Xmtet(isCurrentElem,:) = Xm;
    Xmtet = Xmtet(idx2cubeNew(TetID),:);
    %%% projection of u
    Bas1 = zeros(size(isCurrentElem,1),3,ne); Bas2 = Bas1;
    Bas1(isCurrentElem,:,:) = GPB1;
    Bas2(isCurrentElem,:,:) = GPB2;
    Bas1 = Bas1(idx2cubeNew(TetID),:,:); Bas2 = Bas2(idx2cubeNew(TetID),:,:);
    Bas = zeros(sum(TetID),3,ne);
    piecetmp = tetElemLoc(TetID);
    Bas(piecetmp==1,:,:) = Bas1(piecetmp==1,:,:);
    Bas(piecetmp==2,:,:) = Bas2(piecetmp==2,:,:);

    ft1 = pde.f1(gx,gy,gz);
    ft2 = pde.f2(gx,gy,gz);
    ft3 = pde.f3(gx,gy,gz);
    Currentb = zeros(size(ft1));
    cnt = size(ft1,1);
    bii = ne*cnt;
    count = 0 ;
    for i = 1:ne
        Basi = squeeze(Bas(:,:,i));
        Currentb(:,i) = sum(gw*(ft1.*Basi(:,1)+ft2.*Basi(:,2)+ft3.*Basi(:,3)),2).*...
            currentvolume;
        bii(count+1:count+cnt) = currentTetDoF(:,i);
        count = count+cnt;
    end
    b = b + sparse(bii,1,reshape(Currentb,[],1),NEdof,1);

    toc

end

Kcurl = sparse(iiP, jjP, ssKC, NEdof, NEdof);
Scurl = sparse(iiF, jjF, ssSC, NEdof, NEdof);
MU = sparse(iiP, jjP, ssMU, NEdof, NEdof);
SU = sparse(iiF, jjF, ssSU, NEdof, NEdof);

MatI = Kcurl + Scurl + MU +SU;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% generate matrix on non-interface elements (for test)
feEvalBas1 = @EvalNed1Bas3D;
feEvalBas2 = @EvalNed1Bas3D;

dof1 = 6; dof2 = 6; nloc = dof1*dof2;
ntID = find(mesh.tLoc > 0); ntN = length(ntID);

%% 1. Matrix on noninterface elements
MatInd = 1;
AN = fem.area(ntID);
gxN = fem.gx(ntID,:); gyN = fem.gy(ntID,:); gzN = fem.gz(ntID,:); gw = fem.gw;
XN = zeros(nloc*ntN, 1);

coefN = feval(pde.A,gxN,gyN,gzN);
Ibasx = cell(dof1,1); Ibasy = cell(dof1,1); Ibasz = cell(dof1,1);
Jbasx = cell(dof2,1); Jbasy = cell(dof2,1); Jbasz = cell(dof2,1);

for i = 1:dof1
    Ibasx{i} = feEvalBas1(fem.bas, ntID, gxN, gyN, gzN, i, MatInd, 1).*t_e_orit(:,i);
    Ibasy{i} = feEvalBas1(fem.bas, ntID, gxN, gyN, gzN, i, MatInd, 2).*t_e_orit(:,i);
    Ibasz{i} = feEvalBas1(fem.bas, ntID, gxN, gyN, gzN, i, MatInd, 3).*t_e_orit(:,i);
end
for j = 1:dof2
    Jbasx{j} = feEvalBas2(fem.bas, ntID, gxN, gyN, gzN, j, MatInd, 1).*t_e_orit(:,j);
    Jbasy{j} = feEvalBas2(fem.bas, ntID, gxN, gyN, gzN, j, MatInd, 2).*t_e_orit(:,j);
    Jbasz{j} = feEvalBas2(fem.bas, ntID, gxN, gyN, gzN, j, MatInd, 3).*t_e_orit(:,j);
end

IN = reshape(repmat(g2ldofNint(:,1:6),6,1),nloc*ntN,1);
JN = repmat(reshape(g2ldofNint(:,1:6),dof2*ntN,1),6,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        XN(ind+1:ind+ntN) = AN.*(sum(((Ibasx{i}.*(coefN.*Jbasx{j})).*gw'),2) + ...
            sum(((Ibasy{i}.*(coefN.*Jbasy{j})).*gw'),2) + ...
            sum(((Ibasz{i}.*(coefN.*Jbasz{j})).*gw'),2));
        ind = ind + ntN;
    end
end
ID = find(XN~=0);
SN = sparse(IN(ID),JN(ID),XN(ID),NEdof,NEdof);

StiffCurl = SN + Kcurl + Scurl;

%%%%%%

MatInd = 0;
AN = fem.area(ntID);
gxN = fem.gx(ntID,:); gyN = fem.gy(ntID,:); gzN = fem.gz(ntID,:); gw = fem.gw;
XN = zeros(nloc*ntN, 1);

coefN = feval(pde.A,gxN,gyN,gzN);
Ibasx = cell(dof1,1); Ibasy = cell(dof1,1); Ibasz = cell(dof1,1);
Jbasx = cell(dof2,1); Jbasy = cell(dof2,1); Jbasz = cell(dof2,1);

for i = 1:dof1
    Ibasx{i} = feEvalBas1(fem.bas, ntID, gxN, gyN, gzN, i, MatInd, 1).*t_e_orit(:,i);
    Ibasy{i} = feEvalBas1(fem.bas, ntID, gxN, gyN, gzN, i, MatInd, 2).*t_e_orit(:,i);
    Ibasz{i} = feEvalBas1(fem.bas, ntID, gxN, gyN, gzN, i, MatInd, 3).*t_e_orit(:,i);
end
for j = 1:dof2
    Jbasx{j} = feEvalBas2(fem.bas, ntID, gxN, gyN, gzN, j, MatInd, 1).*t_e_orit(:,j);
    Jbasy{j} = feEvalBas2(fem.bas, ntID, gxN, gyN, gzN, j, MatInd, 2).*t_e_orit(:,j);
    Jbasz{j} = feEvalBas2(fem.bas, ntID, gxN, gyN, gzN, j, MatInd, 3).*t_e_orit(:,j);
end

IN = reshape(repmat(g2ldofNint(:,1:6),6,1),nloc*ntN,1);
JN = repmat(reshape(g2ldofNint(:,1:6),dof2*ntN,1),6,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        XN(ind+1:ind+ntN) = AN.*(sum(((Ibasx{i}.*(coefN.*Jbasx{j})).*gw'),2) + ...
            sum(((Ibasy{i}.*(coefN.*Jbasy{j})).*gw'),2) + ...
            sum(((Ibasz{i}.*(coefN.*Jbasz{j})).*gw'),2));
        ind = ind + ntN;
    end
end
ID = find(XN~=0);
MN = sparse(IN(ID),JN(ID),XN(ID),NEdof,NEdof);

MassU = MN + MU + SU;

%%%%%%%%%%

dof = 6;  nloc = dof;
X = zeros(nloc*ntN, 1);

fN1 = feval(pde.exactu1,gxN,gyN,gzN);
fN2 = feval(pde.exactu2,gxN,gyN,gzN);
fN3 = feval(pde.exactu3,gxN,gyN,gzN);
ind = 0;
I = reshape(g2ldofNint(:,1:6),nloc*ntN,1);
for i = 1:dof
    ibas1 = feEvalBas1(fem.bas, ntID, gxN, gyN, gzN, i, 0, 1);
    ibas2 = feEvalBas1(fem.bas, ntID, gxN, gyN, gzN, i, 0, 2);
    ibas3 = feEvalBas1(fem.bas, ntID, gxN, gyN, gzN, i, 0, 3);
    X(ind+1:ind+ntN) = AN.*sum((ibas1.*fN1+ibas2.*fN2+ibas3.*fN3).*gw',2).*fem.t_e_orit(ntID,i);
    ind = ind + ntN;
end
rhsN = sparse(I,1,X,NEdof,1);

rhs = rhsN + b;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[gew,gex,gey,gez] = gaussPedge(node(gdof(:,1),:),node(gdof(:,2),:),1);

tu1 = sum(feval(pde.exactu1,gex,gey,gez).*gew,2);
tu2 = sum(feval(pde.exactu2,gex,gey,gez).*gew,2);
tu3 = sum(feval(pde.exactu3,gex,gey,gez).*gew,2);

tgt = node(gdof(:,2),:) - node(gdof(:,1),:);
tgt = tgt./sum(tgt.^2,2).^(1/2);
tu = tu1.*tgt(:,1) + tu2.*tgt(:,2) + tu3.*tgt(:,3);

Atotal = StiffCurl + MassU;
%b = ones(size(MassU,1),1);
x0 = zeros(size(b));

bcind = fem.bcind;
[bc,mapper] = boundaryEdge3D(node,gdof,bcind);
bdidx = zeros(NEdof,1);
isBdEdge = true(NEdof,1);
isBdEdge(mapper) = false;
bdidx(isBdEdge) = 1;
Tbd = spdiags(bdidx,0,NEdof,NEdof);
T = spdiags(1-bdidx,0,NEdof,NEdof);
A = T*Atotal*T + Tbd;

ub = tu;
ub(mapper) = 0;
rhsB = Atotal*ub;
f = rhs - rhsB;
f(isBdEdge) = tu(isBdEdge);

option.outsolver = 'cg';
alpha = am*ones(size(gdof,1),1);
% alpha(femI.eLoc>0) = ap;
% alpha(femI.eLoc==0) = (am+ap)/2;
beta = bm*ones(size(gdof,1),1);
% beta(femI.eLoc>0) = bp;
% beta(femI.eLoc==0) = (bm+bp)/2;
option.alpha = alpha;
option.beta = beta;
option.solver = 'amg';
edge = gdof;
option.isBdEdge = isBdEdge;
option.smoother = 'BD';
D = diag(A); D = D(1:BasicEdgeNum);
M = sparse(1:BasicEdgeNum,1:BasicEdgeNum,D,NEdof,NEdof);
M(BasicEdgeNum+1:end,BasicEdgeNum+1:end) = M(BasicEdgeNum+1:end,BasicEdgeNum+1:end) +...
    A(BasicEdgeNum+1:end,BasicEdgeNum+1:end);
option.M = M;
%option.M = A(BasicEdgeNum+1:end,BasicEdgeNum+1:end);
option.blkId = BasicEdgeNum;
[x,info] = amgMaxwell(A,f,node,edge,option);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% generate exact solution

% [gew,gex,gey,gez] = gaussPedge(node(gdof(:,1),:),node(gdof(:,2),:),1);
%
%
% tu1 = sum(feval(pde.exactu1,gex,gey,gez).*gew,2);
% tu2 = sum(feval(pde.exactu2,gex,gey,gez).*gew,2);
% tu3 = sum(feval(pde.exactu3,gex,gey,gez).*gew,2);
%
% tgt = node(gdof(:,2),:) - node(gdof(:,1),:);
% tgt = tgt./sum(tgt.^2,2).^(1/2);
% tu = tu1.*tgt(:,1) + tu2.*tgt(:,2) + tu3.*tgt(:,3);
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %% test
% DC = full(StiffCurl*tu);
% DU = full(SU*tu);

% for i = 1:CNP
%     vv = sum(tu(currentElem(1,:)).*(squeeze(GPA1(1,:,:))'),1);
%     if sum(abs(vv)) > 10^(-8)
%         stp
%     end
% end


% Usage: [xopt, fmin, retcode] = nlopt_minimize(algorithm, f, f_data, lb, ub,
%                                               xinit, stop)
%
% Minimizes a nonlinear multivariable function f(x, f_data{:}), where
% x is a row vector, returning the optimal x found (xopt) along with
% the minimum function value (fmin = f(xopt)) and a return code (retcode).
% A variety of local and global optimization algorithms can be used,
% as specified by the algorithm parameter described below.  lb and ub
% are row vectors giving the upper and lower bounds on x, xinit is
% a row vector giving the initial guess for x, and stop is a struct
% containing termination conditions (see below).
%
% This function is a front-end for the external routine nlopt_minimize
% in the free NLopt nonlinear-optimization library, which is a wrapper
% around a number of free/open-source optimization subroutines.  More
% details can be found on the NLopt web page (ab-initio.mit.edu/nlopt)
% and also under 'man nlopt_minimize' on Unix.
%
% f should be a handle (@) to a function of the form:
%
%    [val, gradient] = f(x, ...)
%
% where x is a row vector, val is the function value f(x), and gradient
% is a row vector giving the gradient of the function with respect to x.
% The gradient is only used for gradient-based optimization algorithms;
% some of the algorithms (below) are derivative-free and only require
% f to return val (its value).  f can take additional arguments (...)
% which are passed via the argument f_data: f_data is a cell array
% of the additional arguments to pass to f.  (Recall that cell arrays
% are specified by curly brackets { ... }.  For example, pass f_data={}
% for functions that require no additional arguments.)
%
% stop describes the termination criteria, and is a struct with a
% number of optional fields:
%     stop.ftol_rel = fractional tolerance on function value
%     stop.ftol_abs = absolute tolerance on function value
%     stop.xtol_rel = fractional tolerance on x
%     stop.xtol_abs = row vector of absolute tolerances on x components
%     stop.fmin_max = stop when f < fmin_max is found
%     stop.maxeval = maximum number of function evaluations
%     stop.maxtime = maximum run time in seconds
%     stop.verbose = > 0 indicates verbose output
% Minimization stops when any one of these conditions is met; any
% condition that is omitted from stop will be ignored.  WARNING:
% not all algorithms interpret the stopping criteria in exactly the
% same way, and in any case ftol/xtol specify only a crude estimate
% for the accuracy of the minimum function value/x.
%
% The algorithm should be one of the following constants (name and
% interpretation are the same as for the C function).  Names with
% _G*_ are global optimization, and names with _L*_ are local
% optimization.  Names with _*N_ are derivative-free, while names
% with _*D_ are gradient-based algorithms.  Algorithms:
%
% NLOPT_GD_MLSL_LDS, NLOPT_GD_MLSL, NLOPT_GD_STOGO, NLOPT_GD_STOGO_RAND,
% NLOPT_GN_CRS2_LM, NLOPT_GN_DIRECT_L, NLOPT_GN_DIRECT_L_NOSCAL,
% NLOPT_GN_DIRECT_L_RAND, NLOPT_GN_DIRECT_L_RAND_NOSCAL, NLOPT_GN_DIRECT,
% NLOPT_GN_DIRECT_NOSCAL, NLOPT_GN_ISRES, NLOPT_GN_MLSL_LDS, NLOPT_GN_MLSL,
% NLOPT_GN_ORIG_DIRECT_L, NLOPT_GN_ORIG_DIRECT, NLOPT_LD_AUGLAG_EQ,
% NLOPT_LD_AUGLAG, NLOPT_LD_LBFGS, NLOPT_LD_LBFGS_NOCEDAL, NLOPT_LD_MMA,
% NLOPT_LD_TNEWTON, NLOPT_LD_TNEWTON_PRECOND,
% NLOPT_LD_TNEWTON_PRECOND_RESTART, NLOPT_LD_TNEWTON_RESTART,
% NLOPT_LD_VAR1, NLOPT_LD_VAR2, NLOPT_LN_AUGLAG_EQ, NLOPT_LN_AUGLAG,
% NLOPT_LN_BOBYQA, NLOPT_LN_COBYLA, NLOPT_LN_NELDERMEAD,
% NLOPT_LN_NEWUOA_BOUND, NLOPT_LN_NEWUOA, NLOPT_LN_PRAXIS, NLOPT_LN_SBPLX
%
% For more information on individual algorithms, see their individual
% help pages (e.g. "help NLOPT_LN_SBPLX").
function [xopt, fmin, retcode] = nlopt_minimize(algorithm, f, f_data, lb, ub, xinit, stop)

  [xopt, fmin, retcode] = nlopt_minimize_constrained(algorithm, f, f_data, {}, {}, lb, ub, xinit, stop);


function [spectrum,ntaper,freqoi] = ft_specest_mtmfft(dat, time, varargin)

% FT_SPECEST_MTMFFT computes a fast Fourier transform using multitapering with
% multiple tapers from the DPSS sequence or using a variety of single tapers.
%
% Use as
%   [spectrum,ntaper,freqoi] = ft_specest_mtmfft(dat,time...)
% where
%   dat        = matrix of chan*sample
%   time       = vector, containing time in seconds for each sample
%   spectrum   = matrix of taper*chan*freqoi of fourier coefficients
%   ntaper     = vector containing number of tapers per element of freqoi
%   freqoi     = vector of frequencies in spectrum
%
% Optional arguments should be specified in key-value pairs and can include
%   taper      = 'dpss', 'hanning' or many others, see WINDOW (default = 'dpss')
%   pad        = number, total length of data after zero padding (in seconds)
%   padtype    = string, indicating type of padding to be used (see ft_preproc_padding, default: zero)
%   freqoi     = vector, containing frequencies of interest
%   tapsmofrq  = the amount of spectral smoothing through multi-tapering. Note: 4 Hz smoothing means plus-minus 4 Hz, i.e. a 8 Hz smoothing box
%   dimord     = 'tap_chan_freq' (default) or 'chan_time_freqtap' for memory efficiency (only when use variable number slepian tapers)
%   polyorder  = number, the order of the polynomial to fitted to and removed from the data prior to the fourier transform (default = 0 -> remove DC-component)
%   taperopt   = additional taper options to be used in the WINDOW function, see WINDOW
%   verbose    = output progress to console (0 or 1, default 1)
%
% See also FT_FREQANALYSIS, FT_SPECEST_MTMCONVOL, FT_SPECEST_TFR, FT_SPECEST_HILBERT, FT_SPECEST_WAVELET

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

% these are for speeding up computation of tapers on subsequent calls
persistent previous_argin previous_tap

% get the optional input arguments
taper     = ft_getopt(varargin, 'taper'); if isempty(taper), ft_error('You must specify a taper'); end
pad       = ft_getopt(varargin, 'pad');
padtype   = ft_getopt(varargin, 'padtype', 'zero');
freqoi    = ft_getopt(varargin, 'freqoi', 'all');
tapsmofrq = ft_getopt(varargin, 'tapsmofrq');
dimord    = ft_getopt(varargin, 'dimord', 'tap_chan_freq');
fbopt     = ft_getopt(varargin, 'feedback');
verbose   = ft_getopt(varargin, 'verbose', true);
polyorder = ft_getopt(varargin, 'polyorder', 0);
tapopt    = ft_getopt(varargin, 'taperopt');

if isempty(fbopt)
  fbopt.i = 1;
  fbopt.n = 1;
end

% throw errors for required input
if isempty(tapsmofrq) && (strcmp(taper, 'dpss') || strcmp(taper, 'sine'))
  ft_error('you need to specify tapsmofrq when using dpss or sine tapers')
end

% this does not work on integer data
dat = cast(dat, 'double');

% Set n's
[nchan,ndatsample] = size(dat);

% This does not work on integer data
if ~isa(dat, 'double') && ~isa(dat, 'single')
  dat = cast(dat, 'double');
end

% Remove polynomial fit from the data -> default is demeaning
if polyorder >= 0
  dat = ft_preproc_polyremoval(dat, polyorder, 1, ndatsample);
end

% Determine fsample and set total time-length of data
fsample = 1./mean(diff(time));
dattime = ndatsample / fsample; % total time in seconds of input data

% Zero padding
if round(pad * fsample) < ndatsample
  ft_error('the padding that you specified is shorter than the data');
end
if isempty(pad) % if no padding is specified padding is equal to current data length
  pad = dattime;
end
postpad    = ceil((pad - dattime) * fsample);
endnsample = round(pad * fsample);  % total number of samples of padded data
endtime    = pad;                   % total time in seconds of padded data

% Set freqboi and freqoi
freqoiinput = freqoi;
if isnumeric(freqoi) % if input is a vector
  freqboi   = round(freqoi ./ (fsample ./ endnsample)) + 1; % is equivalent to: round(freqoi .* endtime) + 1;
  freqboi   = unique(freqboi);
  freqoi    = (freqboi-1) ./ endtime; % boi - 1 because 0 Hz is included in fourier output
elseif strcmp(freqoi,'all') % if input was 'all'
  freqboilim = round([0 fsample/2] ./ (fsample ./ endnsample)) + 1;
  freqboi    = freqboilim(1):1:freqboilim(2);
  freqoi     = (freqboi-1) ./ endtime;
end
nfreqboi = length(freqboi);
nfreqoi  = length(freqoi);
if (strcmp(taper, 'dpss') || strcmp(taper, 'sine')) && numel(tapsmofrq)~=1 && (numel(tapsmofrq)~=nfreqoi)
  ft_error('tapsmofrq needs to contain a smoothing parameter for every frequency when requesting variable number of slepian tapers')
end

% throw a warning if input freqoi is different from output freqoi
if isnumeric(freqoiinput)
  % check whether padding is appropriate for the requested frequency resolution
  rayl = 1/endtime;
  if any(rem(freqoiinput,rayl)) % not always the case when they mismatch
    ft_warning('padding not sufficient for requested frequency resolution, for more information please see the FAQs on www.ru.nl/neuroimaging/fieldtrip');
  end
  if numel(freqoiinput) ~= numel(freqoi) % freqoi will not contain double frequency bins when requested
    ft_warning('output frequencies are different from input frequencies, multiples of the same bin were requested but not given');
  else
    if any(abs(freqoiinput-freqoi) >= eps*1e6)
      ft_warning('output frequencies are different from input frequencies');
    end
  end
end

% determine whether tapers need to be recomputed
current_argin = {time, postpad, taper, tapsmofrq, freqoi, tapopt, dimord}; % reasoning: if time and postpad are equal, it's the same length trial, if the rest is equal then the requested output is equal
if isequal(current_argin, previous_argin)
  % don't recompute tapers
  tap = previous_tap;

else
  % recompute tapers
  switch taper

    case 'dpss'
      if numel(tapsmofrq)==1
        % create a sequence of DPSS tapers, ensure that the input arguments are double precision
        tap = double_dpss(ndatsample,ndatsample*(tapsmofrq./fsample))';
        % remove the last taper because the last slepian taper is always messy
        tap = tap(1:(end-1), :);

        % give error/warning about number of tapers
        if isempty(tap)
          ft_error('datalength to short for specified smoothing\ndatalength: %.3f s, smoothing: %.3f Hz, minimum smoothing: %.3f Hz',ndatsample/fsample,tapsmofrq,fsample/ndatsample);
        elseif size(tap,1) == 1
          ft_warning('using only one taper for specified smoothing');
        end
      elseif numel(tapsmofrq)>1
        tap = cell(1,nfreqoi);
        for ifreqoi = 1:nfreqoi
          % create a sequence of DPSS tapers, ensure that the input arguments are double precision
          currtap = double_dpss(ndatsample, ndatsample .* (tapsmofrq(ifreqoi) ./ fsample))';
          % remove the last taper because the last slepian taper is always messy
          currtap = currtap(1:(end-1), :);

          % give error/warning about number of tapers
          if isempty(currtap)
            ft_error('%.3f Hz: datalength to short for specified smoothing\ndatalength: %.3f s, smoothing: %.3f Hz, minimum smoothing: %.3f Hz',freqoi(ifreqoi), ndatsample/fsample,tapsmofrq(ifreqoi),fsample/ndatsample(ifreqoi));
          elseif size(currtap,1) == 1
            disp([num2str(freqoi(ifreqoi)) ' Hz: WARNING: using only one taper for specified smoothing'])
          end
          tap{ifreqoi} = currtap;
        end
      end

    case 'sine'
      if numel(tapsmofrq)==1
        % create a sequence of sine tapers,
        tap = sine_taper(ndatsample, ndatsample*(tapsmofrq./fsample))';
        % remove the last taper
        tap = tap(1:(end-1), :);

        % give error/warning about number of tapers
        if isempty(tap)
          ft_error('datalength to short for specified smoothing\ndatalength: %.3f s, smoothing: %.3f Hz, minimum smoothing: %.3f Hz',ndatsample/fsample,tapsmofrq,fsample/ndatsample);
        elseif size(tap,1) == 1
          ft_warning('using only one taper for specified smoothing');
        end
      elseif numel(tapsmofrq)>1
        tap = cell(1,nfreqoi);
        for ifreqoi = 1:nfreqoi
          % create a sequence of sine tapers
          currtap = sine_taper(ndatsample, ndatsample .* (tapsmofrq(ifreqoi) ./ fsample))';
          % remove the last taper because the last slepian taper is always messy
          currtap = currtap(1:(end-1), :);

          % give error/warning about number of tapers
          if isempty(currtap)
            ft_error('%.3f Hz: datalength to short for specified smoothing\ndatalength: %.3f s, smoothing: %.3f Hz, minimum smoothing: %.3f Hz',freqoi(ifreqoi), ndatsample/fsample,tapsmofrq(ifreqoi),fsample/ndatsample(ifreqoi));
          elseif size(currtap,1) == 1
            disp([num2str(freqoi(ifreqoi)) ' Hz: WARNING: using only one taper for specified smoothing'])
          end
          tap{ifreqoi} = currtap;
        end
      end

    case 'sine_old'
      % to provide compatibility with the tapers being scaled (which was default
      % behavior prior to 29apr2011) yet this gave different magnitude of power
      % when comparing with slepian multi tapers
      tap = sine_taper_scaled(ndatsample, ndatsample*(tapsmofrq./fsample))';
      tap = tap(1:(end-1), :); % remove the last taper

    case 'alpha'
      ft_error('not yet implemented');

    case 'hanning'
      tap = hanning(ndatsample)';
      tap = tap./norm(tap, 'fro');

    otherwise
      % create the taper and ensure that it is normalized
      if isempty(tapopt) % some windowing functions don't support nargin>1, and window.m doesn't check it
        tap = window(taper, ndatsample)';
      else
        tap = window(taper, ndatsample, tapopt)';
      end
      tap = tap ./ norm(tap,'fro');

  end % switch taper
end % isequal currargin

% set ntaper
if ~((strcmp(taper,'dpss') || strcmp(taper,'sine')) && numel(tapsmofrq)>1) % variable number of slepian tapers not requested
  ntaper = repmat(size(tap,1),nfreqoi,1);
else % variable number of slepian tapers requested
  ntaper = cellfun(@size,tap,repmat({1},[1 nfreqoi]));
end

% determine phase-shift so that for all frequencies angle(t=0) = 0
timedelay = time(1);
if timedelay ~= 0
  angletransform = complex(zeros(1,nfreqoi));
  for ifreqoi = 1:nfreqoi
    missedsamples = round(timedelay * fsample);
    % determine angle of freqoi if oscillation started at 0
    % the angle of wavelet(cos,sin) = 0 at the first point of a cycle, with sine being in upgoing flank, which is the same convention as in mtmconvol
    anglein = (missedsamples) .* ((2.*pi./fsample) .* freqoi(ifreqoi));
    coswav  = cos(anglein);
    sinwav  = sin(anglein);
    angletransform(ifreqoi) = atan2(sinwav, coswav);
  end
  angletransform = repmat(angletransform,[nchan,1]);
end

% compute fft
if ~((strcmp(taper,'dpss') || strcmp(taper,'sine')) && numel(tapsmofrq)>1) % ariable number of slepian tapers not requested
  str = sprintf('nfft: %d samples, datalength: %d samples, %d tapers',endnsample,ndatsample,ntaper(1));
  [st, cws] = dbstack;
  if length(st)>1 && strcmp(st(2).name, 'ft_freqanalysis')
    % specest_mtmfft has been called by ft_freqanalysis, meaning that ft_progress has been initialised
    ft_progress(fbopt.i./fbopt.n, ['processing trial %d/%d ',str,'\n'], fbopt.i, fbopt.n);
  elseif verbose
    fprintf([str, '\n']);
  end
  spectrum = cell(ntaper(1),1);

  for itap = 1:ntaper(1)
    dum = fft(ft_preproc_padding(bsxfun(@times,dat,tap(itap,:)), padtype, 0, postpad),[], 2);
    dum = dum(:,freqboi);
    % phase-shift according to above angles
    if timedelay ~= 0
      dum = dum .* exp(-1i*angletransform);
    end
    dum = dum .* sqrt(2 ./ endnsample);
    spectrum{itap} = dum;
  end

  spectrum = reshape(vertcat(spectrum{:}),[nchan ntaper(1) nfreqboi]); % collecting in a cell-array and later reshaping provides significant speedups
  spectrum = permute(spectrum, [2 1 3]);

else % variable number of slepian tapers requested
  switch dimord

    case 'tap_chan_freq' % default
      % start fft'ing
      spectrum = complex(NaN([max(ntaper) nchan nfreqoi]));
      for ifreqoi = 1:nfreqoi
        str = sprintf('nfft: %d samples, datalength: %d samples, frequency %d (%.2f Hz), %d tapers',endnsample,ndatsample,ifreqoi,freqoi(ifreqoi),ntaper(ifreqoi));
        [st, cws] = dbstack;
        if length(st)>1 && strcmp(st(2).name, 'ft_freqanalysis') && verbose
          % specest_mtmconvol has been called by ft_freqanalysis, meaning that ft_progress has been initialised
          ft_progress(fbopt.i./fbopt.n, ['processing trial %d, ',str,'\n'], fbopt.i);
        elseif verbose
          fprintf([str, '\n']);
        end
        for itap = 1:ntaper(ifreqoi)

          dum = fft(ft_preproc_padding(bsxfun(@times,dat,tap{ifreqoi}(itap,:)), padtype, 0, postpad), [], 2);

          dum = dum(:,freqboi(ifreqoi));
          % phase-shift according to above angles
          if timedelay ~= 0
            dum = dum .* exp(-1i*angletransform(:,ifreqoi));
          end
          dum = dum .* sqrt(2 ./ endnsample);
          spectrum(itap,:,ifreqoi) = dum;
        end
      end % for nfreqoi

    case 'chan_freqtap' % memory efficient representation
      % create tapfreqind
      freqtapind = cell(1,nfreqoi);
      tempntaper = [0; cumsum(ntaper(:))];
      for ifreqoi = 1:nfreqoi
        freqtapind{ifreqoi} = tempntaper(ifreqoi)+1:tempntaper(ifreqoi+1);
      end

      % start fft'ing
      spectrum = complex(zeros([nchan sum(ntaper)]));
      for ifreqoi = 1:nfreqoi
        str = sprintf('nfft: %d samples, datalength: %d samples, frequency %d (%.2f Hz), %d tapers',endnsample,ndatsample,ifreqoi,freqoi(ifreqoi),ntaper(ifreqoi));
        [st, cws] = dbstack;
        if length(st)>1 && strcmp(st(2).name, 'ft_freqanalysis') && verbose
          % specest_mtmconvol has been called by ft_freqanalysis, meaning that ft_progress has been initialised
          ft_progress(fbopt.i./fbopt.n, ['processing trial %d, ',str,'\n'], fbopt.i);
        elseif verbose
          fprintf([str, '\n']);
        end
        for itap = 1:ntaper(ifreqoi)

          dum = fft(ft_preproc_padding(bsxfun(@times,dat,tap{ifreqoi}(itap,:)), padtype, 0, postpad), [], 2);

          dum = dum(:,freqboi(ifreqoi));
          % phase-shift according to above angles
          if timedelay ~= 0
            dum = dum .* exp(-1i*angletransform(:,ifreqoi));
          end
          dum = dum .* sqrt(2 ./ endnsample);
          spectrum(:,freqtapind{ifreqoi}(itap)) = dum;
        end
      end % for nfreqoi
  end % switch dimord
end

% remember the current input arguments, so that they can be
% reused on a subsequent call in case the same input argument is given
previous_argin = current_argin;
previous_tap   = tap;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION ensure that the first two input arguments are of double
% precision this prevents an instability (bug) in the computation of the
% tapers for MATLAB 6.5 and 7.0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [tap] = double_dpss(a, b, varargin)
tap = dpss(double(a), double(b), varargin{:});


function gr = lme_mass_RgGradient1(X,Zcols,SIGMA,W,invH,L,phi,re,ni,G,GDa,GDb)
% gr = lme_mass_RgGradient1(X,Zcols,W,invH,L,phi,re,ni,G,GDa,GDb)
%
% Gradient vector of the restricted log-likelihood for a whole region. This
% is less computationally efficient than lme_mass_RgGradient but more
% numerically stable.
%
% Input
% X: Ordered design Matrix (according to time for each subject).
% Zcols: Vector with the indices of the colums of X that will be considered
% as random effects.
% W: Inverses of the estimated temporal covariance matrices for each
% subject stacked in W.
% invH: Asymptotic covariance matrix of the fixed effects.
% L: Cholesky factor of the covariance matrix of the random effects (D).
% phi: Within-subject standard deviation of the errors.
% re: Residuals;
% ni: Vector whose entries are the number of repeated measures for each
% subject in the study (ordered according to X).
% G: Spatial covariance matrix.
% GDa: Derivative of the spatial covariance matrix for the first spatial
% parameter.
% GDb: Derivative of the spatial covariance matrix for the second spatial
% parameter (empty for spatial models with a single parameter).
%
% Output
% gr: Gradient vector.
%
% $Revision: 1.2 $  $Date: 2015/01/06 17:14:55 $
% Original Author: Jorge Luis Bernal Rusiel
% CVS Revision Info:
%    $Author: mreuter $
%    $Date: 2015/01/06 17:14:55 $
%    $Revision: 1.2 $
%
m = length(ni);
q = length(Zcols);
Z = X(:,Zcols);
nth = q*(q+1)/2+1;
nv = size(G,1);
%Log-likelihoood derivative for L
gr = zeros(nth+2,1);
jk = 0;
for k=1:q
    for j=1:k
        jk = jk + 1;
        posi = 1; posir = 1;
        a1 = 0; a2 = 0; M1 = 0;
        for i=1:m
            posf = posi+ni(i)-1;
            posfr = posir+ni(i)*nv-1;
            Zi = Z(posi:posf,:);
            Wi = W(posi:posf,1:ni(i));
            SIGMAi = SIGMA(posi:posf,1:ni(i));
            Xi = X(posi:posf,:);
            rei = re(posir:posfr);
            Mjki = Zi(:,k)*L(j,:)*Zi';
            Mjki = Mjki + Mjki';
            Maux = Mjki*Wi;
            a1 = a1 - trace(Maux);
            a2 = a2 + (rei'/kron(G,SIGMAi))*(kron(eye(nv),Mjki*Wi)*rei);
            M1 = M1 + Xi'*Wi*Maux*Xi;
            posi = posf+1;
            posir = posfr+1;
        end;
        a3 = -trace(invH*M1);
        gr(jk) = (nv*a1+a2-nv*a3)/2;
    end;
end;
%Log-likelihoood derivative for phi
posi = 1; posir = 1;
a1 = 0; a2 =0; M1 = 0;
for i=1:m
    posf = posi+ni(i)-1;
    posfr = posir+ni(i)*nv-1;
    Wi = W(posi:posf,1:ni(i));
    SIGMAi = SIGMA(posi:posf,1:ni(i));
    Xi = X(posi:posf,:);
    rei = re(posir:posfr);
    a1 = a1 - trace(Wi);
    Maux = Wi*Wi;
    a2 = a2 + (rei'/kron(G,SIGMAi))*(kron(eye(nv),Wi)*rei);
    M1 = M1 + Xi'*Maux*Xi;
    posi = posf+1;
    posir = posfr+1;
end;
a3 = -trace(invH*M1);
gr(nth) = phi*(nv*a1+a2-nv*a3);
%Log-likelihoood derivative for a
posi = 1; posir = 1;
a2 =0;
for i=1:m
    posf = posi+ni(i)-1;
    posfr = posir+ni(i)*nv-1;
    SIGMAi = SIGMA(posi:posf,1:ni(i));
    rei = re(posir:posfr);
    a2 = a2 + (rei'/kron(G,eye(ni(i))))*(kron(GDa,eye(ni(i))))*(kron(G,SIGMAi)\rei);
    posi = posf+1;
    posir = posfr+1;
end;
aux = -trace(G\GDa);
a1 = sum(ni)*aux;
a3 = size(X,2)*aux;
gr(nth+1) = (a1+a2-a3)/2;
if ~isempty(GDb)
    %Log-likelihoood derivative for b
    posi = 1; posir = 1;
    a2 =0;
    for i=1:m
        posf = posi+ni(i)-1;
        posfr = posir+ni(i)*nv-1;
        SIGMAi = SIGMA(posi:posf,1:ni(i));
        rei = re(posir:posfr);
        a2 = a2 + (rei'/kron(G,eye(ni(i))))*(kron(GDb,eye(ni(i))))*(kron(G,SIGMAi)\rei);
        posi = posf+1;
        posir = posfr+1;
    end;
    aux = -trace(G\GDb);
    a1 = sum(ni)*aux;
    a3 = size(X,2)*aux;
    gr(nth+2) = (a1+a2-a3)/2;
else
    gr = gr(1:nth+1);
end;


function [gmu, gsigmavar, factors] = gpPosteriorGradMeanCovar(model, X);

% GPPOSTERIORGRADMEANCOVAR Gadient of the mean and variances of the posterior at points given by X.
% FORMAT
% DESC computes the gradient of the mean and covariances of the
% posterior distribution of a Gaussian process with respect to the
% input locations.
% ARG model : the model for which gradients are to be computed.
% ARG X : the input locations where gradients are to be computed.
% RETURN gmu : the gradient of the posterior mean with respect to
% the input locations.
% RETURN gCovar : the gradients of the posterior covariance with
% respect to the input locations. By raw, we mean that the gradient
% has not yet been multiplied by any output scale in that direction
% (as is done for gpPosteriorGradMeanCovar). The gradients are
% stored in a cell array of dimension MODEL.q x MODEL.d.
%
% DESC computes the gradient of the mean and covariances of the
% posterior distribution of a Gaussian process with respect to the
% input locations. Returns a compact representation for the
% covariances which separates the factors associated with the
% different dimensions from the covariance gradients.
% ARG model : the model for which gradients are to be computed.
% ARG X : the input locations where gradients are to be computed.
% RETURN gmu : the gradient of the posterior mean with respect to
% the input locations.
% RETURN grCovar : the 'raw' gradient of the posterior covariance with
% respect to the input locations. By raw, we mean that the gradient
% has not yet been multiplied by any output scale in that direction
% (as is done for gpPosteriorGradMeanCovar). The gradients are
% stored in a cell array of length MODEL.q.
% RETURN factors : the factors for multiplying the 'raw' gradients
% of the covariances by.
%
% SEEALSO : gpCreate, gpPosteriorMeanVar
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006, 2009

% GP

if ~isfield(model, 'alpha')
  model = gpComputeAlpha(model);
end

switch model.approx
 case 'ftc'
  gX = kernGradX(model.kern, X, model.X);
  kX_star = kernCompute(model.kern, X, model.X)';
 case {'dtc', 'dtcvar', 'fitc', 'pitc'}
  gX = kernGradX(model.kern, X, model.X_u);
  kX_star = kernCompute(model.kern, X, model.X_u)';
 otherwise
  error('Unrecognised approximation type');

end
K = kernGradX(model.kern, X);

if ~model.isMissingData
  for i = 1:model.q
    switch model.approx
     case 'ftc'
      KinvgK = model.invK_uu*squeeze(gX(:, i, :));
     case {'dtc', 'dtcvar', 'fitc', 'pitc'}
      KinvgK = (model.invK_uu - (1/model.beta)*model.Ainv)*squeeze(gX(:, i, :));
     otherwise
      error('Unrecognised approximation type');
    end
    kXTKinvgK = kX_star'*KinvgK;
    gCovar{i} = squeeze(K(:, i, :))-kXTKinvgK - diag(diag(kXTKinvgK));
    gmu{i} = squeeze(gX(:, i, :))'*model.alpha.*repmat(model.scale, ...
                                                      size(X, 1), 1);
  end

  % Deal with scaling.
  if nargout < 3
    for i = 1:model.q
      for j = 1:model.d
        gsigmavar{i, j} = gCovar{i}*model.scale(j)*model.scale(j);
      end
    end
  else
    factors = model.scale.*model.scale;
    gsigmavar = gCovar;
  end
else
  error('Not yet implemented for models trained on missing data.');
end


  function fit = de_ftab_fit_exp3(xrs, mac, varargin)
%|function fit = de_ftab_fit_exp3(xrs, mac, [options])
%|
%| Fit to DE table fm(), suitable for subsequent interpolation / extrapolation.
%| Uses a special experimental exponential model:
% todo -log(sum_k p_k exp(-m_k . s))
% fit a 3-term exponential model that has the proper derivatives
% at 0, and at +/- infinity
%|
%| in
%|	xrs	strum		X-ray spectra; see xray_read_spectra.m
%|	mac	[Ne,L]		mass attenuation coefficients
%|
%| option
%|	'show'	1|0		plot?
%|
%| out
%|	fit	strum		strum object for fitted f_m(s_1,...,s_L)
%|
%|	methods:
%|	.fmfun(sll)		fm function evaluation, for stacked array sll
%|	.fgrad(sll)		fm gradient evaluation, for stacked array sll
%|	.show_sp(en, sp)	plot true spectrum vs fitted spectrum
%|	.show_fm(sl, fm)	mesh plot of fm and its fit
%|	.show_err(sl, fm)	mesh plot of fit error
%|	.mac_eff		effective mass atten coef based on fit
%|				(valid for 'exp' only)
%|
%| Copyright 2008-8-10, Jeff Fessler, University of Michigan

%if nargin == 1 && streq(xrs, 'test'), de_ftab_fit_exp3_test, return, end
if nargin < 2, ir_usage, end

arg.show = false;
arg = vararg_pair(arg, varargin);

LL = ncol(mac);
if LL ~= 1, fail 'only 1 component done', end

MM = ncol(xrs.Ide);

fit.MM = MM;
fit.type = 'exp3';

massbar = (xrs.Ide' * mac) ./ sum(xrs.Ide)'; % [M,1]

%if ~isempty(wt), warn 'weighting ignored', end
% if isempty(wt), wt = num2cell(ones(MM,1)); end

fit.kev = cell(1,MM);
fit.mac = cell(1,MM);
fit.coef = cell(1,MM);
for mm=1:MM
	good = xrs.sp(:,mm) > 0;
	sp_m = xrs.sp(good,mm); % nonzero spectrum
	sp_m = sp_m / sum(sp_m);
	[mac_m ii] = sort(mac(good));
	if any(mac_m(2:end)) == mac_m(1), error 'non unique min', end
	if any(mac_m(1:end-1)) == mac_m(end), error 'non unique max', end
	kev_m = xrs.en(good);
	kev_m = kev_m(ii);
	q1 = sp_m(1);
	qN = sp_m(end);
	b1 = mac_m(1);
	bN = mac_m(end);
	mac_mid = (massbar(mm) - q1 * b1 - qN * bN) / (1 - q1 - qN);
	fit.mac{mm} = [b1 mac_mid bN]';
	fit.coef{mm} = [q1 1-q1-qN qN]';

	kev_mid = interp1(mac_m, kev_m, mac_mid);
	fit.kev{mm} = [kev_m(1) kev_mid kev_m(end)]';
end

meth = {'fmfun', @de_ftab_fit_exp_eval, '(sll)'; ...
	'fgrad', @de_ftab_fit_exp_grad, '(sll)'; ...
	'show_err', @de_ftab_fit_show_err, '(sl, fm)'; ...
	'show_fm', @de_ftab_fit_show_fm, '(sl, fm)'; ...
	'show_sp', @de_ftab_fit_show_sp, '(en, sp)'; ...
	};
fit = strum(fit, meth);

if arg.show
	fit.show_fm(sl, fm);
end

end % de_ftab_fit_exp3()

%
% de_ftab_fit_exp()
% todo: more documentation
%
function [fit, ffun, fgrad] = de_ftab_fit_exp(sl, fm, MM, wt, mtype, mac, kev)

sll = ndgrid_jf('mat', sl{:});

if isempty(mac)
	if isempty(mtype), error 'mac or mtype required', end
	mac = xray_read_atten(mtype, kev);
end

LL = length(sl);

if isempty(wt), wt = num2cell(ones(MM,1)); end

Ab = 1;
for ll=1:LL
	sl = col(stackpick(sll, ll));
	Ab = Ab .* exp(-sl * mac(:,ll)'); % [#s*, #E]
end

fit.kev = cell(1,MM);
fit.mac = cell(1,MM);
fit.coef = cell(1,MM);
for mm=1:MM
	if LL == 1
		dat = fm(:,mm);
	elseif LL == 2
		dat = fm(:,:,mm);
	else
		fail 'not done'
	end
	dat = stackpick(fm,mm);
	y = exp(-dat);
	Wh = spdiag(sqrt(wt{mm}(:)), 'nowarn');
	x = wls_simplex(Ab, y(:), Wh); % coefficients for each energy

	ie = x > 1e-6; % find key energies
	fit.kev{mm} = kev(ie);
	fit.mac{mm} = mac(ie,:); % [#E,L]
	A = 1;
	for ll=1:LL
		sl = col(stackpick(sll,ll));
		A = A .* exp(-sl * fit.mac{mm}(:,ll)'); % [#s*, #E]
	end
	fit.coef{mm} = wls_simplex(A, y(:), Wh); % refit with key energies
end
ffun = @de_ftab_fit_exp_eval;
fgrad = @de_ftab_fit_exp_grad;

end % de_ftab_fit_exp()

%
% de_ftab_fit_exp_eval()
% evaluate
% in
%	sll	[(Nd),L]	stackup of s1,s2,...,s_L
% out
%	f	[(Nd),M]	stackup of f1,f2,...,f_M
%
function f = de_ftab_fit_exp_eval(fit, sll)
Nd = size(sll); LL = Nd(end); Nd = Nd(1:end-1);
sll = reshape(sll, [], LL); % [*Nd,L]
MM = fit.MM;
f = zeros(prod(Nd),MM);
for mm=1:MM
	A = 1;
	mac = fit.mac{mm};
	for ll=1:LL
		sl = sll(:,ll);
		A = A .* exp(-sl * mac(:,ll)'); % [*Nd,ne]
	end
	tmp = -log(A * fit.coef{mm}); % [*Nd,1]
	f(:,mm) = tmp;
end
f = reshape(f, [Nd MM]);

end % de_ftab_fit_exp_eval()

%
% de_ftab_fit_exp_grad()
% evaluate gradient of f for each of the given s vectors.
% in
%	sll	[(Nd),L]	stackup of s1,s2,...,s_L
% out
%	g	[(Nd),L,M]	stackup of gradients of f(s)
%
function g = de_ftab_fit_exp_grad(fit, sll)
Nd = size(sll); LL = Nd(end); Nd = Nd(1:end-1);
sll = reshape(sll, [], LL); % [*Nd,L]
MM = fit.MM;
g = zeros(prod(Nd), LL, MM);
for mm=1:fit.MM
	A = 1;
	mac = fit.mac{mm}; % [ne,L]
	alf = fit.coef{mm}; % [ne,1]
	for ll=1:LL
		sl = sll(:,ll);
		A = A .* exp(-sl * mac(:,ll)'); % [*Nd,ne]
	end
	vm = A * alf; % [*Nd,1]
	tmp = A * (mac .* repmat(alf, [1 LL])); % [*Nd,L]
	g(:,:,mm) = tmp ./ repmat(vm, [1 LL]);
end
g = reshape(g, [Nd LL MM]);

end % de_ftab_fit_exp_grad()

%
% de_ftab_fit_show_sp()
% compare true spectra to fitted spectra
%
function out = de_ftab_fit_show_sp(fit, en, sp)
if nargin < 3, fail 'de_ftab_fit_show_sp(fit, en, sp)', end

if ~streq(fit.type, 'exp3'), printm 'show_sp only done for exp3', return, end
if im
	clf, pl = (fit.MM+1)*100 + 10 + 1;
	subplot(pl)
	plot(en, sp * diag(1 ./ max(sp)))
	if isfield(fit, 'kev')
		for mm=1:fit.MM
			subplot(pl+mm)
			bar(fit.kev{mm}, fit.coef{mm})
			axis tight, axisx(minmax(en))
		end
	else
		warn 'kev unknown'
	end
end

if nargout, out = []; end

end % de_ftab_fit_show_sp()

%
% de_ftab_fit_show_fm()
% compare fit to sampled fm
%
function out = de_ftab_fit_show_fm(fit, sl, fm)
if nargin < 3, error 'de_ftab_fit_show_fm(fit, sl, fm)', end

sll = ndgrid_jf('mat', sl{:});

if fit.MM ~= 2, error 'only M=2 done', end

fh = fit.fmfun(sll);

switch length(sl) % LL
case 1
	s1 = sl{1};
	im clf
	plot(	s1, fm(:,1), 'c.', s1, fm(:,2), 'y.', ...
		s1, fh(:,1), 'c-', s1, fh(:,2), 'y-')
	xlabel 's1'
	ylabel 'fm(s1)'
	legend('true m=1', 'true m=2', 'fit m=1', 'fit m=2', 4)

case 2
	s1 = sl{1};
	s2 = sl{2};
	smax(1) = max(s1);
	smax(2) = max(s2);
	fmax = max(fm(:));

	ax = [0 smax(1) 0 smax(2) 0 fmax];
	cax = [0 fmax];

	im clf
	im pl 2 2

	show(1, s1, s2, fm(:,:,1), ax, cax, 'f_1(s)')
	% text(-60, 10, '[cm^2/g]')
	show(2, s1, s2, fm(:,:,2), ax, cax, 'f_2(s)')

	show(3, s1, s2, fh(:,:,1), ax, cax, 'f_1 approx')
	show(4, s1, s2, fh(:,:,2), ax, cax, 'f_2 approx')
otherwise
	fail 'not done'
end

if nargout, out = []; end

end % de_ftab_fit_show_fm()

%
% de_ftab_fit_show_err()
% show fit errors, and relate to HU
% f = mac s, mac(70kev,H2O) = 0.2 cm^2 / g = 1000 HU
% mh = mac * 1000 HU / (0.2 g/cm^2)
% so Df/Dmh = Df/Dmac * Dmac/Dmh = (50 g/cm^2) (0.2 cm^2/g) / 1000 HU = 1/100 HU
% so Dmh = 100 HU * Df for 50 cm of H2O.
%
function out = de_ftab_fit_show_err(fit, sl, fm)
if nargin < 3, error 'de_ftab_fit_show_err(fit, sl, fm)', end

sll = ndgrid_jf('mat', sl{:});

fh = fit.fmfun(sll);
err = fh - fm;
printm('worst model error = %g of %g', max(abs(err(:))), max(fm(:)))
printm('worst model error = %g HU over 50cm H2O', 100*max(abs(err(:))))
for mm=1:fit.MM
	ee = stackpick(err,mm);
	printm('worst error (mm=%d): %g', mm, max(abs(ee(:))))
end

if max(abs(err(:))) == 0, return, end
if fit.MM > 2, error 'only M=2 done', end
if 0
	e1 = err(:,:,1);
	e2 = err(:,:,2);
	disp([minmax(e1); minmax(e2)]')
	printm('worst error1 %g', max(col(abs(err(:,1,:)))))
	printm('worst error2 %g', max(col(abs(err(:,2,:)))))
end
%err = abs(err);

switch length(sl) % LL
case 1
	s1 = sl{1};
	plot(s1, err(:,1), 'c-', s1, err(:,2), 'y-')
	xlabel 's1'
	ylabel 'error'
	legend('m=1', 'm=2', 4)

case 2
	s1 = sl{1};
	s2 = sl{2};
	smax(1) = max(s1);
	smax(2) = max(s2);

	elim = minmax(err(:))';
	%elim = [-1 1] * 0.01; % +/- 1 HU
	ax = [0 smax(1) 0 smax(2) elim];
	im plc 1 2
	for mm=1:fit.MM
		show(mm, s1, s2, err(:,:,mm), ax, elim, sprintf('f_%d error', mm))
	end
otherwise
	fail 'not done'
end

if nargout, out = []; end

end % de_ftab_fit_show_err()

%
% show()
%
function show(pl, x, y, f, ax, cax, ti)
if ~im, return, end
im('subplot', pl)
if 1
	mesh(x,y,f')
	colormap hsv, caxis(cax), cbar
	axis(ax)
	xtick, ytick, ztick, zwhite
else
	im(x,y,f), cbar
	xtick, ytick
end
xlabel 's_1', ylabel 's_2', title(ti)

end % show()

%
% de_ftab_fit_exp3_test()
%
function de_ftab_fit_exp3_test

%stype = 'mono,70';
stype = 'ps1';
xrs = xray_read_spectra(stype);

if 1 % L=1 component
	sl{1} = linspace(0, 50, 26);
	mtype = {'water'};
	mas = xray_read_mac(mtype);
	mac = mas.mac(xrs.en);
	if im
		clf, semilogy(xrs.en, mac), legend(mtype{:})
	end
	sll = ndgrid_jf('mat', sl{:});
	fm = de_ftab_fm(sll, mac, xrs.Ide);
	fit = de_ftab_fit(sl, fm, 'type', 'exp', 'mtype', mtype);
	g = fit.fgrad(sll);
%	fit = de_ftab_fit(sl, fm, 'type', 'poly')

	if 1
		fit.show_sp(xrs.en, xrs.sp);
		prompt
		fit.show_fm(sl, fm);
		prompt
		fit.show_err(sl, fm);
		prompt
	end
end

if 1 % L=2
	sl{1} = linspace(0, 50, 26);
	sl{2} = linspace(0, 30, 31);
	mtype = {'water', 'bone'};
	mas = xray_read_mac(mtype);
	mac = mas.mac(xrs.en);
	if im
		clf, semilogy(xrs.en, mac), legend(mtype{:})
	end
	sll = ndgrid_jf('mat', sl{:});
	fm = de_ftab_fm(sll, mac, xrs.Ide);
	fit = de_ftab_fit(sl, fm, 'type', 'exp', 'mtype', mtype);
	g = fit.fgrad(sll);
	%fit = de_ftab_fit(sl, fm, 'type', 'poly')

	if 1
		fit.show_sp(xrs.en, xrs.sp);
		prompt
		fit.show_fm(sl, fm);
		prompt
		fit.show_err(sl, fm);
		prompt
	end
end

end % de_ftab_fit_exp3_test()


function [x,testdata]=alstpz_solve(A,y,tol,varargin)
%Solution of linear systems in TT-format via ALS(t+z) iteration
%!Note! This method is provided mostly for test purposes.
%!Note! You may prefer to use amen_solve2 instead.
%   [X,somedata]=ALSTPZ_SOLVE(A,Y,TOL,OPTIONS) Attempts to solve the linear
%   system A*X = Y with accuracy/residual TOL using the global (t+z)-enriched ALS.
%   Matrix A has to be given in the TT-format, right-hand side Y should be
%   given in the TT-format also. Options are provided in form
%   'PropertyName1',PropertyValue1,'PropertyName2',PropertyValue2 and so
%   on. The parameters are set to default (in brackets in the following)
%   The list of option names and default values are:
%       o x0 - initial approximation [random rank-2 tensor]
%       o nswp - maximal number of sweeps [10]
%       o max_full_size - maximal size of the local matrix to full solver [50]
%       o local_prec - local preconditioner: '' (no prec.), 'ljacobi',
%         'cjacobi', 'rjacobi' ['']
%       o local_iters - number of local gmres restarts [2]
%       o local_restart - dimension of local gmres [40]
%       o kickrank - compression rank of the residual Z, i.e. expansion
%         size [4]
%       o kicktype - how to approximate the residual: via the SVD rounding ('svd'),
%         or using one approximation ALS sweep ('als'). ['svd']
%       o ismex - shall we use the MEX lib solve3d_2 for local solution [true]
%       o resid_damp - solve local problems with accuracy tol/resid_damp.
%         Larger value may reduce a spurious noise from inexact local
%         solutions, but increase CPU time [2]
%       o symm - shall we consider the symmetrized system (A'A)x=(A'y). [false]
%
%       Example:
%           d=8; f=8;
%           mat=tt_qlaplace_dd(d*ones(1,f)); % Laplace in the QTT-format
%           rhs=tt_ones(2,d*f); % Right-hand side of all ones
%           sol = alstpz_solve(mat, rhs, 1e-5); % solve the Poisson eqn.
%
%********
%   References:
%   S. Dolgov, D. Savostyanov.
%   http://arxiv.org/abs/1301.6068
%   http://arxiv.org/abs/1304.1222
%
%   Please send feedback to: {sergey.v.dolgov,dmitry.savostyanov}@gmail.com
%
%********
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
% Inner parameters
max_full_size=50;

resid_damp = 2; % Truncation error to true residual treshold

nswp=10;
local_restart=40;
local_iters=2;

local_prec = '';

ismex = true;

% kicktype = 'als';
kicktype = 'svd';

kickrank = 4;
x=[];
symm = false;
verb = 3;

for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'nswp'
            nswp=varargin{i+1};
        case 'x0'
            x=varargin{i+1};
        case 'local_prec'
            local_prec=varargin{i+1};
        case 'local_restart'
            local_restart=varargin{i+1};
        case 'ismex'
            ismex=varargin{i+1};
        case 'local_iters'
            local_iters=varargin{i+1};
        case 'kickrank'
            kickrank=varargin{i+1};
        case 'kicktype'
            kicktype=varargin{i+1};
        case  'max_full_size'
            max_full_size=varargin{i+1};
        case 'resid_damp'
            resid_damp = varargin{i+1};
        case 'symm'
            symm=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};

        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

% Disable MEX if it does not exist
if (ismex)&&(exist('solve3d_2', 'file')<2)
    warning('MEX local solver is not found, disabled');
    ismex = false;
end;

% Extract sizes and TT blocks
d = y.d;
n = A.n;
if (isempty(x))
    x = tt_rand(n, A.d, 2);
end;

if (symm)
    % Symmetrize the system if necessary
    A2 = A'*A;
    Ay = A'*y;
end;

if (strcmp(kicktype, 'als'))&&(kickrank>0)
    z = tt_rand(n, d, kickrank);
    rz = z.r;
    phi = cell(d+1,1); phi{1}=1; phi{d+1}=1;
end;

% Test output data -- e.g. for convergence tracking
testdata = cell(3,1);
testdata{2} = cell(d, nswp);
testdata{1} = zeros(d, nswp);
testdata{3} = zeros(1, nswp);
t_corr_amen = tic;

% ALS(t+z) sweeps
for swp=1:nswp
    % Truncate the solution first
    x = round(x,tol);
    if (strcmp(kicktype, 'als'))&&(kickrank>0)
        % Update the residual approximation via one ALS sweep for |z-ZZ|->min
        % Exact residual
        if (symm)
            ZZ = Ay - A2*x;
        else
            ZZ = y - A*x;
        end;
        rZZ = ZZ.r;
        ZZ = core2cell(ZZ);
        z = core2cell(z); % Approximate one
        for i=1:d-1
            % Go backward and prepare projections
            cr = reshape(z{i}, rz(i)*n(i), rz(i+1));
            [cr,rv]=qr(cr,0);
            cr2 = reshape(z{i+1}, rz(i+1), n(i+1)*rz(i+2));
            cr2 = rv*cr2;
            rz(i+1) = size(cr, 2);
            z{i} = reshape(cr, rz(i), n(i), rz(i+1));
            z{i+1} = reshape(cr2, rz(i+1), n(i+1), rz(i+2));

            phi{i+1} = compute_next_Phi(phi{i}, z{i}, [], ZZ{i}, 'lr');
        end;
        for i=d:-1:1
            % Go forward and project exact ZZ
            cr = reshape(ZZ{i}, rZZ(i)*n(i), rZZ(i+1));
            cr = cr*phi{i+1};
            cr = reshape(cr, rZZ(i), n(i)*rz(i+1));
            cr = phi{i}*cr;
            if (i>1)
                [cr,rv]=qr(cr.', 0);
                cr2 = reshape(z{i-1}, rz(i-1)*n(i-1), rz(i));
                cr2 = cr2*rv.';
                rz(i) = size(cr, 2);
                z{i} = reshape(cr.', rz(i), n(i), rz(i+1));
                z{i-1} = reshape(cr2, rz(i-1), n(i-1), rz(i));

                phi{i} = compute_next_Phi(phi{i+1}, z{i}, [], ZZ{i}, 'rl');
            else
                z{i} = reshape(cr, rz(i), n(i), rz(i+1));
            end;
        end;
        z = cell2core(tt_tensor, z);
    else
        % SVD rounding of the exact residual up to kickrank
        if (kickrank>0)
            if (symm)
                z = round(Ay - A2*x, tol, kickrank);
            else
                z = round(y - A*x, tol, kickrank);
            end;
        end;
    end;

    % Augment the solution with Z basis
    if (kickrank>0)
        x = x+0*z;
    end;
    t_old = toc(t_corr_amen);
    % Update via the ALS iteration
    if (symm)
        [x,td_als] = als_solve(A2, Ay, tol,  'max_full_size', max_full_size, 'x0', x, 'verb', verb, 'local_prec', local_prec, 'local_iters', local_iters, 'local_restart', local_restart, 'resid_damp', resid_damp, 'ismex', ismex);
    else
        [x,td_als] = als_solve(A, y, tol,  'max_full_size', max_full_size, 'x0', x, 'verb', verb,  'local_prec', local_prec, 'local_iters', local_iters, 'local_restart', local_restart, 'resid_damp', resid_damp, 'ismex', ismex);
    end;
    testdata{1}(:, swp) = t_old+td_als{1}(:,1);
    testdata{2}(:, swp) = td_als{2};
    testdata{3}(swp) = max(td_als{3});
end;

end

function [x,td]=als_solve(A, y, tol, varargin)
% One sweep of the one-site DMRG solution scheme.
% Uses the same parameters as in the main function

local_prec_char = 0;
verb = 1;
ismex = true;

x=[];

for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'x0'
            x=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};
        case 'local_prec'
            local_prec=varargin{i+1};
        case 'local_restart'
            local_restart=varargin{i+1};
        case 'local_iters'
            local_iters=varargin{i+1};
        case 'ismex'
            ismex=varargin{i+1};
        case  'max_full_size'
            max_full_size=varargin{i+1};
        case 'resid_damp'
            resid_damp = varargin{i+1};

        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

if (strcmp(local_prec, 'cjacobi')); local_prec_char = 1;  end;
if (strcmp(local_prec, 'ljacobi')); local_prec_char = 2;  end;
if (strcmp(local_prec, 'rjacobi')); local_prec_char = 3;  end;

% Extract sizes and TT cores
d = y.d;
n = A.n;
if (isempty(x))
    x = tt_rand(n, A.d, 2);
end;

ry = y.r;
ra = A.r;
rx = x.r;

cry = core2cell(y);
crA = core2cell(A);
crx = core2cell(x);

% Interfaces
phia = cell(d+1,1); phia{1}=1; phia{d+1}=1;
phiy = cell(d+1,1); phiy{1}=1; phiy{d+1}=1;

% This is some convergence output for test purposes
td = cell(3,1);
td{1} = zeros(d, 1);
td{2} = cell(d, 1);
td{3} = zeros(d, 1);

t_amr_solve = tic;

% Orthogonalization
for i=d:-1:2
    cr = crx{i};
    cr = reshape(cr, rx(i), n(i)*rx(i+1));
    [cr, rv]=qr(cr.', 0);
    cr2 = crx{i-1};
    cr2 = reshape(cr2, rx(i-1)*n(i-1), rx(i));
    cr2 = cr2*(rv.');
    rx(i) = size(cr, 2);
    cr = reshape(cr.', rx(i), n(i), rx(i+1));
    crx{i-1} = reshape(cr2, rx(i-1), n(i-1), rx(i));
    crx{i} = cr;

    phia{i} = compute_next_Phi(phia{i+1}, cr, crA{i}, cr, 'rl');
    phiy{i} = compute_next_Phi(phiy{i+1}, cr, [], cry{i}, 'rl');
end;

max_res = 0;
max_dx = 0;

% DMRG sweep
for i=1:d
    % Extract elements - matrix
    Phi1 = phia{i}; Phi2 = phia{i+1};
    % Phi1: rx'1, rx1, ra1, or rx'1, ry1
    % Phi2: rx2, ra2, rx'2, or ry'2, rx2
    A1 = crA{i}; y1 = cry{i};
    % RHS - rewrite it in accordance with new index ordering
    rhs = phiy{i}; % rx'1, ry1
    y1 = reshape(y1, ry(i), n(i)*ry(i+1));
    rhs = rhs*y1;
    rhs = reshape(rhs, rx(i)*n(i), ry(i+1));
    rhs = rhs*phiy{i+1};
    rhs = reshape(rhs, rx(i)*n(i)*rx(i+1),1);
    norm_rhs = norm(rhs);
    % sol_prev
    sol_prev = reshape(crx{i}, rx(i)*n(i)*rx(i+1), 1);

    real_tol = (tol/sqrt(d))/resid_damp;

    if (rx(i)*n(i)*rx(i+1)<max_full_size) % Full solution
        %      |     |    |
        % B = Phi1 - A1 - Phi2
        %      |     |    |
        B = reshape(Phi1, rx(i)*rx(i), ra(i));
        B = B*reshape(A1, ra(i), n(i)*n(i)*ra(i+1));
        B = reshape(B, rx(i), rx(i), n(i), n(i)*ra(i+1));
        B = permute(B, [1, 3, 2, 4]);
        B = reshape(B, rx(i)*n(i)*rx(i)*n(i), ra(i+1));
        B = B*reshape(permute(Phi2, [2, 3, 1]), ra(i+1), rx(i+1)*rx(i+1));
        B = reshape(B, rx(i)*n(i), rx(i)*n(i), rx(i+1), rx(i+1));
        B = permute(B, [1, 3, 2, 4]);
        B = reshape(B, rx(i)*n(i)*rx(i+1), rx(i)*n(i)*rx(i+1));

        res_prev = norm(B*sol_prev - rhs)/norm_rhs;

        if (res_prev>real_tol)
            sol = B \ rhs;
            res_new = norm(B*sol-rhs)/norm_rhs;
        else
            sol = sol_prev;
            res_new = res_prev;
        end;

    else % Structured solution.

        res_prev = norm(bfun3(Phi1, A1, Phi2, sol_prev) - rhs)/norm_rhs;

        if (res_prev>real_tol)
            if (~ismex)
                sol = solve3d_2ml(Phi1, A1, Phi2, rhs, real_tol, sol_prev, local_prec_char, local_restart, local_iters);
            else % use MEX
                sol = solve3d_2(Phi1, A1, Phi2, rhs, real_tol, 1, sol_prev, local_prec_char, local_restart, local_iters, 0);
            end;

            res_new = norm(bfun3(Phi1, A1, Phi2, sol) - rhs)/norm_rhs;
        else
            sol = sol_prev;
            res_new = res_prev;
        end;

    end;

    if (res_prev/res_new<resid_damp)&&(res_new>real_tol)
        fprintf('--warn-- the residual damp was smaller than in the truncation\n');
    end;

    dx = norm(sol-sol_prev)/norm(sol);
    max_dx = max(max_dx, dx);
    max_res = max(max_res, res_prev);

    td{3}(i) = dx;

    % QR
    sol = reshape(sol, rx(i)*n(i), rx(i+1));
    [u,v]=qr(sol, 0);
    r = size(u,2);

    if (verb==2)
        fprintf('=als_solve=   block %d, dx: %3.3e, res: %3.3e, r: %d\n', i, dx, res_prev, r);
    end;

    if (i<d) % left-to-right, kickrank, etc
        cr2 = crx{i+1};
        cr2 = reshape(cr2, rx(i+1), n(i+1)*rx(i+2));
        v = v*cr2; % size r+radd, n2, r3

        u = reshape(u, rx(i), n(i), r);
        v = reshape(v, r, n(i+1), rx(i+2));

        % Recompute phi. Left ones, so permute them appropriately
        phia{i+1} = compute_next_Phi(phia{i}, u, crA{i}, u, 'lr');
        phiy{i+1} = compute_next_Phi(phiy{i}, u, [], cry{i}, 'lr');

        % Stuff back
        rx(i+1) = r;
        crx{i} = u;
        crx{i+1} = v;
    else
        % Just stuff back the last core
        sol = reshape(sol, rx(i), n(i), rx(i+1));
        crx{i} = sol;
    end;

    if (verb>2)
        td{1}(i) = toc(t_amr_solve);
        if (verb>3)||(i==d) % each microstep is returned only if really asked for
            % Otherwise the memory will blow up
            x = cell2core(x, crx); % for test
            td{2}{i} = x;
        end;
    end;

end;

if (verb>0)
     fprintf('=als_solve= max_dx: %3.3e, max_res: %3.3e, erank: %g\n', max_dx, max_res, sqrt(rx(1:d)'*(n.*rx(2:d+1))/sum(n)));
end;

x = cell2core(x, crx);

end

function [Phi] = compute_next_Phi(Phi_prev, x, A, y, direction)
% Performs the recurrent Phi (or Psi) matrix computation
% Phi = Phi_prev * (x'Ay).
% If direction is 'lr', computes Psi
% if direction is 'rl', computes Phi
% A can be empty, then only x'y is computed.

% Phi1: rx1, ry1, ra1, or rx1, ry1
% Phi2: ry2, ra2, rx2, or ry2, rx2

rx1 = size(x,1); n = size(x,2); rx2 = size(x,3);
ry1 = size(y,1); m = size(y,2); ry2 = size(y,3);
if (~isempty(A))
    ra1 = size(A,1); ra2 = size(A,4);
else
    ra1 = 1; ra2 = 1;
end;

if (strcmp(direction, 'lr'))
    %lr: Phi1
    x = reshape(x, rx1, n*rx2);
    Phi = reshape(Phi_prev, rx1, ry1*ra1);
    Phi = x'*Phi;
    if (~isempty(A))
        Phi = reshape(Phi, n*rx2*ry1, ra1);
        Phi = Phi.';
        Phi = reshape(Phi, ra1*n, rx2*ry1);
        A = reshape(A, ra1*n, m*ra2);
        Phi = A.'*Phi;
        Phi = reshape(Phi, m, ra2*rx2*ry1);
    else
        Phi = reshape(Phi, n, rx2*ry1);
    end;
    Phi = Phi.';
    Phi = reshape(Phi, ra2*rx2, ry1*m);

    y = reshape(y, ry1*m, ry2);
    Phi = Phi*y;
    if (~isempty(A))
        Phi = reshape(Phi, ra2, rx2*ry2);
        Phi = Phi.';
    end;
    Phi = reshape(Phi, rx2, ry2, ra2);
else
    %rl: Phi2
    y = reshape(y, ry1*m, ry2);
    Phi = reshape(Phi_prev, ry2, ra2*rx2);
    Phi = y*Phi;
    if (~isempty(A))
        Phi = reshape(Phi, ry1, m*ra2*rx2);
        Phi = Phi.';
        Phi = reshape(Phi, m*ra2, rx2*ry1);
        A = reshape(A, ra1*n, m*ra2);
        Phi = A*Phi;
        Phi = reshape(Phi, ra1*n*rx2, ry1);
        Phi = Phi.';
    end;

    Phi = reshape(Phi, ry1*ra1, n*rx2);
    x = reshape(x, rx1, n*rx2);
    Phi = Phi*x';
    if (~isempty(A))
        Phi = reshape(Phi, ry1, ra1, rx1);
    else
        Phi = reshape(Phi, ry1, rx1);
    end;
end;

end

% new
function [y]=bfun3(Phi1, A, Phi2, x)
% Phi1: ry1, rx1, ra1
ry1 = size(Phi1,1);
rx1 = size(Phi1,2);
ra1 = size(Phi1,3);
% Phi2: rx2, ra2, ry2
ry2 = size(Phi2,3);
rx2 = size(Phi2,1);
ra2 = size(Phi2,2);

n = size(A,2);
m = size(A,3);

y = reshape(x, rx1*m, rx2);
Phi2 = reshape(Phi2, rx2, ra2*ry2);
y = y*Phi2;
y = reshape(y, rx1, m*ra2*ry2);
y = y.';
y = reshape(y, m*ra2, ry2*rx1);
A = reshape(A, ra1*n, m*ra2);
y = A*y;
y = reshape(y, ra1*n*ry2, rx1);
y = y.';
y = reshape(y, rx1*ra1, n*ry2);
Phi1 = reshape(Phi1, ry1, rx1*ra1);
y = Phi1*y;
y = reshape(y, ry1*n*ry2, 1);
end


function a = lt( x, y )

%   Disciplined convex programming information for LT (<):
%      The left-hand side of a less-than constraint must be convex. The
%      right-hand side must be concave. Of course, real constant and
%      affine expressions are both convex and concave and can be used on
%      either side as well.
%
%   Disciplined geometric programming information for LT (<):
%      The left-hand side of a less-than constraint must be log-convex,
%      including positive constants, monomials, posynomials, generalized
%      posynomials, and products thereof. The right-hand side must be
%      log-concave---including positive constants, monomials,
%      reciprocals of log-convex expressions, and products thereof.
%
%   Note that CVX does not distinguish between strict less-than (<) and
%   less-than-or-equal (<=) constraints; they are treated identically.
%   Feasible interior-point solvers tend to return points which satisfy
%   strict inequality, but not all solvers do.

evalin( 'caller', 'cvx_verify' );
b = cvx_pushcnstr( x, y, '<' );
if nargout, a = b; end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [W,BC,DV,Q,r,B] = voxelize(V,F,side,varargin)
  % VOXELIZE Given a mesh compute a voxelization of that mesh on a regular grid
  % fit to the bounding box.
  %
  % W = voxelize(V,F,side)
  % [W,BC,DV,Q] = voxelize(V,F,side,'ParameterName',ParameterValue, ...)
  %
  % Inputs:
  %   V  #V by 3 list of vertex positions
  %   F  #F by 3 list of triangle indices into V, expects counterclockwise
  %     orientation to produce outward pointing normal
  %   side  either the number of cells in the x direction or a triple
  %     containing the number of cells in each dimension
  %   Optional:
  %     'Pad'  followed by number of padding to add to each side. 1 would mean
  %       add one extra cell on the top, bottom, left, right, front, and back
  %       of bounding box lattice {0}.
  %     'Boundary'  followed by wether to consider any cell intersecting the
  %       mesh as inside {true}.
  %     'Interior'  followed by whether to consider any cell full inside the
  %        mesh as inside {true}
  %     'Closed'  followed by whether to assume mesh is closed when determining
  %       interior (avoid winding number computation, use flood filling)
  %     'Centers'  followed by prod(side) centers (e.g., already produced by
  %       call to previous call to voxel_grid.m)
  % Outputs:
  %   W  side(1) by side(2) by side(3) matrix with W(i,j,k) ~= if location
  %     cell centered at BC(i,j,k) overlaps with the volume of (V,F)
  %   BC  prod(side) by dim matrix of cell barycenters
  %   DV  side(1)+1 by side(2)+1 by side(3)+1 matrix of cell corner locations
  %   Q  #Q by 4 list of quads indexing DV
  %   r  dim-long list of voxel widths in each direction
  %
  % Known issues: the ouput surface mesh will contain non-manifold edges and
  % vertices at voxels that meet at such edges or vertices.
  %
  % Example:
  %   [W,BC,DV,Q] = voxelize(V,F,50);
  %   % Get triangles from quads
  %   DF = [Q(:,[1 2 3]);Q(:,[1 3 4])];
  %   % Remove unreferenced corners
  %   [SV,IM] = remove_unreferenced(DV,DF);
  %   % re-index
  %   SF = IM(DF);
  %   SQ = IM(Q);
  %
  %   [W,BC] = voxelize(V,F,50,'Pad',1);
  %   % Use matlab's isosurface to extract surface as triangle mesh
  %   BC = reshape(BC,[size(W) 3]);
  %   surf = isosurface(BC(:,:,:,1),BC(:,:,:,2),BC(:,:,:,3),W,0.5);
  %   DV = bsxfun(@plus,bsxfun(@times,bsxfun(@rdivide,surf.vertices-1, ...
  %     [size(W,2) size(W,1) size(W,3)]-1),max(BC)-min(BC)),min(BC));
  %   DF = surf.faces;
  %
  %
  % See also: bwboundaries, isosurface

  with_boundary = true;
  with_interior = true;
  pad_count = 0;
  closed = [];
  BC = [];
  % default values
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Boundary','Interior','Pad','Centers','Closed'}, ...
    {'with_boundary','with_interior','pad_count','BC','closed'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end
  if isempty(closed)
    closed = isempty(boundary_faces(F));
  end

  assert(any(size(side) == 1),'side should be a vector');
  % Make sure we have a row vector
  side = side(:)';

  dim = size(V,2);

  if isempty(BC)
    [BC,side,r] = voxel_grid(V,side,'Pad',pad_count);
  else
    assert(prod(side) == size(BC,1),'side dims must match BC');
    r = (max(BC)-min(BC))./(side-1);
  end
  NV = min(BC);
  XV = max(BC);

  switch dim
  case 2
    W  = zeros([side(2) side(1)]);
    [DV,~,QT,QL] = voxel_surface(W,'Centers',BC);

    if with_interior && ~closed
      WDV = reshape(winding_number(V,F,DV),[side(2) side(1)]+1);
      WDV = abs(WDV) >= 0.5;
      W = ( ...
        WDV(1:end-1,1:end-1) | ...
        WDV(1:end-1,  2:end) | ...
        WDV(  2:end,1:end-1) | ...
        WDV(  2:end,  2:end));
    end
    Q = [QT;QL];
    if with_boundary
      for ei = 1:size(F,1)

        sqrD = segment_segment_squared_distance( ...
          V(F(ei,1),:), V(F(ei,2),:), ...
          DV(Q(:,1),:), DV(Q(:,2),:));
        IF = mod(find(sqrD<eps)-1,size(Q,1))+1;
        iQT = IF(IF<=size(QT,1));
        iQL = IF(IF>size(QT,1))-size(QT,1);

        [II,JJ] = ind2sub([side(2) side(1)]+1,max(QT(iQT,:),[],2));
        W(sub2ind(side([2 1]),II-1,JJ-1)) = 1;
        W(sub2ind(side([2 1]),II  ,JJ-1)) = 1;

        [II,JJ] = ind2sub([side(2) side(1)]+1,max(QL(iQL,:),[],2));
        W(sub2ind(side([2 1]),II-1,JJ-1)) = 1;
        W(sub2ind(side([2 1]),II-1,JJ  )) = 1;

        %clf;
        %hold on;
        %surf(reshape(BC(:,1),size(W)),reshape(BC(:,2),size(W)),reshape(BC(:,1)*0,size(W)),'CData',W*1,fphong,'EdgeColor','none');
        %plot_edges(V,F(ei,:),'y','LineWidth',8);
        %plot_edges(V,F,'g','LineWidth',4);
        %text(BC(:,1),BC(:,2),num2str((1:size(BC,1))'),'BackgroundCOlor',[0.8 0.8 0.8]);
        %text(DV(:,1),DV(:,2),num2str((1:size(DV,1))'),'BackgroundCOlor',[0.8 0.1 0.1]);
        %plot_edges(DV,QT,'--r','LineWidth',1);
        %plot_edges(DV,QL,'--b','LineWidth',1);
        %plot_edges(DV,QT(iQT,:),'r','LineWidth',3);
        %plot_edges(DV,QL(iQL,:),'b','LineWidth',3);
        %hold off;
        %axis equal;
        %colormap([zeros(20,3);1 1 1]);
        %drawnow;

      end
    end
    if with_interior && closed
      W = imfill(W,'holes');
    end
    % Pad winding number with 0s and take differences in all 6 directions
    Wp = padarray(W,[1 1],0);
    Dy = diff(Wp,1,1);
    Dx = diff(Wp,1,2);
    Q = [ ...
      QL(Dx(2:end-1,1:end,2:end-1)>0.5,:); ...
      QT(Dy(1:end,2:end-1,2:end-1)>0.5,:); ...
      fliplr(QL(Dx(2:end-1,1:end,2:end-1)<-0.5,:)); ...
      fliplr(QT(Dy(1:end,2:end-1,2:end-1)<-0.5,:)); ...
      ];
  case 3
    W = zeros([side(2) side(1) side(3)]);
    [DV,~,QT,QF,QL] = voxel_surface(W,'Centers',BC);

    if with_interior && ~closed
      % Winding number is a heavy handed way of determining inside/outside for a
      % simple closed polyhedron.  If the boundary has already been detected then
      % this should be floodfilling instead.

      %W = winding_number(V,F,BC);
      WDV = reshape(winding_number(V,F,DV,'Fast',true),[side(2) side(1) side(3)]+1);
      WDV = abs(WDV) >= 0.5;
      W = ( ...
        WDV(1:end-1,1:end-1,1:end-1) | ...
        WDV(1:end-1,1:end-1,2:end) | ...
        WDV(1:end-1,  2:end,1:end-1) | ...
        WDV(1:end-1,  2:end,2:end) | ...
        WDV(  2:end,1:end-1,1:end-1) | ...
        WDV(  2:end,1:end-1,2:end) | ...
        WDV(  2:end,  2:end,1:end-1) | ...
        WDV(  2:end,  2:end,2:end));
    end

    Q = [QT;QF;QL];

    if with_boundary
      % TODO: Could at least ignore already-inside cells.
      DF = [Q(:,[3 2 1]);Q(:,[4 3 1])];
      IF = intersect_other(V,F,DV,DF);
      JF = IF(:,1);
      IF = mod(IF(:,2)-1,size(Q,1))+1;
      B = zeros(side(2),side(1),side(3));
      % Force winding number to mark voxels intersecting boundary as inside
      % (This should just be replaced with rasterizing the surface)
      iQ = cell(3,1);
      iQ{1} = IF(IF<=size(QT,1));
      iQ{3} = IF(IF>size(QT,1) & IF<=size(QT,1)+size(QF,1))-size(QT,1);
      iQ{2} = IF(IF>size(QT,1)+size(QF,1))-size(QT,1)-size(QF,1);
      jQ{1} = JF(IF<=size(QT,1));
      jQ{3} = JF(IF>size(QT,1) & IF<=size(QT,1)+size(QF,1));
      jQ{2} = JF(IF>size(QT,1)+size(QF,1));

      side123 = side([2 1 3]);
      for d = 1:3
        sz = side123;
        sz(d) = sz(d)+1;
        II = cell(3,1);
        [II{1},II{2},II{3}] = ind2sub(sz,iQ{d});
        for pass = 0:1
          II{d} = II{d}-pass;
          keep = II{d}<=side123(d);
          B(sub2ind(side123,II{1}(keep),II{2}(keep),II{3}(keep))) = jQ{d}(keep);
        end
      end

      W = W | B;
    end

    if with_interior && closed
      W = imfill(W,'holes');
    end

    %trisurf(Q(IF,:),DV(:,1),DV(:,2),DV(:,3),'FaceColor','r');
    %hold on;
    %tsurf(F,V,'FaceColor','g','FaceAlpha',0.2,'EdgeAlpha',0.2);
    %hold off;

    %% From cells to faces:
    %[II,JJ,KK] = ind2sub([side(2) side(1) side(3)],find(abs(W)>0.5));
    %Q = [ ...
    %  QT(sub2ind([side(2)+1 side(1)   side(3)  ],II+1,JJ,KK),:); ...
    %  QT(sub2ind([side(2)+1 side(1)   side(3)  ],II+0,JJ,KK),:); ...
    %  QF(sub2ind([side(2)   side(1)   side(3)+1],II,JJ,KK+1),:); ...
    %  QF(sub2ind([side(2)   side(1)   side(3)+1],II,JJ,KK+0),:); ...
    %  QL(sub2ind([side(2)   side(1)+1 side(3)  ],II,JJ+1,KK),:); ...
    %  QL(sub2ind([side(2)   side(1)+1 side(3)  ],II,JJ+0,KK),:); ...
    %  ];
    %trisurf(Q,DV(:,1),DV(:,2),DV(:,3),'FaceAlpha',0.5,'EdgeAlpha',0.5);
    %hold on;
    %tsurf(F,V,'FaceColor','g','FaceAlpha',0.2,'EdgeAlpha',0.2);
    %hold off;
    %axis equal
    %
    %error

    % Pad winding number with 0s and take differences in all 6 directions
    Wp = padarray(W,[1 1 1],0);
    Dy = diff(Wp,1,1);
    Dx = diff(Wp,1,2);
    Dz = diff(Wp,1,3);
    Q = [ ...
      QF(Dz(2:end-1,2:end-1,1:end)>0.5,:); ...
      QL(Dx(2:end-1,1:end,2:end-1)>0.5,:); ...
      QT(Dy(1:end,2:end-1,2:end-1)>0.5,:); ...
      fliplr(QF(Dz(2:end-1,2:end-1,1:end)<-0.5,:)); ...
      fliplr(QL(Dx(2:end-1,1:end,2:end-1)<-0.5,:)); ...
      fliplr(QT(Dy(1:end,2:end-1,2:end-1)<-0.5,:)); ...
      ];
  end
end