filename='BridgeCool_Quadrilateral_Bilinear_Structured';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = 1;
constraint = {'volume'};
optimizer = 'MMA'; incrementFactor = 1;
filterType = 'P1';
constraint_case = 'INEQUALITY';
line_search_initiator = 'STANDARD';

nsteps = 15;
Vfrac_final = 0.2;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function C = top_left_corner(A, r, c)
%TOP_LEFT_CORNER Select the top-left corner of a matrix
%
% C = top_left_corner(A, r, c)
%
% Inputs:
%  A  a rectangular matrix
%  r  the number of rows of the corner we want to select
%  c  the number of columns of the corner we want to select
% Outputs:
%  C  the r-by-c top left corner of A

end


function sR = region(m,varargin)
% return spherical region associated to a set of crystal directions

if check_option(varargin,{'fundamentalRegion','fundamentalSector'}) ...
    && ~check_option(varargin,'complete')
  sR = m.CS.fundamentalSector(varargin{:});
else
  sR = region@vector3d(m,varargin{:});
end

end


%This function is used for mapping the fixation record into the corresponding fixation maps.
screen_res_x = 1440;
screen_res_y = 900;

datasetFile1 = 'movie';
datasetFile = 'videodata';
gazeFile = 'gaze_record';

videoFiles = dir(fullfile('./', datasetFile));
videoNUM = length(videoFiles)-2;
rate = 30;

 for videonum = 1:700
        videofolder =  videoFiles(videonum+2).name
        options.infolder = fullfile( './', datasetFile,  videofolder, 'images' );
        % Cache all frames in memory
        [data.frames,names,video_res_y,video_res_x,nframe ]= readAllFrames( options );
        a=video_res_x/screen_res_x;
        b=(screen_res_y-video_res_y/a)/2;
        all_fixation = zeros(video_res_y,video_res_x,nframe);
        for person = 1:17
            txtloc = fullfile('./', gazeFile, sprintf('P%03d',person), [sprintf('P%03d_Trail',person), sprintf('%04d.txt',videonum)]);
            if exist(txtloc, 'file')
                [time,x_screen,y_screen,event]=textread(txtloc,'%f%f%f%s','headerlines',1);
                if size(time,1)
                    time = time-time(1);
                    event = cellfun(@(x) x(1), event);
                    for index = 1:nframe
                            eff = find( ((index-1)<rate*time/1000000)&(rate*time/1000000<index)&event=='F'); %framerate = 10;
                            x_stimulus=int32(a*x_screen(eff));
                            y_stimulus=int32(a*(y_screen(eff)-b));
                            t = x_stimulus<=0|x_stimulus>=video_res_x|y_stimulus<=0|y_stimulus>=video_res_y;
                            all_fixation(y_stimulus(~t),x_stimulus(~t),index) = 1;
                    end
                end
            end
        end
end


%Copyright 2013 The MathWorks, Inc
function [sSAnt, sSAntPlat] = splitSubArrays(sAnt)
faceSlope = 45; % Change face slope
sAnt.SubarrayNormal = [0 90 180 -90;90-faceSlope 90-faceSlope 90-faceSlope 90-faceSlope];

%% Build 4 separate arrays
epos = getElementPosition(sAnt);
aznorm = [0 90 180 -90];
for i=1:4,
    sSAnt{i} = phased.ConformalArray; %#ok<*AGROW>
    set(sSAnt{i},'ElementPosition',epos(:,(i-1)*16+(1:16)),...
                 'ElementNormal',repmat([aznorm(i);faceSlope],1,16));
    % Use isotropic antenna elements
    sSAnt{i}.Element.FrequencyRange = sAnt.Subarray.Element.FrequencyRange;
    sSAnt{i}.Element.BackBaffled = true;
end

%% Antenna Platform
sSAntPlat = phased.Platform([2.5e4; 0; 0]);


function mono_print_test ( )

%*****************************************************************************80
%
%% MONO_PRINT_TEST tests MONO_PRINT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 November 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MONO_PRINT_TEST\n' );
  fprintf ( 1, '  MONO_PRINT can print out a monomial.\n' );
  fprintf ( 1, '\n' );

  m = 1;
  f = [ 5 ];
  mono_print ( m, f, '  Monomial [5]:' );

  m = 1;
  f = [ -5 ];
  mono_print ( m, f, '  Monomial [5]:' );

  m = 4;
  f = [ 2, 1, 0, 3 ];
  mono_print ( m, f, '  Monomial [2,1,0,3]:' );

  m = 3;
  f = [ 17, -3, 199 ];
  mono_print ( m, f, '  Monomial [17,-3,199]:' );

  return
end


function [L,perm,xsuper,split,tmpsiz] = symfctmex(adjncy, perm, cachsz) %#ok
% [L,perm,xsuper,split,tmpsiz] = symfctmex(X, perm, cachsz)
%
%   Computes sparse symbolic factor L, updated permutation PERM,
%   super-node partition XSUPER, and a splitting of supernodes
%   (SPLIT) to optimize use of the computer cache (assuming
%   CACHSZ*1024 byte available).   TMPSIZ is the amount of floating
%   point working storage that has to be allocated within blkfctmex.
%
%   Invokes ORNL block Cholesky library (Fortran).
%
% **********  INTERNAL FUNCTION OF CHOLTOOL  **********
%
% See also sedumi

% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA

%Indicate to the user Matlab cannot find the SeDuMi binaries
sedumi_binary_error();


function varargout = findcoord(DEM)

%FINDCOORD Find coordinates of nonzero elements in GRIDobj
%
% Syntax
%
%     [x,y] = findcoord(DEM)
%     [x,y,val] = findcoord(DEM)
%
% Description
%
%     This function returns the coordinates of nonzero values in DEM.
%
% Input arguments
%
%     DEM     GRIDobj
%
% Output arguments
%
%     x,y     coordinate pair
%     val     value of nonzero element in DEM.Z
%
% See also: find, GRIDobj/coord2ind, GRIDobj/ind2coord, GRIDobj/find
%
% Author: Wolfgang Schwanghart (w.schwanghart[at]geo.uni-potsdam.de)
% Date: 10. December, 2014

ix = find(DEM);
[x,y] = ind2coord(DEM,ix);

varargout{1} = x;
varargout{2} = y;
if nargout == 3
    varargout{3} = DEM.Z(ix);
end


% This folder contains scripts/utilities to support the functionality of classifiers
%
%
%CLSUTIL_SHRINKAGE - Determine shrinkage parameter of a classifier.
%                    Example: the degree of regularization of the estimated covariance matrix
%                    towards a unit sphere (used in LDA classifiers)


function [elem,bdFlag] = sortelem(elem,bdFlag)
%% SORTELEM sort elem in ascend ordering
%
% [elem,bdFlag] = SORTELEM(elem,bdFlag) sorts the elem such that
% elem(t,1)< elem(t,2)< elem(t,3). A simple sort(elem,2) cannot
% switch bdFlag.
%
% See also  sortelem3
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Dimension check
if size(elem,2) >= 4 % 3D case
    [elem,bdFlag] = sortelem3(elem,bdFlag);
    return
end

%% Step 1: make elem(:,3) to the largest one
[tempvar,idx] = max(elem,[],2); %#ok<ASGLU>
elem(idx==1,1:3) = elem(idx==1,[2 3 1]);
elem(idx==2,1:3) = elem(idx==2,[3 1 2]);
if exist('bdFlag','var') && ~isempty(bdFlag)
    bdFlag(idx==1,1:3) = bdFlag(idx==1,[2 3 1]);
    bdFlag(idx==2,1:3) = bdFlag(idx==2,[3 1 2]);
end

%% Step 2: swtich the first two vertices such that elem(:,1)<elem(:2)
idx = (elem(:,2) < elem(:,1));
elem(idx,[1 2]) = elem(idx,[2 1]);
if exist('bdFlag','var') && ~isempty(bdFlag)
    bdFlag(idx,[1 2]) = bdFlag(idx,[2 1]);
else
    bdFlag = [];
end


%TRANIMATE Animate a coordinate frame
%
% TRANIMATE(P1, P2, OPTIONS) animates a 3D coordinate frame moving from pose X1
% to pose X2.  Poses X1 and X2 can be represented by:
%   - homogeneous transformation matrices (4x4)
%   - orthonormal rotation matrices (3x3)
%   - Quaternion
%
% TRANIMATE(X, OPTIONS) animates a coordinate frame moving from the identity pose
% to the pose X represented by any of the types listed above.
%
% TRANIMATE(XSEQ, OPTIONS) animates a trajectory, where XSEQ is any of
%   - homogeneous transformation matrix sequence (4x4xN)
%   - orthonormal rotation matrix sequence (3x3xN)
%   - Quaternion vector (Nx1)
%
% Options::
%  'fps', fps    Number of frames per second to display (default 10)
%  'nsteps', n   The number of steps along the path (default 50)
%  'axis',A      Axis bounds [xmin, xmax, ymin, ymax, zmin, zmax]
%  'movie',M     Save frames as files in the folder M
%
%  Additional options are passed through to TRPLOT.
%
% Notes::
% - Uses the Animate helper class to record the frames.
% - Poses X1 and X2 must both be of the same type
% - The 'movie' options saves frames as files NNNN.png.
% - To convert frames to a movie use a command like:
%        ffmpeg -r 10 -i %04d.png out.avi
%
% See also TRPLOT, Animate.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

% TODO
%  auto detect the axis scaling
function tranimate(P2, varargin)

    opt.fps = 10;
    opt.nsteps = 50;
    opt.axis = [];
    opt.movie = [];

    [opt, args] = tb_optparse(opt, varargin);

    if ~isempty(opt.movie)
        anim = Animate(opt.movie);
    end
    P1 = [];

    % convert quaternion and rotation matrix to hom transform
    if isa(P2, 'Quaternion')
        T2 = P2.T;   % convert quaternion to transform
        if ~isempty(args) && isa(args{1},'Quaternion')
            T1 = T2;
            Q2 = args{1};
            T2 = Q2.T;
            args = args(2:end);
        else
            T1 = eye(4,4);
        end
    elseif isrot(P2)
        T2 = r2t(P2);
        if ~isempty(args) && isrot(args{1})
            T1 = T2;
            T2 = r2t(args{1});
            args = args(2:end);
        else
            T1 = eye(4,4);
        end
    elseif ishomog(P2)
        T2 = P2;
        if ~isempty(args) && ishomog(args{1})
            T1 = T2;
            T2 = args{1};
            args = args(2:end);
        else
            T1 = eye(4,4);
        end
    end

    % at this point
    %   T1 is the initial pose
    %   T2 is the final pose
    %
    %  T2 may be a sequence

    if size(T2,3) > 1
        % tranimate(Ts)
        % we were passed a homog sequence
        if ~isempty(P1)
            error('only 1 input argument if sequence specified');
        end
        Ttraj = T2;
    else
        % tranimate(P1, P2)
        % create a path between them
        Ttraj = ctraj(T1, T2, opt.nsteps);
    end

    if isempty(opt.axis)
        % create axis limits automatically based on motion of frame origin
        t = transl(Ttraj);
        mn = min(t) - 1.5;  % min value + length of axis + some
        mx = max(t) + 1.5;  % max value + length of axis + some
        axlim = [mn; mx];
        axlim = axlim(:)';
        args = [args 'axis' axlim];
    end

    hg = trplot(eye(4,4), args{:});  % create a frame at the origin

    % animate it for all poses in the sequence
    for i=1:size(Ttraj,3)
        T = Ttraj(:,:,i);
        trplot(hg, T);

        if ~isempty(opt.movie)
            anim.add();
        end

        pause(1/opt.fps);
    end


function display(ker)

% DISPLAY
%
% Display a textual representation of a linear kernel object.
%
%    display(ker);

%
% File        : @linear/display.m
%
% Date        : Tuesday 12th September
%
% Author      : Dr Gavin C. Cawley
%
% Description : Display method for a class implementing a linear kernel.
%               Part of an object-oriented implementation of Vapnik's Support
%               Vector Machine, as described in [1].
%
% References  : [1] V.N. Vapnik,
%                   "The Nature of Statistical Learning Theory",
%                   Springer-Verlag, New York, ISBN 0-387-94559-8,
%                   1995.
%
% History     : 12/09/2000 - v1.00
%
% Copyright   : (c) Dr Gavin C. Cawley, September 2000.
%
%    This program is free software; you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation; either version 2 of the License, or
%    (at your option) any later version.
%
%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program; if not, write to the Free Software
%    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
%

fprintf(1,'\nlinear kernel\n\n');

% bye bye...


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

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


function [vol, M, dcminfo, mr_parms] = load_dicom_fl(flist)
% [vol, M, dcminfo, mr_parms] = load_dicom_fl(flist)
%
% Loads a volume from the dicom files in flist.
%
% The volume dimensions are arranged such that the
% readout dimension is first, followed by the phase-encode,
% followed by the slices (this is not implemented yet)
%
% M is the 4x4 vox2ras transform such that
% vol(i1,i2,i3), xyz1 = M*[i1 i2 i3 1] where the
% indicies are 0-based.
%
% mr_parms = [tr flipangle te ti]
%
% Does not handle multiple frames correctly yet.
%

%
% load_dicom_fl.m
%
% Original Author: Doug Greve
% CVS Revision Info:
%    $Author: nicks $
%    $Date: 2011/03/02 00:04:12 $
%    $Revision: 1.11 $
%
% Copyright © 2011 The General Hospital Corporation (Boston, MA) "MGH"
%
% Terms and conditions for use, reproduction, distribution and contribution
% are found in the 'FreeSurfer Software License Agreement' contained
% in the file 'LICENSE' found in the FreeSurfer distribution, and here:
%
% https://surfer.nmr.mgh.harvard.edu/fswiki/FreeSurferSoftwareLicense
%
% Reporting: freesurfer@nmr.mgh.harvard.edu
%

vol=[];
M=[];

if(nargin ~= 1)
  fprintf('[vol, M] = load_dicom_fl(flist)\n');
  return;
end

nfiles = size(flist,1);

tic
fprintf('Loading dicom info foreach file \n');
for n = 1:nfiles
  fname = deblank(flist(n,:));
  % fprintf('n = %d/%d, %s   %g\n',n,nfiles,fname,toc);
  tmpinfo = dicominfo(fname);
  if(isempty(tmpinfo))
    fprintf('ERROR: reading %s\n',fname);
    return;
  end
  if(n > 1)
    % Check that the nth series number agrees with the first
    if(tmpinfo.SeriesNumber ~= dcminfo0(1).SeriesNumber)
      fprintf('ERROR: series number inconsistency (%s)\n',fname);
      return;
    end
  end
  tmpinfo.fname = fname;
  dcminfo0(n) = tmpinfo;
end

% Sort by slice location %
dcminfo = sort_by_sliceloc(dcminfo0);

% Slice direction cosine %
sdc = dcminfo(nfiles).ImagePositionPatient-dcminfo(1).ImagePositionPatient;
sdc = sdc /sqrt(sum(sdc.^2));

% Distance between slices %
dslice = sqrt(sum((dcminfo(2).ImagePositionPatient-dcminfo(1).ImagePositionPatient).^2));

% Matrix of direction cosines %
Mdc = zeros(3,3);
Mdc(:,1) = dcminfo(1).ImageOrientationPatient(1:3);
Mdc(:,2) = dcminfo(1).ImageOrientationPatient(4:6);
Mdc(:,3) = sdc;

% Voxel resolution %
delta = [dcminfo(1).PixelSpacing; dslice];
D = diag(delta);

% XYZ of first voxel in first slice %
P0 = dcminfo(1).ImagePositionPatient;

% Change Siemens to be RAS %
% GE is also LPS - change it to be RAS, too - ebeth %
Manufacturer = dcminfo(1).Manufacturer;
if(strcmpi(Manufacturer,'Siemens') | strcmpi(Manufacturer,'ge medical systems'))
  % Change to RAS
  Mdc(1,:) = -Mdc(1,:);
  Mdc(2,:) = -Mdc(2,:);
  P0(1)    = -P0(1);
  P0(2)    = -P0(2);
end

% Compute vox2ras transform %
M = [Mdc*D P0; 0 0 0 1];
if (0&strcmpi(Manufacturer,'Siemens'))
  % Correcting for P0 being at corner of
  % the first voxel instead of at the center
  M = M*[[eye(3) [0.5 0.5 0]']; 0 0 0 1];  %'
end

% Pre-allocate vol. Note: column and row designations do
% not really mean anything. The "column" is the fastest
% dimension. The "row" is the next fastest, etc.
ndim1 = dcminfo(1).Columns;
ndim2 = dcminfo(1).Rows;
ndim3 = nfiles;
vol = zeros(ndim1,ndim2,ndim3);

fprintf('Loading data from each file.\n');
for n = 1:nfiles
  %fprintf('n = %d, %g\n',n,toc);
  fname = dcminfo(n).fname;
  x = dicomread(fname);
  if(isempty(x))
    fprintf('ERROR: could not load pixel data from %s\n',fname);
    return;
  end
  % Note: dicomread will transposed the image. This is supposed
  % to help. Anyway, to make the vox2ras transform agree with
  % the volume, the image is transposed back.
  vol(:,:,n) = x'; %'
end

% Reorder dimensions so that ReadOut dim is first %
if(0 & ~strcmpi(dcminfo(1).PhaseEncodingDirection,'ROW'))
  % This does not work
  fprintf('INFO: permuting vol so that ReadOut is first dim\n');
  vol = permute(vol,[2 1 3]);
  Mtmp = M;
  M(:,1) = Mtmp(:,2);
  M(:,2) = Mtmp(:,1);
end

% Lines below correct for the Z-offset in GE machines - ebeth %
% We're told ge machines recenter along superior/inferior axis but
% don't update c_ras - but now c_s should be zero.
if(strcmpi(Manufacturer,'ge medical systems'))
  % Lines below correct for the Z-offset in GE machines
  firstZ = dcminfo(1).ImagePositionPatient(3);
  lastXYZ = M*[size(vol)';1]; %'
  % size(imvol) = number of slices in all 3 dirs
  lastZ = lastXYZ(3);
  offsetZ = (lastZ + firstZ)/2.0;
  % Z0 = Z + offsetZ;  [XYZ1]' = M*[CRS1]', need to add to M(3,4)(?)
  M(3,4) = M(3,4) - offsetZ;
end

% if(strcmpi(Manufacturer,'ge medical systems'))
%   M(3,4) = 0; % Wow - that was actually completely wrong!
% end

% Pull out some info from the header %
if(isfield(dcminfo(1),'FlipAngle')) FlipAngle = pi*dcminfo(1).FlipAngle/180;
else FlipAngle = 0;
end
if(isfield(dcminfo(1),'EchoTime')) EchoTime = dcminfo(1).EchoTime;
else EchoTime = 0;
end
if(isfield(dcminfo(1),'RepetitionTime')) RepetitionTime = dcminfo(1).RepetitionTime;
else RepetitionTime = 0;
end
InversionTime = 0;
mr_parms = [RepetitionTime FlipAngle EchoTime InversionTime];

return;

%----------------------------------------------------------%
function dcminfo2 = sort_by_sliceloc(dcminfo)
  nslices = length(dcminfo);
  sliceloc = zeros(nslices,1);
  for n = 1:nslices
    sliceloc(n) = dcminfo(n).SliceLocation;
  end

  [tmp ind] = sort(sliceloc);
  dcminfo2 = dcminfo(ind);
return;

%----------------------------------------------------------%


%% ANAP-building
% %{
imfolder = 'images\ANAP-building';
im_n = 2;
imfile = cell(im_n,1);
imfile{1} = [imfolder '\' 'building0.jpg'];
imfile{2} = [imfolder '\' 'building1.jpg'];

im = cell(im_n,1);
for ii = 1:im_n
    im{ii} = imread(imfile{ii});
end

edge_list = [1,2];

imsize = zeros(im_n,3);

for ii = 1:im_n
    imsize(ii,:) = size(im{ii});
    if imsize(ii,1) > 720
        scale = 720/size(im{ii}, 1);
        im{ii} = imresize(im{ii}, scale);

        imsize(ii,:) = size(im{ii});
    end
end

mosaic = REW_mosaic( im, edge_list, 1, 'persp', 200, imfolder );
%}


% 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);


filename='Sphere_Tetrahedra_Linear_Unstructured';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'sphere';
fracRadius = 1-1e-6;
cost = {'compliance'};
weights = [1];
constraint = {'volumeConstraint'};
optimizer = 'HAMILTON-JACOBI';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'P1';

nsteps = 1;
Vfrac_final = 1;
Perimeter_target=3.5;
optimality_final =1e-5;
constr_final =1e-5;

BCscale_factor = 0.3;
HJiter0 = 1;
e2 = 5;

Vfrac_initial = 1;
optimality_initial = 5e-2;
constr_initial = 5e-2;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;

% maxiter = 10;
% maxiter = 1;
TOL.nu_plus = 0.3;
TOL.nu_minus = 0.3;

plotting = 0;
printing = 0;
monitoring = 0;
monitoring_interval = 0;


function ndx = mysub2ind(siz,subinds)

k = [1 cumprod(siz(1:end-1))];
ndx = ones(size(subinds,1),1);
for i = 1:size(subinds,2)
    for r =1:size(subinds,1)
        v = subinds(r,i);
        ndx(r) = ndx(r) + (v-1)*k(i);
    end;
end


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 [ y, m ] = month_carry_english ( y, m )

%*****************************************************************************80
%
%% MONTH_CARRY_ENGLISH carries a year of months on the English calendar.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, the year and month.
%    On output, M is no greater than 12.
%
  while ( 1 )

    months = year_length_months_english ( y );

    if ( m <= months )
      break
    end

    m = m - months;
    y = y + 1;

  end

  return
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


%% Set bounds for optimization problem

% Set Bounds
model_bounds = rabbit.getLimits();
model_bounds.states.x.lb = [-10, 0.5, -1, 2, 0.5, 2, 0.5];
model_bounds.states.x.ub = [10, 2, 1, 5, 2, 5, 2];
model_bounds.states.dx.lb = -20*ones(1,7);
model_bounds.states.dx.ub = 20*ones(1,7);
model_bounds.states.ddx.lb = -100*ones(1,7);
model_bounds.states.ddx.ub = 100*ones(1,7);
bounds = struct();

% Right Stance
bounds.RightStance = model_bounds;

bounds.RightStance.time.t0.lb = 0;
bounds.RightStance.time.t0.ub = 0;
bounds.RightStance.time.t0.x0 = 0;

bounds.RightStance.time.tf.lb = 0.25;
bounds.RightStance.time.tf.ub = 0.75;
bounds.RightStance.time.tf.x0 = 0.75;

bounds.RightStance.time.duration.lb = 0.25;
bounds.RightStance.time.duration.ub = 0.75;
bounds.RightStance.time.duration.x0 = 0.75;

bounds.RightStance.inputs.ConstraintWrench.fRightToe.lb = -1000;
bounds.RightStance.inputs.ConstraintWrench.fRightToe.ub = 1000;
bounds.RightStance.inputs.ConstraintWrench.fRightToe.x0 = 100;

bounds.RightStance.inputs.Control.u.lb = -100*ones(4,1);
bounds.RightStance.inputs.Control.u.ub = 100*ones(4,1);
bounds.RightStance.inputs.Control.u.x0 = zeros(4,1);

bounds.RightStance.params.pRightToe.lb = -0*ones(3,1);
bounds.RightStance.params.pRightToe.ub = 0*ones(3,1);
bounds.RightStance.params.pRightToe.x0 = zeros(3,1);

bounds.RightStance.params.atime.lb = -10*ones(6*4,1);
bounds.RightStance.params.atime.ub = 10*ones(6*4,1);
bounds.RightStance.params.atime.x0 = zeros(6*4,1);

bounds.RightStance.params.ptime.lb = [bounds.RightStance.time.tf.lb, bounds.RightStance.time.t0.lb];
bounds.RightStance.params.ptime.ub = [bounds.RightStance.time.tf.ub, bounds.RightStance.time.t0.ub];
bounds.RightStance.params.ptime.x0 = [bounds.RightStance.time.t0.x0, bounds.RightStance.time.tf.x0];

bounds.RightStance.time.kp = 100;
bounds.RightStance.time.kd = 20;

% Right Impact
bounds.RightImpact = model_bounds;


function [geneKO] = calculateGeneKOMatrix(model, varargin)
% Build a rxn-gene matrix such that the i-th column indicates what
% reactions become inactive because of the i-th gene's knock-out.
%
% USAGE:
%
%    geneKO = calculateGeneKOMatrix(model, SeparateTranscript, printLevel)
%
% INPUT:
%    model:             The COBRA Model structure
%
% OPTIONAL INPUTS:
%    SeparateTranscript Character used to separate
%                       different transcripts of a gene. (default = '')
%                       Examples:
%                           - SeparateTranscript = ''
%                              - gene 10005.1    ==>    gene 10005.1
%                              - gene 10005.2    ==>    gene 10005.2
%                              - gene 10005.3    ==>    gene 10005.3
%                           - SeparateTranscript = '.'
%                              - gene 10005.1
%                              - gene 10005.2    ==>    gene 10005
%                              - gene 10005.3
%    printLevel:       Integer. 1 if the process is wanted to be shown
%                      on the screen, 0 otherwise. (default = 1)
%
% OUTPUT:
%    geneKO:            Struct which contains matrix with blocked
%                       reactions for each gene in the metabolic model,
%                       name of reactions and name of genes.
%
% .. Authors:
%       - Luis V. Valcarcel, Oct 2017, University of Navarra, CIMA & TECNUN School of Engineering.
%       - Luis V. Valcarcel, 03/11/2018, University of Navarra, CIMA & TECNUN School of Engineering.

p = inputParser; % check the input information
addRequired(p, 'model');
addOptional(p, 'SeparateTranscript', '', @ischar);
addOptional(p, 'printLevel', 1, @(x)isnumeric(x)&&isscalar(x));
parse(p, model, varargin{:});

% define gene set
genes = unique(strtok(model.genes, p.Results.SeparateTranscript));
ngenes = numel(genes);

% generate output matrix
ko_rxn_gene = zeros(numel(model.rxns),ngenes);

% affected reactions
if p.Results.printLevel > 0
    showprogress(0, 'Calculate Gene Knock-out matrix');
end
for gen = 1:ngenes
    if p.Results.printLevel > 0
        showprogress(gen/ngenes, 'Calculate Gene Knock-out matrix');
    end

    transcripts = model.genes(strcmp(strtok(model.genes,p.Results.SeparateTranscript),genes{gen})); % to support R2015b
    [~, hasEffect, constrRxnNames] = deleteModelGenes(model, transcripts);

    if hasEffect
        % search index of bloced reactions
        [~,idx] = ismember(constrRxnNames,model.rxns);
        ko_rxn_gene(idx,gen) = 1;
    end
end
if p.Results.printLevel > 0
    fprintf('\tGeneKOMatrix calculated\n');
end

% geneKO
geneKO.genes = genes;
geneKO.rxns = model.rxns;
geneKO.matrix = (ko_rxn_gene~=0);
end


% input in seconds

TR = 2;     % 1 = leave in seconds; 2, downsample by 2, etc.

len = 200;  % length in s
tp = 5;    % estimates 20 time points

scanspersess = [40 40 10];     % images per session; length is num sessions

[X,delta,delta_hires,hrf] = onsets2delta(ons,TR,len);

[DX,sf] = tor_make_deconv_mtx3(delta,tp,1,0,1,0,scanspersess);

% for phys data, .01 s -> 1s
Y = RESAMPLE(X,1,100);


  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()


% drawLineSegments draws Lines using given set of points.
% Input     - MapSize : The size of EdgeMap which can be expressed as
%                       [ROW COL]
%             LineSegments : The set of points which will be drawn.
%                           EdgeContour can be used as input as well.
function drawLineSegments(MapSize, LineSegments)

%% Initialize
NumberOfSegments = length(LineSegments);

% Default setting for plot.
hold on;
minx = 1; maxx = MapSize(2);
miny = 1; maxy = MapSize(1);
axis([minx maxx miny maxy]);
axis off;
set(gca, 'Ydir', 'reverse')

% Use different colors to distinguish each edge.
ColorSet = [
0.00 0.00 1.00 % Data 1 - blue
0.00 1.00 0.00 % Data 2 - green
1.00 0.00 0.00 % Data 3 - red
0.00 1.00 1.00 % Data 4 - cyan
1.00 0.00 1.00 % Data 5 - magenta
0.75 0.75 0.00 % Data 6 - RGB
0.25 0.25 0.25 % Data 7
0.75 0.25 0.25 % Data 8
0.95 0.95 0.00 % Data 9
0.25 0.25 0.75 % Data 10
0.75 0.75 0.75 % Data 11
0.00 0.50 0.00 % Data 12
0.76 0.57 0.17 % Data 13
0.54 0.63 0.22 % Data 14
0.34 0.57 0.92 % Data 15
1.00 0.10 0.60 % Data 16
0.88 0.75 0.73 % Data 17
0.10 0.49 0.47 % Data 18
0.66 0.34 0.65 % Data 19
0.99 0.41 0.23 % Data 20
];

%% Draw each Segement.
for i=1:NumberOfSegments
    Segment = LineSegments{i};
    x = Segment(:,2);
    y = Segment(:,1);
    plot(x, y, 'LineWidth', 2, 'Color', ColorSet(mod(i,20)+1,:));
end

end