% SP_H1_ERROR: Evaluate the error in H^1 norm.
%
%   [errh1, errl2, errh1s] = sp_h1_error (space, msh, u, uex, graduex)
%
% INPUT:
%
%    space:   object defining the space of discrete functions (see sp_vector)
%    msh:     object defining the domain partition and the quadrature rule (see msh_cartesian)
%    u:       vector of dof weights
%    uex:     function handle to evaluate the exact solution
%    graduex: function handle to evaluate the gradient of the exact solution
%
% OUTPUT:
%
%     errh1:  error in H^1 norm
%     errl2:  error in L^2 norm
%     errh1s: error in H^1 seminorm
%
% Copyright (C) 2010 Carlo de Falco
% Copyright (C) 2011, 2015 Rafael Vazquez
%
% 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 3 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 Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

function [errh1, errl2, errh1s] = sp_h1_error (space, msh, u, uex, graduex)

  if (numel(u) ~= space.ndof)
    error ('Wrong size of the vector of degrees of freedom')
  end

  errl2 = 0; errh1s = 0;
  for iel = 1:msh.nel_dir(1)
    msh_col = msh_evaluate_col (msh, iel);
    sp_col  = sp_evaluate_col (space, msh_col, 'value', true, 'gradient', true);

    [~, err_l2, err_h1s] = sp_h1_error (sp_col, msh_col, u, uex, graduex);

    errh1s = errh1s + err_h1s.^2;
    errl2 = errl2 + err_l2.^2;
  end

  errh1 = sqrt (errl2 + errh1s);
  errl2 = sqrt (errl2);
  errh1s = sqrt (errh1s);

end


function [ csd ] = bz_CSD (lfp, varargin)

% [ CSD ] = bz_CSD (lfp, varargin)
% Calculates the 1D approximation of current source density (CSD) from a
% linear array of LFPs

% INPUT
%    lfp            a buzcode structure with fields lfp.data,
%                                                   lfp.timestamps
%                                                   lfp.samplingRate
%                   -lfp can also be a [t x 1] timeseries signal. in which
%                   case you need to input 'samplingRate'
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%       channels    vector with channels to inlcude. If empty take all (default)
%       win         time interval to compute CSD. If empty take all (default)
%       spat_sm     degree of spatial smoothing. Default = 0.
%       temp_sm     degree of temporal smoothing. Default = 0.
%       plotCSD     true/false. Default true.
%       plotLFP     true/false. Default true.
%    =========================================================================

% OUTPUT:
%    CSD           a buzcode structure with fields csd.data,
%                                                   csd.timestamps
%                                                   csd.samplingRate
%                                                   csd.channels
%                                                   csd.params

% Antonio FR, 7/18

%% Parse inputs

p = inputParser;
addParameter(p,'channels',1:size(lfp.data,2),@isvector);
addParameter(p,'samplingRate',1250,@isnumeric);
addParameter(p,'win',[1 size(lfp.data,1)],@isnumeric);
addParameter(p,'spat_sm',11,@isnumeric);
addParameter(p,'temp_sm',11,@isnumeric);
addParameter(p,'doDetrend',false,@islogical);
addParameter(p,'plotCSD',true,@islogical);
addParameter(p,'plotLFP',true,@islogical);

parse(p,varargin{:});
channels = p.Results.channels;
samplingRate = p.Results.samplingRate;
spat_sm = p.Results.spat_sm;
temp_sm = p.Results.temp_sm;
doDetrend = p.Results.doDetrend;
plotCSD = p.Results.plotCSD;
plotLFP = p.Results.plotLFP;

%lfp input
if isstruct(lfp)
    data = lfp.data;
    timestamps = lfp.timestamps;
    samplingRate = lfp.samplingRate;
elseif iscell(lfp) %for multiple trials
    celllengths = cellfun(@length,lfp);
    data = vertcat(lfp{:});
elseif isnumeric(lfp)
    data = lfp;
    timestamps = [1:length(lfp)]'./samplingRate;
end

win = p.Results.win*samplingRate;

%% Compute CSD

lfp_frag = data(win(1):win(2),channels)*-1;

% detrend
if doDetrend
   lfp_frag = detrend(lfp_frag')';
end

% temporal smoothing
if temp_sm > 0
   for ch = 1:size(lfp_frag,2)
       lfp_frag(:,ch) = smooth(lfp_frag(:,ch),temp_sm,'sgolay');
   end
end

% spatial smoothing
if spat_sm > 0
   for t = 1:size(lfp_frag,1)
       lfp_frag(t,:) = smooth(lfp_frag(t,:),spat_sm,'lowess');
   end
end

% calculate CSD
CSD = diff(lfp_frag,2,2);

% generate output structure
csd.data = CSD;
csd.timestamps = timestamps(win(1):win(2));
csd.samplingRate = samplingRate;
csd.channels = channels;
csd.params.spat_sm = spat_sm;
csd.params.temp_sm = temp_sm;
csd.params.detrend = doDetrend;

%% Plot

if plotLFP

    cmax = max(max(CSD));

    figure;
    subplot(1,2,1);
    contourf(timestamps(win(1):win(2)),1:size(CSD,2),CSD',40,'LineColor','none');hold on;
    colormap jet; caxis([-cmax cmax]);
    set(gca,'YDir','reverse');xlabel('time (s)');ylabel('channel');title('CSD');

    subplot(1,2,2);
    for ch=1:size(lfp_frag,2)
        offset = 500*(ch-1);
        sh_tmp = 10e5*(lfp_frag(:,ch)) + offset;
        plot(timestamps(win(1):win(2)),sh_tmp,'k','LineWidth',1.5); hold on;
        clear sh_tmp
    end
    set(gca,'YDir','reverse','YTickLabel',[]);ylim([-500 offset+500]);xlim([timestamps(win(1)) timestamps(win(2))]);
    xlabel('time (s)');ylabel('channel');title('LFP');

elseif plotCSD

     cmax = max(max(CSD));

     figure;
     contourf(timestamps(win(1):win(2)),1:size(CSD,2),CSD',40,'LineColor','none');hold on;
     colormap jet; caxis([-cmax cmax]);
     set(gca,'YDir','reverse');xlabel('time (s)');ylabel('channel');title(CSD);

end

end


function [TET,Vtet,C]=hex2tet(varargin)

% function [TET,Vtet,C]=hex2tet(HEX,V,C,tetOpt)

%%

switch nargin
    case 2
        HEX=varargin{1};
        V=varargin{2};
        C=[];
        tetOpt=1;
    case 3
        HEX=varargin{1};
        V=varargin{2};
        C=varargin{3};
        tetOpt=1;
    case 4
        HEX=varargin{1};
        V=varargin{2};
        C=varargin{3};
        tetOpt=varargin{4};
    otherwise
        error('Wrong number of input arguments');
end

%%

C=C(:);
switch tetOpt
    case 1 %Add central node and cross side faces

        [F,~]=element2patch(HEX,C,'hex8');

        numV=size(V,1);
        numE=size(HEX,1);

        %The original vertices
        X=V(:,1); Y=V(:,2); Z=V(:,3);

        %The mid-element points
        if numE==1
            Vm=[mean(X(HEX),1) mean(Y(HEX),1) mean(Z(HEX),1)];
        else
            Vm=[mean(X(HEX),2) mean(Y(HEX),2) mean(Z(HEX),2)];
        end

        %The mid-face points
        Vf=[mean(X(F),2) mean(Y(F),2) mean(Z(F),2)];

        %TET point collection
        Vtet=[V; Vm; Vf];

        %Defining tetrahedral node list per hex element
        numV2=numV+numE+1;
        indAdd=reshape(numV2:(numV2+6*numE)-1,numE,6);
        TET_set=[HEX (numV+1:numV+numE)' indAdd];

        TET_format=[14 13 9 10;...
                    12 14 9 10;...
                    15 12 9 10;...
                    13 15 9 10;...
                    %
                    13 14 9 11;...
                    14 12 9 11;...
                    12 15 9 11;...
                    15 13 9 11;...
                    %
                    1 10 12 15;...
                    2 10 14 12;...
                    3 10 13 14;...
                    4 10 15 13;...
                    %
                    5 11 15 12;...
                    6 11 12 14;...
                    7 11 14 13;...
                    8 11 13 15;...
                    %
                    1 2 12 10;...
                    2 3 14 10;...
                    3 4 13 10;...
                    4 1 15 10;...
                    %
                    5 6 11 12;...
                    6 7 11 14;...
                    7 8 11 13;...
                    8 5 11 15;...
                    %
                    1 5 15 12;...
                    2 6 12 14;...
                    3 7 14 13;...
                    4 8 13 15;...
                    ];

        %Reform TET_set as an nx4
        TET_set_reform1=TET_set(:,TET_format(:))';
        TET_set_reform2=reshape(TET_set_reform1,size(TET_format,1),numel(TET_set_reform1)/size(TET_format,1))';
        TET=reshape(TET_set_reform2,4,numel(TET_set_reform2)/4)';

        %Fix color information
        C=repmat(C,size(TET_format,1),1);

        %Removing double vertices
        [TET,Vtet]=mergeVertices(TET,Vtet);

    case 2 %Delaunay based 6 tetrahedron decomposition of cube applied to all
        HEX=HEX(:,[1 2 4 3 5 6 8 7]);
        tetInd =[5     1     2     3;...
            6     5     2     3;...
            6     7     5     3;...
            6     4     7     3;...
            6     2     4     3;...
            6     8     7     4];
        a=tetInd';
        a=a(:)';
        A=HEX(:,a);
        TET=reshape(A',4,6.*size(HEX,1))';
        if ~isempty(C)
            C=(ones(6,1)*C');
            C=C(:);
        end
        Vtet=V;
    case 3 %Same as 2 but flipped top to bottom

        %Switch top and bottom
        HEX=HEX(:,[8 7 5 6 4 3 1 2]);

        tetInd =[5     1     2     3;...
            6     5     2     3;...
            6     7     5     3;...
            6     4     7     3;...
            6     2     4     3;...
            6     8     7     4];
        a=tetInd';
        a=a(:)';
        A=HEX(:,a);
        TET=reshape(A',4,6.*size(HEX,1))';
        if ~isempty(C)
            C=(ones(6,1)*C');
            C=C(:);
        end
        Vtet=V;
    case 4 % 5 tetrahedron decomposition of cube
        tetInd=[1 8 6 5; 7 8 6 3; 2 1 3 6; 4 8 3 1; 6 3 8 1];
        a=tetInd';
        a=a(:)';
        A=HEX(:,a);
        TET=reshape(A',4,5.*size(HEX,1))';
        if ~isempty(C)
            C=(ones(5,1)*C');
            C=C(:);
        end
        Vtet=V;
    case 5 %Same as 4 but flipped top to bottom
        %Switch top and bottom
        HEX=(HEX(:,[4 3 7 8 1 2 6 5 ]));

        tetInd=[1 8 6 5; 7 8 6 3; 2 1 3 6; 4 8 3 1; 6 3 8 1];
        a=tetInd';
        a=a(:)';
        A=HEX(:,a);
        TET=reshape(A',4,5.*size(HEX,1))';
        if ~isempty(C)
            C=(ones(5,1)*C');
            C=C(:);
        end
        Vtet=V;
    case 6 % tets for octet-truss lattice
         [F,~]=element2patch(HEX,C,'hex8');

        numV=size(V,1);
        numE=size(HEX,1);

        %The original vertices
        X=V(:,1); Y=V(:,2); Z=V(:,3);

        %The mid-element points
        if numE==1
            Vm=[mean(X(HEX),1) mean(Y(HEX),1) mean(Z(HEX),1)];
        else
            Vm=[mean(X(HEX),2) mean(Y(HEX),2) mean(Z(HEX),2)];
        end

        %The mid-face points
        Vf=[mean(X(F),2) mean(Y(F),2) mean(Z(F),2)];

        %TET point collection
        Vtet=[V; Vm; Vf];

        %Defining tetrahedral node list per hex element
        numV2=numV+numE+1;
        indAdd=reshape(numV2:(numV2+6*numE)-1,numE,6);
        TET_set=[HEX (numV+1:numV+numE)' indAdd];

        TET_format=[...
%                     %
%                     14 13 9 10;...
%                     12 14 9 10;...
%                     15 12 9 10;...
%                     13 15 9 10;...
%                     %
%                     13 14 9 11;...
%                     14 12 9 11;...
%                     12 15 9 11;...
%                     15 13 9 11;...
                    %
                    1 10 12 15;...
                    2 10 14 12;...
                    3 10 13 14;...
                    4 10 15 13;...
                    %
                    5 11 15 12;...
                    6 11 12 14;...
                    7 11 14 13;...
                    8 11 13 15;...

                    %
                    1 5 15 12;...
                    2 6 12 14;...
                    3 7 14 13;...
                    4 8 13 15;...
                    ];

        %Reform TET_set as an nx4
        TET_set_reform1=TET_set(:,TET_format(:))';
        TET_set_reform2=reshape(TET_set_reform1,size(TET_format,1),numel(TET_set_reform1)/size(TET_format,1))';
        TET=reshape(TET_set_reform2,4,numel(TET_set_reform2)/4)';

        %Fix color information
        C=repmat(C,size(TET_format,1),1);

        %Removing double vertices
        [TET,Vtet]=mergeVertices(TET,Vtet);
end

TET=TET(:,[1 2 4 3]); %Invert

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2019  Kevin Mattheus Moerman
%
% 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 3 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, see <http://www.gnu.org/licenses/>.


function IN = inpolyhedron(varargin)
%INPOLYHEDRON  Tests if points are inside a 3D triangulated (faces/vertices) surface
%   BY CONVENTION, SURFACE NORMALS SHOULD POINT OUT from the object. (see
%   FLIPNORMALS option below for details)
%
%   IN = INPOLYHEDRON(FV,QPTS) tests if the query points (QPTS) are inside the
%   patch/surface/polyhedron defined by FV (a structure with fields 'vertices' and
%   'faces'). QPTS is an N-by-3 set of XYZ coordinates. IN is an N-by-1 logical
%   vector which will be TRUE for each query point inside the surface.
%
%   INPOLYHEDRON(FACES,VERTICES,...) takes faces/vertices separately, rather than in
%   an FV structure.
%
%   IN = INPOLYHEDRON(..., X, Y, Z) voxelises a mask of 3D gridded query points
%   rather than an N-by-3 array of points. X, Y, and Z coordinates of the grid
%   supplied in XVEC, YVEC, and ZVEC respectively. IN will return as a 3D logical
%   volume with SIZE(IN) = [LENGTH(YVEC) LENGTH(XVEC) LENGTH(ZVEC)], equivalent to
%   syntax used by MESHGRID. INPOLYHEDRON handles this input faster and with a lower
%   memory footprint than using MESHGRID to make full X, Y, Z query points matrices.
%
%   INPOLYHEDRON(...,'PropertyName',VALUE,'PropertyName',VALUE,...) tests query
%   points using the following optional property values:
%
%   TOL           - Tolerance on the tests for "inside" the surface. You can think of
%   tol as the distance a point may possibly lie above/below the surface, and still
%   be perceived as on the surface. Due to numerical rounding nothing can ever be
%   done exactly here. Defaults to ZERO. Note that in the current implementation TOL
%   only affects points lying above/below a surface triangle (in the Z-direction).
%   Points coincident with a vertex in the XY plane are considered INside the surface.
%   More formal rules can be implemented with input/feedback from users.
%
%   GRIDSIZE      - Internally, INPOLYHEDRON uses a divide-and-conquer algorithm to
%   split all faces into a chessboard-like grid of GRIDSIZE-by-GRIDSIZE regions.
%   Performance will be a tradeoff between a small GRIDSIZE (few iterations, more
%   data per iteration) and a large GRIDSIZE (many iterations of small data
%   calculations). The sweet-spot has been experimentally determined (on a win64
%   system) to be correlated with the number of faces/vertices. You can overwrite
%   this automatically computed choice by specifying a GRIDSIZE parameter.
%
%   FACENORMALS   - By default, the normals to the FACE triangles are computed as the
%   cross-product of the first two triangle edges. You may optionally specify face
%   normals here if they have been pre-computed.
%
%   FLIPNORMALS   - (Defaults FALSE). To match a wider convention, triangle
%   face normals are presumed to point OUT from the object's surface. If
%   your surface normals are defined pointing IN, then you should set the
%   FLIPNORMALS option to TRUE to use the reverse of this convention.
%
%   Example:
%       tmpvol = zeros(20,20,20);       % Empty voxel volume
%       tmpvol(5:15,8:12,8:12) = 1;     % Turn some voxels on
%       tmpvol(8:12,5:15,8:12) = 1;
%       tmpvol(8:12,8:12,5:15) = 1;
%       fv = isosurface(tmpvol, 0.99);  % Create the patch object
%       fv.faces = fliplr(fv.faces);    % Ensure normals point OUT
%       % Test SCATTERED query points
%       pts = rand(200,3)*12 + 4;       % Make some query points
%       in = inpolyhedron(fv, pts);     % Test which are inside the patch
%       figure, hold on, view(3)        % Display the result
%       patch(fv,'FaceColor','g','FaceAlpha',0.2)
%       plot3(pts(in,1),pts(in,2),pts(in,3),'bo','MarkerFaceColor','b')
%       plot3(pts(~in,1),pts(~in,2),pts(~in,3),'ro'), axis image
%       % Test STRUCTURED GRID of query points
%       gridLocs = 3:2.1:19;
%       [x,y,z] = meshgrid(gridLocs,gridLocs,gridLocs);
%       in = inpolyhedron(fv, gridLocs,gridLocs,gridLocs);
%       figure, hold on, view(3)        % Display the result
%       patch(fv,'FaceColor','g','FaceAlpha',0.2)
%       plot3(x(in), y(in), z(in),'bo','MarkerFaceColor','b')
%       plot3(x(~in),y(~in),z(~in),'ro'), axis image
%
% See also: UNIFYMESHNORMALS (on the file exchange)

% TODO-list
% - Optmise overall memory footprint. (need examples with MEM errors)
% - Implement an "ignore these" step to speed up calculations for:
%     * Query points outside the convex hull of the faces/vertices input
% - Get a better/best gridSize calculation. User feedback?
% - Detect cases where X-rays or Y-rays would be better than Z-rays?

%
%   Author: Sven Holcombe
% - 10 Jun 2012: Version 1.0
% - 28 Aug 2012: Version 1.1 - Speedup using accumarray
% - 07 Nov 2012: Version 2.0 - BEHAVIOUR CHANGE
%                Query points coincident with a VERTEX are now IN an XY triangle
% - 18 Aug 2013: Version 2.1 - Gridded query point handling with low memory footprint.
% - 10 Sep 2013: Version 3.0 - BEHAVIOUR CHANGE
%                NEW CONVENTION ADOPTED to expect face normals pointing IN
%                Vertically oriented faces are now ignored. Speeds up
%                computation and fixes bug where presence of vertical faces
%                produced NaN distance from a query pt to facet, making all
%                query points under facet erroneously NOT IN polyhedron.
% - 25 Sep 2013: Version 3.1 - Dropped nested unique call which was made
%                mostly redundant via v2.1 gridded point handling. Also
%                refreshed grid size selection via optimisation.
% - 25 Feb 2014: Version 3.2 - Fixed indeterminate behaviour for query
%                points *exactly* in line with an "overhanging" vertex.
%%

% FACETS is an unpacked arrangement of faces/vertices. It is [3-by-3-by-N],
% with 3 1-by-3 XYZ coordinates of N faces.
[facets, qPts, options] = parseInputs(varargin{:});
numFaces = size(facets,3);
if ~options.griddedInput            % SCATTERED QUERY POINTS
    numQPoints = size(qPts,1);
else                                % STRUCTURED QUERY POINTS
    numQPoints = prod(cellfun(@numel,qPts(1:2)));
end

% Precompute 3d normals to all facets (triangles). Do this via the cross
% product of the first edge vector with the second. Normalise the result.
allEdgeVecs = facets([2 3 1],:,:) - facets(:,:,:);
if isempty(options.facenormals)
    allFacetNormals =  bsxfun(@times, allEdgeVecs(1,[2 3 1],:), allEdgeVecs(2,[3 1 2],:)) - ...
        bsxfun(@times, allEdgeVecs(2,[2 3 1],:), allEdgeVecs(1,[3 1 2],:));
    allFacetNormals = bsxfun(@rdivide, allFacetNormals, sqrt(sum(allFacetNormals.^2,2)));
else
    allFacetNormals = permute(options.facenormals,[3 2 1]);
end
if options.flipnormals
    allFacetNormals = -allFacetNormals;
end
% We use a Z-ray intersection so we don't even need to consider facets that
% are purely vertically oriented (have zero Z-component).
isFacetUseful = allFacetNormals(:,3,:) ~= 0;

%% Setup grid referencing system
% Function speed can be thought of as a function of grid size. A small number of grid
% squares means iterating over fewer regions (good) but with more faces/qPts to
% consider each time (bad). For any given mesh/queryPt configuration, there will be a
% sweet spot that minimises computation time. There will also be a constraint from
% memory available - low grid sizes means considering many queryPt/faces at once,
% which will require a larger memory footprint. Here we will let the user specify
% gridsize directly, or we will estimate the optimum size based on prior testing.
if ~isempty(options.gridsize)
    gridSize = options.gridsize;
else
    % Coefficients (with 95% confidence bounds):
    p00 =         -47;    p10 =       12.83;    p01 =       20.89;
    p20 =      0.7578;    p11 =      -6.511;    p02 =      -2.586;
    p30 =     -0.1802;    p21 =      0.2085;    p12 =      0.7521;
    p03 =     0.09984;    p40 =    0.005815;    p31 =    0.007775;
    p22 =    -0.02129;    p13 =    -0.02309;
    GSfit = @(x,y)p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2 + p30*x^3 + p21*x^2*y + p12*x*y^2 + p03*y^3 + p40*x^4 + p31*x^3*y + p22*x^2*y^2 + p13*x*y^3;
    gridSize = min(150 ,max(1, ceil(GSfit(log(numQPoints),log(numFaces)))));
    if isnan(gridSize), gridSize = 1; end
end

%% Find candidate qPts -> triangles pairs
% We have a large set of query points. For each query point, find potential
% triangles that would be pierced by vertical rays through the qPt. First,
% a simple filter by XY bounding box

% Calculate the bounding box of each facet
minFacetCoords = permute(min(facets(:,1:2,:),[],1),[3 2 1]);
maxFacetCoords = permute(max(facets(:,1:2,:),[],1),[3 2 1]);

% Set rescale values to rescale all vertices between 0(-eps) and 1(+eps)
scalingOffsetsXY = min(minFacetCoords,[],1) - eps;
scalingRangeXY = max(maxFacetCoords,[],1) - scalingOffsetsXY + 2*eps;

% Based on scaled min/max facet coords, get the [lowX lowY highX highY] "grid" index
% of all faces
lowToHighGridIdxs = floor(bsxfun(@rdivide, ...
    bsxfun(@minus, ... % Use min/max coordinates of each facet (+/- the tolerance)
    [minFacetCoords-options.tol maxFacetCoords+options.tol],...
    [scalingOffsetsXY scalingOffsetsXY]),...
    [scalingRangeXY scalingRangeXY]) * gridSize) + 1;

% Build a grid of cells. In each cell, place the facet indices that encroach into
% that grid region. Similarly, each query point will be assigned to a grid region.
% Note that query points will be assigned only one grid region, facets can cover many
% regions. Furthermore, we will add a tolerance to facet region assignment to ensure
% a query point will be compared to facets even if it falls only on the edge of a
% facet's bounding box, rather than inside it.
cells = cell(gridSize);
[unqLHgrids,~,facetInds] = unique(lowToHighGridIdxs,'rows');
tmpInds = accumarray(facetInds(isFacetUseful),find(isFacetUseful),[size(unqLHgrids,1),1],@(x){x});
for xi = 1:gridSize
    xyMinMask = xi >= unqLHgrids(:,1) & xi <= unqLHgrids(:,3);
    for yi = 1:gridSize
        cells{yi,xi} = cat(1,tmpInds{xyMinMask & yi >= unqLHgrids(:,2) & yi <= unqLHgrids(:,4)});
        % The above line (with accumarray) is faster with equiv results than:
        % % cells{yi,xi} = find(ismember(facetInds, xyInds));
    end
end
% With large number of facets, memory may be important:
clear lowToHightGridIdxs LHgrids facetInds tmpInds xyMinMask minFacetCoords maxFacetCoords

%% Compute edge unit vectors and dot products

% Precompute the 2d unit vectors making up each facet's edges in the XY plane.
allEdgeUVecs = bsxfun(@rdivide, allEdgeVecs(:,1:2,:), sqrt(sum(allEdgeVecs(:,1:2,:).^2,2)));

% Precompute the inner product between edgeA.edgeC, edgeB.edgeA, edgeC.edgeB
allEdgeEdgeDotPs = sum(allEdgeUVecs .* -allEdgeUVecs([3 1 2],:,:),2) - 1e-9;

%% Gather XY query locations
% Since query points are most likely given as a (3D) grid of query locations, we only
% need to consider the unique XY locations when asking which facets a vertical ray
% through an XY location would pierce.
if ~options.griddedInput            % SCATTERED QUERY POINTS
    qPtsXY = @(varargin)qPts(:,1:2);
    qPtsXYZViaUnqIndice = @(ind)qPts(ind,:);
    outPxIndsViaUnqIndiceMask = @(ind,mask)ind(mask);
    outputSize = [size(qPts,1),1];
    reshapeINfcn = @(INMASK)INMASK;
    minFacetDistanceFcn = @minFacetToQptDistance;
else                                % STRUCTURED QUERY POINTS
    [xmat,ymat] = meshgrid(qPts{1:2});
    qPtsXY = [xmat(:) ymat(:)];
    % A standard set of Z locations will be shifted around by different
    % unqQpts XY coordinates.
    zCoords = qPts{3}(:) * [0 0 1];
    qPtsXYZViaUnqIndice = @(ind)bsxfun(@plus, zCoords, [qPtsXY(ind,:) 0]);
    % From a given indice and mask, we will turn on/off the IN points under
    % that indice based on the mask. The easiest calculation is to setup
    % the IN matrix as a numZpts-by-numUnqPts mask. At the end, we must
    % unpack/reshape this 2D mask to a full 3D logical mask
    numZpts = size(zCoords,1);
    baseZinds = 1:numZpts;
    outPxIndsViaUnqIndiceMask = @(ind,mask)(ind-1)*numZpts + baseZinds(mask);
    outputSize = [numZpts, size(qPtsXY,1)];
    reshapeINfcn = @(INMASK)reshape(INMASK', cellfun(@numel, qPts([2 1 3])));
    minFacetDistanceFcn = @minFacetToQptsDistance;
end

% Start with every query point NOT inside the polyhedron. We will
% iteratively find those query points that ARE inside.
IN = false(outputSize);
% Determine with grids each query point falls into.
qPtGridXY = floor(bsxfun(@rdivide, bsxfun(@minus, qPtsXY(:,:), scalingOffsetsXY),...
    scalingRangeXY) * gridSize) + 1;
[unqQgridXY,~,qPtGridInds] = unique(qPtGridXY,'rows');
% We need only consider grid indices within those already set up
ptsToConsidMask = ~any(qPtGridXY<1 | qPtGridXY>gridSize, 2);
if ~any(ptsToConsidMask)
    IN = reshapeINfcn(IN);
    return;
end
% Build the reference list
cellQptContents = accumarray(qPtGridInds(ptsToConsidMask),find(ptsToConsidMask), [],@(x){x});
gridsToCheck = unqQgridXY(~any(unqQgridXY<1 | unqQgridXY>gridSize, 2),:);
cellQptContents(cellfun('isempty',cellQptContents)) = [];
gridIndsToCheck = sub2ind(size(cells), gridsToCheck(:,2), gridsToCheck(:,1));

% For ease of multiplication, reshape qPt XY coords to [1-by-2-by-1-by-N]
qPtsXY = permute(qPtsXY(:,:),[4 2 3 1]);

% There will be some grid indices with query points but without facets.
emptyMask = cellfun('isempty',cells(gridIndsToCheck))';
for i = find(~emptyMask)
    % We get all the facet coordinates (ie, triangle vertices) of triangles
    % that intrude into this grid location. The size is [3-by-2-by-N], for
    % the [3vertices-by-XY-by-Ntriangles]
    allFacetInds = cells{gridIndsToCheck(i)};
    candVerts = facets(:,1:2,allFacetInds);
    % We need the XY coordinates of query points falling into this grid.
    allqPtInds = cellQptContents{i};
    queryPtsXY = qPtsXY(:,:,:,allqPtInds);

    % Get unit vectors pointing from each triangle vertex to my query point(s)
    vert2ptVecs = bsxfun(@minus, queryPtsXY, candVerts);
    vert2ptUVecs = bsxfun(@rdivide, vert2ptVecs, sqrt(sum(vert2ptVecs.^2,2)));
    % Get unit vectors pointing around each triangle (along edge A, edge B, edge C)
    edgeUVecs = allEdgeUVecs(:,:,allFacetInds);
    % Get the inner product between edgeA.edgeC, edgeB.edgeA, edgeC.edgeB
    edgeEdgeDotPs = allEdgeEdgeDotPs(:,:,allFacetInds);
    % Get inner products between each edge unit vec and the UVs from qPt to vertex
    edgeQPntDotPs = sum(bsxfun(@times, edgeUVecs, vert2ptUVecs),2);
    qPntEdgeDotPs = sum(bsxfun(@times,vert2ptUVecs, -edgeUVecs([3 1 2],:,:)),2);
    % If both inner products 2 edges to the query point are greater than the inner
    % product between the two edges themselves, the query point is between the V
    % shape made by the two edges. If this is true for all 3 edge pair, the query
    % point is inside the triangle.
    resultIN = all(bsxfun(@gt, edgeQPntDotPs, edgeEdgeDotPs) & bsxfun(@gt, qPntEdgeDotPs, edgeEdgeDotPs),1);
    resultONVERTEX = any(any(isnan(vert2ptUVecs),2),1);
    result = resultIN | resultONVERTEX;
    qPtHitsTriangles = any(result,3);
    % If NONE of the query points pierce ANY triangles, we can skip forward
    if ~any(qPtHitsTriangles), continue, end

    % In the next step, we'll need to know the indices of ALL the query points at
    % each of the distinct XY coordinates. Let's get their indices into "qPts" as a
    % cell of length M, where M is the number of unique XY points we had found.
    for ptNo = find(qPtHitsTriangles(:))'
        % Which facets does it pierce?
        piercedFacetInds = allFacetInds(result(1,1,:,ptNo));

        % Get the 1-by-3-by-N set of triangle normals that this qPt pierces
        piercedTriNorms = allFacetNormals(:,:,piercedFacetInds);

        % Pick the first vertex as the "origin" of a plane through the facet. Get the
        % vectors from each query point to each facet origin
        facetToQptVectors = bsxfun(@minus, ...
            qPtsXYZViaUnqIndice(allqPtInds(ptNo)),...
            facets(1,:,piercedFacetInds));

        % Calculate how far you need to go up/down to pierce the facet's plane.
        % Positive direction means "inside" the facet, negative direction means
        % outside.
        facetToQptDists = bsxfun(@rdivide, ...
            sum(bsxfun(@times,piercedTriNorms,facetToQptVectors),2), ...
            abs(piercedTriNorms(:,3,:)));

        % Since it's possible for two triangles sharing the same vertex to
        % be the same distance away, I want to sum up all the distances of
        % triangles that are closest to the query point. Simple case: The
        % closest triangle is unique Edge case: The closest triangle is one
        % of many the same distance and direction away. Tricky case: The
        % closes triangle has another triangle the equivalent distance
        % but facing the opposite direction
        IN( outPxIndsViaUnqIndiceMask(allqPtInds(ptNo), ...
            minFacetDistanceFcn(facetToQptDists)<options.tol...
            )) = true;
    end
end

% If they provided X,Y,Z vectors of query points, our output is currently a
% 2D mask and must be reshaped to [LEN(Y) LEN(X) LEN(Z)].
IN = reshapeINfcn(IN);

%% Called subfunctions

% vertices = [
%     0.9046    0.1355   -0.0900
%     0.8999    0.3836   -0.0914
%     1.0572    0.2964   -0.0907
%     0.8735    0.1423   -0.1166
%     0.8685    0.4027   -0.1180
%     1.0337    0.3112   -0.1173
%     0.9358    0.1287   -0.0634
%     0.9313    0.3644   -0.0647
%     1.0808    0.2816   -0.0641
% ];
% faces = [
%      1     2     5
%      1     5     4
%      2     3     6
%      2     6     5
%      3     1     4
%      3     4     6
%      6     4     5
%      2     1     8
%      8     1     7
%      3     2     9
%      9     2     8
%      1     3     7
%      7     3     9
%      7     9     8
% ];
% point = [vertices(3,1),vertices(3,2),1.5];

function closestTriDistance = minFacetToQptDistance(facetToQptDists)
% FacetToQptDists is a 1pt-by-1-by-Nfacets array of how far you need to go
% up/down to pierce each facet's plane. If the Qpt was directly over an
% "overhang" vertex, then two facets with opposite orientation will be
% equally distant from the Qpt, with one distance positive and one
% negative. In such cases, it is impossible for the Qpt to actually be
% "inside" this pair of facets, so their distance is updated to Inf.

[~,minInd] = min(abs(facetToQptDists),[],3);
while any( abs(facetToQptDists + facetToQptDists(minInd)) < 1e-15 )
    % Since the above comparison is made every time, but the below variable
    % setting is done only in the rare case that a query point coincides
    % with an overhang vertex, it is more efficient to re-compute the
    % equality when it's true, rather than store the result every time.
    facetToQptDists( abs(facetToQptDists) - abs(facetToQptDists(minInd)) < 1e-15) = inf;
    if ~any(isfinite(facetToQptDists))
        break;
    end
    [~,minInd] = min(abs(facetToQptDists),[],3);
end
closestTriDistance = facetToQptDists(minInd);

function closestTriDistance = minFacetToQptsDistance(facetToQptDists)
% As above, but facetToQptDists is an Mpts-by-1-by-Nfacets array.

% The multi-point version is a little more tricky. While below is quite a
% bit slower when the while loop is entered, it is very rarely entered and
% very fast to make just the initial comparison.
[minVals,minInds] = min(abs(facetToQptDists),[],3);
while any(...
        any(abs(bsxfun(@plus,minVals,facetToQptDists))<1e-15,3) & ...
        any(abs(bsxfun(@minus,minVals,facetToQptDists))<1e-15,3))
    maskP = abs(bsxfun(@plus,minVals,facetToQptDists))<1e-15;
    maskN = abs(bsxfun(@minus,minVals,facetToQptDists))<1e-15;
    mustAlterMask = any(maskP,3) & any(maskN,3);
    for i = find(mustAlterMask)'
        facetToQptDists(i,:,maskP(i,:,:) | maskN(i,:,:)) = inf;
    end
    [newMv,newMinInds] = min(abs(facetToQptDists(mustAlterMask,:,:)),[],3);
    minInds(mustAlterMask) = newMinInds(:);
    minVals(mustAlterMask) = newMv(:);
end
% Below is a tiny speedup on basically a sub2ind call.
closestTriDistance = facetToQptDists((minInds-1)*size(facetToQptDists,1) + (1:size(facetToQptDists,1))');

%% Input handling subfunctions
function [facets, qPts, options] = parseInputs(varargin)

% Gather FACES and VERTICES
if isstruct(varargin{1})                        % inpolyhedron(FVstruct, ...)
    if ~all(isfield(varargin{1},{'vertices','faces'}))
        error( 'Structure FV must have "faces" and "vertices" fields' );
    end
    faces = varargin{1}.faces;
    vertices = varargin{1}.vertices;
    varargin(1) = []; % Chomp off the faces/vertices

else                                            % inpolyhedron(FACES, VERTICES, ...)
    faces = varargin{1};
    vertices = varargin{2};
    varargin(1:2) = []; % Chomp off the faces/vertices
end

% Unpack the faces/vertices into [3-by-3-by-N] facets. It's better to
% perform this now and have FACETS only in memory in the main program,
% rather than FACETS, FACES and VERTICES
facets = vertices';
facets = permute(reshape(facets(:,faces'), 3, 3, []),[2 1 3]);

% Extract query points
if length(varargin)<2 || ischar(varargin{2})    % inpolyhedron(F, V, [x(:) y(:) z(:)], ...)
    qPts = varargin{1};
    varargin(1) = []; % Chomp off the query points
else                                            % inpolyhedron(F, V, xVec, yVec, zVec, ...)
    qPts = varargin(1:3);
    % Chomp off the query points and tell the world that it's gridded input.
    varargin(1:3) = [];
    varargin = [varargin {'griddedInput',true}];
end

% Extract configurable options
options = parseOptions(varargin{:});

% Check if face normals are unified
if options.testNormals
    options.normalsAreUnified = checkNormalUnification(faces);
end

function options = parseOptions(varargin)
IP = inputParser;
IP.addParamValue('gridsize',[], @(x)isscalar(x) && isnumeric(x))
IP.addParamValue('tol', 0, @(x)isscalar(x) && isnumeric(x))
IP.addParamValue('tol_ang', 1e-5, @(x)isscalar(x) && isnumeric(x))
IP.addParamValue('facenormals',[]);
IP.addParamValue('flipnormals',false);
IP.addParamValue('griddedInput',false);
IP.addParamValue('testNormals',false);
IP.parse(varargin{:});
options = IP.Results;


function [varargout]=tricontf(varargin);
% TRICONT  Filled contours data on a triangular mesh
%     [CS,h]=TRICONTF(X,Y,M,Z) takes the mesh specified by points
%     (X,Y), with heights Z, and the  Nx3 triangulation M (where
%     each row gives the indexes into X/Y vectors of the triangle
%     corners), and make filled contours.
%
%     TRICONTF(...,LEVELS) fills contours at the specified levels.
%
%
%     TRICONTF(...,LINESPEC) takes line style and colour
%     parameters for the lines.
%
%     [CS,h] are the inputs needed by CLABEL, but if you want
%     labelled contours you are better off doing something like
%
%
%     [CS,h]=TRICONTF(...);
%     set(h,'edgecolor','none');
%     hold on;
%     [CS,h]=TRICONT(...);
%     hold off
%     clabel(CS,h)
%
%     since the CS and h returned by TRICONTF contain boundary
%     information which is not usually required for labelling.
%
%     The advantage of using TRICONTF over the GRIDDATA method is the
%     the triangulation is already specified (and may be non-convex
%     as well as containing holes), and the contours are also exact
%     on the triangulation (no weird boundary jaggies). However,
%     LINEAR finite elements are assumed.
%
%     Note - unlike contourf, TRICONTF will not
%     work 'properly' with NaN values in Z - if you
%     have bad data remove those triangles from M!
%
%     See also TRICONT
%
%     Rich Pawlowicz (rpawlowicz@eos.ubc.ca)  March/2013
%

% Copy a bunch of lines from contourf
error(nargchk(4,9,nargin,'struct'));
[cax,args,nargs] = axescheck(varargin{:});

cax = newplot(cax);

% Check for empty arguments.
for i = 1:nargs
  if isempty(args{i})
    error('MATLAB:contourf:EmptyInput','Input argument is empty');
  end
end

% Trim off the last arg if it's a string (line_spec).
nin = nargs;
if ischar(args{end})
  [lin,col,mark,msg] = colstyle(args{end}); %#ok
  if ~isempty(msg), error(msg); end %#ok
  nin = nin - 1;
else
  lin = '';
  col = '';
end

% Closed contours are made up of a) contours through the
% interior, and b) curves around the grid boundaries (there
% can be interior "islands" in an FEM mesh too).  The
% general strategy is to get a bunch of line segments, and
% then join them into continuous curves.

% First, get all the interior contours (this is pretty fast)
% but don't draw anything yet; this also does some checking
% on M to make sure triangles are oriented.

[Xp,Yp,M,Zp,nv,CS,xx,yy,zz]=tricont(args{1:nin});
nCS=[find(isnan(CS(2,:))) size(CS,2)+1];
lCS=CS(1,nCS(1:end-1));

% Don't fill contours below the lowest level specified in nv.
% To fill all contours, specify a value of nv lower than the
% minimum of the surface.
i = find(isfinite(Zp));
minz = min(Zp(i));
maxz = max(Zp(i));
draw_min=0;
if any(nv <= minz),
  draw_min=1;
end

% Second step - get all the boundary curves.

BS=findboundary(M);
iBS=find(isnan(BS));

% Third step:
% Once we have interior and boundary curves, join *them* together.
% However, we have to have separate boundary curves for every
% level, because we need only the parts of the boundary where the
% interior is higher than that level.

fCS=NaN(2,size(CS,2)+size(BS,2));iCS=1;
ncurves = 0;
I = [];
Area=[];levs=[];

for l=1:length(nv),   % For each level

  lvlBS=NaN(2,length(CS)+length(BS)+50);jBS=1;

  iCSlvl=find(lCS==nv(l));
  if any(iCSlvl),

     % Create a structure with all the lines at level
     % nv(k). First add the interior contours

     for k=1:length(iCSlvl),
       nseg=nCS(iCSlvl(k)+1)-nCS(iCSlvl(k))-1;
       level=CS(1,nCS(iCSlvl(k)));
       xseg=CS(1,nCS(iCSlvl(k))+[1:nseg]);
       yseg=CS(2,nCS(iCSlvl(k))+[1:nseg]);
       lvlBS(:,jBS+[0:nseg])=[ nv(l) xseg ; NaN yseg ];
       jBS=jBS+1+nseg;
     end;
  end;

     % Now look through all the boundaries, and for each boundary segment
     % take only the parts that are higher than nv(l). In general we will
     % have to interpolate to get the first and last position of the curve,
     % except when it it as the beginning or end of a boundary curve.

     for k=1:length(iBS)-1,
       Bseg=BS(iBS(k)+1:iBS(k+1)-1);
       id=Zp(Bseg)>nv(l);

       segstart=find(diff(id)== 1)+1;
       if id(1)==1,segstart=[1;segstart]; end;
       segend=  find(diff(id)==-1);
       if id(end)==1,segend=[segend;length(Bseg)]; end;

       for m=1:length(segstart),
	 if segstart(m)==1,  % First point is above...
	   xstart=[];
	   ystart=[];
	 else                % ...otherwise interpolate it
	   xstart=(Xp(Bseg(segstart(m)))*(nv(l)-Zp(Bseg(segstart(m)-1))) + Xp(Bseg(segstart(m)-1))*(Zp(Bseg(segstart(m)))-nv(l)) )./(Zp(Bseg(segstart(m)))-Zp(Bseg(segstart(m)-1)));
	   ystart=(Yp(Bseg(segstart(m)))*(nv(l)-Zp(Bseg(segstart(m)-1))) + Yp(Bseg(segstart(m)-1))*(Zp(Bseg(segstart(m)))-nv(l)) )./(Zp(Bseg(segstart(m)))-Zp(Bseg(segstart(m)-1)));
	 end;
	 if segend(m)==length(Bseg),  % Last point is above....
	   xend=[];
	   yend=[];
	 else;               % ...otherwise interpolate it.
	   xend=(Xp(Bseg(segend(m)+1))*(nv(l)-Zp(Bseg(segend(m)    ))) + Xp(Bseg(segend(m)    ))*(Zp(Bseg(segend(m)+1))-nv(l)) )./(Zp(Bseg(segend(m)+1))-Zp(Bseg(segend(m)    ))) ;
	   yend=(Yp(Bseg(segend(m)+1))*(nv(l)-Zp(Bseg(segend(m)    ))) + Yp(Bseg(segend(m)    ))*(Zp(Bseg(segend(m)+1))-nv(l)) )./(Zp(Bseg(segend(m)+1))-Zp(Bseg(segend(m)    )));
	 end;
         xseg=[ xstart , Xp(Bseg(segstart(m):segend(m)))' , xend ];
         yseg=[ ystart , Yp(Bseg(segstart(m):segend(m)))' , yend ];
	 lvlBS(:,jBS+[0:length(xseg)])=[ nv(l) xseg ; NaN yseg ];
         jBS=jBS+1+length(xseg);
        end;
      end;
      lvlBS(:,jBS+1:end)=[];

      ii=find(isnan(lvlBS(2,:)));

      % Now, apply the line joing algorithm *again* to put the different curves together
      % into a bunch of closed curves.

      % Begin and end of all line segments
      x2=[lvlBS(1,ii(1:end-1)+1) ; lvlBS(1,ii(2:end)-1) ];
      y2=[lvlBS(2,ii(1:end-1)+1) ; lvlBS(2,ii(2:end)-1) ];

      iZ=ones(1,size(x2,2));
      iN=1;
      iZ(iN)=0;
      seg=iN;
      rev1=1;rev2=2;  % keeps track of which direction to add points
       while any(iN),

	% Any line segments with the same endpoint that aren't used?
	% Use == for real numbers because the end points are calculated using
	% the same formula and hence should be exactly the same
	iN=find( x2(rev1,:)==x2(rev2,iN) & y2(rev1,:)==y2(rev2,iN) & iZ,1);

	if any(iN),  % If yes...
	  seg=[seg iN];  % Add it to the segment list...
	  iZ(iN)=0;      % ...and mark it used
	else         % If none...
	  if rev1==1;  % If we haven't searched backwards yet
            iN=seg(1);  % Take the other end of the lines
            seg=seg(end:-1:1); % Reverse the segment list
            rev1=2;rev2=1;
	  else         % we *have* searched backwards, and it really is the end
	    xseg=[];yseg=[];
	    for k=1:length(seg),
	      xseg=[lvlBS(1,ii(seg(k))+1:ii(seg(k)+1)-1) xseg];
	      yseg=[lvlBS(2,ii(seg(k))+1:ii(seg(k)+1)-1) yseg];
	    end;

	    fCS(:,iCS)=[nv(l);length(xseg)];
	    fCS(:,iCS+[1:length(xseg)])=[xseg ;yseg];

            % need these stats later
	    ncurves = ncurves + 1;
            levs(ncurves)=nv(l);
            I(ncurves) = iCS;
            Area(ncurves)=sum( diff(xseg).*(yseg(1:end-1)+yseg(2:end))/2 );

	    iCS=iCS+1+length(xseg);

            iN=find(iZ,1); % ...and go find the next unused segment
	    seg=iN;
            iZ(iN)=0;
            rev1=1;rev2=2;   % ...and search forward.
	  end;
	end;
      end;

end;
fCS(:,iCS:end)=[];

% OK, now we have all the closed curves formed.

% Plot patches in order of decreasing size. This makes sure that
% all the levels get drawn, not matter if we are going up a hill or
% down into a hole. When going down we shift levels though, you can
% tell whether we are going up or down by checking the sign of the
% area (since curves are oriented so that the high side is always
% the same side). Lowest curve is largest and encloses higher data
% always.

% The areas of 'interior holes' will be the same at all
% levels, as will any levels that go all around the outside (which will
% happen if there are interior valleys, which causes a problem (this
% didn't happen in contourf because boundary-finding was
% done differently) so we use sortrows to make sure the highest level
% gets drawn on top for positive areas (i.e. hills), but the lowest
% level gets drawn on top for NEGATIVE areas (holes).

%[FA,IA]=sort(-abs(Area));
[FA,IA]=sortrows([-abs(Area)' (sign(Area).*levs)']);

% below here code is basically identical to contourf

if ~ishold(cax),
    view(cax,2);
    set(cax,'Box','on','Layer','top');
    grid(cax,'off')
end

fig = ancestor(cax,'figure');
H=[];

% This is the colour for holes
if ~ischar(get(cax,'color'))
  bg = get(cax,'color');
else
  bg = get(fig,'color');
end

if isempty(col)
  edgec = get(fig,'defaultsurfaceedgecolor');
else
  edgec = col;
end
if isempty(lin)
  edgestyle = get(fig,'defaultpatchlinestyle');
else
  edgestyle = lin;
end

for jj=IA',
  nl=fCS(2,I(jj));
  lev=fCS(1,I(jj));
  if (lev ~= minz || draw_min ),
    xp=fCS(1,I(jj)+(1:nl));
    yp=fCS(2,I(jj)+(1:nl));
    clev = lev;           % color for filled region above this level
    if (sign(Area(jj)) ~=sign(Area(IA(1))) ),
      kk=find(nv==lev);
      kk0 = 1 + sum(nv<=minz) * (~draw_min);
      if (kk > kk0)
        clev=nv(kk-1);    % in valley, use color for lower level
      elseif (kk == kk0)
        clev=NaN;
      else
        clev=NaN;         % missing data section
        lev=NaN;
      end
    end

    if (isfinite(clev)),
      H=[H;patch(xp,yp,clev,'facecolor','flat','edgecolor',edgec, ...
              'linestyle',edgestyle,'userdata',lev,'parent',cax)];
    else
      H=[H;patch(xp,yp,clev,'facecolor',bg,'edgecolor',edgec, ...
              'linestyle',edgestyle,'userdata',fCS(1,I(jj)),'parent',cax)];
    end
  end;
end;

% Contourf strips out bnoundary points but I can't be bothered - if you
% want labelled contours you should really follow a tricontf call with
% a tricont call.

varargout={fCS,H};

%-----------------------------------------------------------------
function BS=findboundary(M);
% Finds the curves that make up the
% boundary of a triangulation
%
% First, examining ALL 2 point line segments in M
% to see if they are contained in one or
% two triangles in the mesh! This is slow.
%
lenM=size(M,1);

%Bseg=NaN(2,lenM);iB=1;
%
%pat=[1 2;  % Edge indices in order when triangles
%     2 3;  % are arranged CW
%     3 1];
%for k=1:lenM,
% for l=1:3,
%    [id,jid]  =find( (      M==M(k,pat(l,1))) );
%    [id2,jid2]=find( (M(id,:)==M(k,pat(l,2))) );
%    if length(id2)==1,
%      Bseg(:,iB)=M(k,pat(l,:))';
%      iB=iB+1;
%    end;
% end;
%end;
%Bseg(:,iB:end)=[];

% Try another way - this works MUCH MUCH faster
% (can be many orders of magnitude faster for large
% problems!), but will fail if the mesh is screwy
% (i.e. edges do not appear ONLY one or two times
% in the whole list of triangles()

% Put all the triangle edges in a list
alledge=[ M(:,[1 2]) ; M(:,[2 3]) ; M(:,[3 1]) ];

% put rows in increasing numerical order, then sort
% so the rows will either be unique edges, or pairs
% of edges
[sortedge,I]=sortrows(sort(alledge,2));
% Now identify the boundaries between pairs and
% unique edges - the diff will == 0 between
% pairs of identical edges
chges=[1;any(diff(sortedge)~=0,2);1];
% so find places where we have 1s one after another,
% since this marks a transition through a unique
% edge
ibdy=find(diff(chges)==0);
% ...and now get the original rows, before the
% numerical sorting
Bseg=alledge(I(ibdy),:)'; % back to original list

% Now I join all the individual boundary segments
% into a smaller number of actual curves using the
% follow foward/follow backward algorithm, all curves
% should traverse with the interior to their right.

BS=NaN(1,length(Bseg)+50);iBS=1;  % preallocate
iZ=ones(1,size(Bseg,2));  % flag to tell me if a line segment has not been added
                        % to a line
iN=1;
iZ(iN)=0;
seg=iN;
rev1=1;rev2=2;  % keeps track of which direction to add points
                % (differs if I am going forward or backward from a point)
while any(iN),

  % Any line segments with the same endpoint that aren't used?
  iN=find( Bseg(rev1,:)==Bseg(rev2,iN) & iZ ,1);

  if any(iN),  % If yes...
    seg=[seg iN];  % Add it to the segment list...
    iZ(iN)=0;      % ...and mark it used
  else         % If none...
    if rev1==1;  % If we haven't searched backwards yet
      iN=seg(1);  % Take the other end of the lines
    %  iZ(iN)=0;
      seg=seg(end:-1:1); % Reverse the segment list
      rev1=2;rev2=1;
    else         % we *have* searched backwards, and it really is the end
      xseg=[Bseg(2,seg),Bseg(1,seg(end))];  % make up the line

      BS(iBS+[0:length(xseg)])=[NaN xseg(end:-1:1) ];
      iBS=iBS+1+length(xseg);

      iN=find(iZ,1); % ...and go find the next unused segment
      seg=iN;
      iZ(iN)=0;
      rev1=1;rev2=2;   % ...and search forward.
    end;
  end;
end;
BS(iBS+1:end)=[];


function [lambda] = spm_mci_adjoint_int (U,P,M,V,djdx,tol)
% Integrate adjoint equation
% FORMAT [lambda] = spm_mci_adjoint_int (U,P,M,V,djdx,tol)
%
% U         Inputs
% P         Parameters
% M         Model structure
% V         states
% djdx      derivative of log likelihood wrt states
% tol       tolerances
%
% lambda    adjoint parameters, at times M.t
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny and Biswa Sengupta
% $Id: spm_mci_adjoint_int.m 6548 2015-09-11 12:39:47Z will $

init_t=M.t(1);
final_t=M.T;

tol.abs=1e-3;
tol.rel=1e-5;

% parameters for the intergrator
options = odeset('AbsTol',tol.abs,'RelTol',tol.rel);

% integrate adjoint equation backwards
lam_init=zeros(M.n,1);
[TT, ld] = ode15s(@(t,lam) integrate_adjoints(t,lam,djdx,V,U,P,M),[final_t init_t], lam_init, options);

% interpolate to M.t
% (Note, interp1q is not accurate enough here
% as TT typically has less resolution than M.t)
lambda = interp1(TT,ld,M.t,'spline');

end

%--------------------------------------------------------------------------
function DlDt = integrate_adjoints(t,lam,djdx,V,U,P,M)

Nx=M.n;
DlDt = zeros(Nx,1);

% Interpolate state and dj/dx to current time point
for d=1:Nx,
    local_djdx(d)=interp1q(M.t,djdx(:,d),t);
    v(d)=interp1q(M.t,V(:,d),t);
end

% Find nearest time point for which we have pre-computed input
if isempty(U)
    ut=[];
else
    [tmp,ind]=min(abs(t-M.t));
    ut=U(:,ind);
end

% Evaluate state Jacobian
if isfield(M,'dfdx')
    Fx = feval(M.dfdx,v(:),ut,P,M);
else
    Fx = spm_diff(M.f,v(:),ut,P,M,1);
end

DlDt=local_djdx-lam'*Fx;
DlDt=DlDt';

end


%   TTeMPS Toolbox.
%   Michael Steinlechner, 2013-2016
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt
function [X, residuum, cost, times] = alsLinsolve_fast( L, F, X, opts )

t_start = tic();
% set default opts
if ~exist( 'opts', 'var');       opts = struct();       end
if ~isfield( opts, 'nSweeps');   opts.nSweeps = 4;      end
if ~isfield( opts, 'solver');    opts.solver = 'pcg';   end

d = X.order;
n = X.size;

normF = norm(F);
g = apply(L, X) - F;
cost = cost_function_res( X, g );
residuum = norm( g ) / normF;
times = toc(t_start);

X = orthogonalize(X, 1);
for sweep = 1:opts.nSweeps
    % ====================================================================
    % LEFT-TO-RIGHT SWEEP
    % ====================================================================
    disp( ['STARTING SWEEP ', num2str(sweep), ' from left to right'] )
    disp( '===========================================================')
    for idx = 1:d-1
        disp( ['Current core: ', num2str(idx)] )

        Fi = contract( X, F, idx );
        sz = [X.rank(idx), X.size(idx), X.rank(idx+1)];

        if strcmpi( opts.solver, 'direct' )
            % if system very small
            Li = contract( L, X, idx );
            Ui = Li \ Fi(:);
            X.U{idx} = reshape( Ui, sz );

        elseif strcmpi( opts.solver, 'pcg' )

            [left, right] = Afun_prepare( L, X, idx );
            [B2, V, E] =  prepare_precond( L.L0, X, idx );

            Ui = pcg( @(y) Afun( L, y, idx, sz, left, right), ...
                     Fi(:), ...
                     1e-10, 1000, ...
                     @(y) apply_precond( B2, V, E, y, sz ), [],...
                     X.U{idx}(:) );

            X.U{idx} = reshape( Ui, sz );

        elseif strcmpi( opts.solver, 'diag' )
            X.U{idx} = solve_inner( L.L0, X, Fi, idx );

        else
            error( 'Unknown opts.solver type. Use either ''direct'', ''pcg'' (default) or ''diag''.' )
        end

        X = orth_at( X, idx, 'left', true );

        g = apply(L, X) - F;
        residuum = [residuum; norm( g ) / normF];
        cost = [cost; cost_function_res( X, g )];
        times = [times; toc(t_start)];
    end

    % ====================================================================
    % RIGHT-TO-LEFT
    % ====================================================================
    disp( 'Starting right-to-left half-sweep:')
    for idx = d:-1:2
        disp( ['Current core: ', num2str(idx)] )

        Fi = contract( X, F, idx );
        sz = [X.rank(idx), X.size(idx), X.rank(idx+1)];

        if strcmpi( opts.solver, 'direct' )
            % if system very small
            Li = contract( L, X, idx );
            Ui = Li \ Fi(:);
            X.U{idx} = reshape( Ui, sz );

        elseif strcmpi( opts.solver, 'pcg' )

            [left, right] = Afun_prepare( L, X, idx );
            [B2, V, E] =  prepare_precond( L.L0, X, idx );

            Ui = pcg( @(y) Afun( L, y, idx, sz, left, right), ...
                     Fi(:), ...
                     1e-10, 1000, ...
                     @(y) apply_precond( B2, V, E, y, sz ), [],...
                     X.U{idx}(:) );

            X.U{idx} = reshape( Ui, sz );

        elseif strcmpi( opts.solver, 'diag' )
            X.U{idx} = solve_inner( L.L0, X, Fi, idx );

        else
            error( 'Unknown opts.solver type. Use either ''direct'', ''pcg'' (default) or ''diag''.' )
        end

        X = orth_at( X, idx, 'right', true );

        g = apply(L, X) - F;
        residuum = [residuum; norm( g ) / normF];
        cost = [cost; cost_function_res( X, g )];
        times = [times; toc(t_start)];
    end

end

end

function res = cost_function( L, X, F )
res = 0.5*innerprod( X, apply(L, X) ) - innerprod( X, F );
end

function res = cost_function_res( X, res )
res = 0.5*innerprod( X, res );
end

function [left, right] = Afun_prepare( A, x, idx )
    y = A.apply(x);
    if idx == 1
        right = innerprod( x, y, 'RL', idx+1 );
        left = [];
    elseif idx == x.order
        left = innerprod( x, y, 'LR', idx-1 );
        right = [];
    else
        left = innerprod( x, y, 'LR', idx-1 );
        right = innerprod( x, y, 'RL', idx+1 );
    end
end

function res = Afun( A, U, idx, sz, left, right )

    V = reshape( U, sz );
    V = A.apply( V, idx );

    if idx == 1
        tmp = tensorprod_ttemps( V, right, 3 );
    elseif idx == A.order
        tmp = tensorprod_ttemps( V, left, 1 );
    else
        tmp = tensorprod_ttemps( V, right, 3);
        tmp = tensorprod_ttemps( tmp, left, 1);
    end

    res = tmp(:);
end

%function res = apply_local_precond( A, U, sz, expB)
%
%    p = size(U, 2);
%
%    x = reshape( U, [sz, p] );
%    res = zeros( [sz, p] );
%
%    for i = 1:size( expB, 1)
%        tmp = reshape( x, [sz(1), sz(2)*sz(3)*p] );
%        tmp = reshape( expB{1,i}*tmp, [sz(1), sz(2), sz(3), p] );
%
%        tmp = reshape( permute( tmp, [2 1 3 4] ), [sz(2), sz(1)*sz(3)*p] );
%        tmp = ipermute( reshape( expB{2,i}*tmp, [sz(2), sz(1), sz(3), p] ), [2 1 3 4] );
%
%        tmp = reshape( permute( tmp, [3 1 2 4] ), [sz(3), sz(1)*sz(2)*p] );
%        tmp = ipermute( reshape( expB{3,i}*tmp, [sz(3), sz(1), sz(2), p] ), [3 1 2 4] );
%
%        res = res + tmp;
%    end
%    res = reshape( res, [prod(sz), p] );
%
%end

function res = solve_inner( L0, X, Fi, idx )
    n = size(L0, 1);
    rl = X.rank(idx);
    rr = X.rank(idx+1);

    B1 = zeros( rl );
    % calculate B1 part:
    for i = 1:idx-1
        % apply L to the i'th core
        tmp = X;
        tmp.U{i} = tensorprod_ttemps( tmp.U{i}, L0, 2 );
        B1 = B1 + innerprod( X, tmp, 'LR', idx-1);
    end

    % calculate B2 part:
    B2 = L0;

    B3 = zeros( rr );
    % calculate B3 part:
    for i = idx+1:X.order
        tmp = X;
        tmp.U{i} = tensorprod_ttemps( tmp.U{i}, L0, 2 );
        B3 = B3 + innerprod( X, tmp, 'RL', idx+1);
    end

    [V,E] = eig( kron( eye(rr), B1 ) + kron( B3, eye(rl) ) );
    E = diag(E);

    rhs = matricize( Fi, 2 ) * V;
    Y = zeros(size(rhs));
    for i=1:length(E)
        Y(:,i) = (B2 + E(i)*speye(n)) \ rhs(:,i);
    end
    res = tensorize( Y*V', 2, [rl, n, rr] );
end

function [B2, V, E] = prepare_precond( L0, X, idx )
    n = size(L0, 1);
    rl = X.rank(idx);
    rr = X.rank(idx+1);

    B1 = zeros( rl );
    % calculate B1 part:
    for i = 1:idx-1
        % apply L to the i'th core
        tmp = X;
        tmp.U{i} = tensorprod_ttemps( tmp.U{i}, L0, 2 );
        B1 = B1 + innerprod( X, tmp, 'LR', idx-1);
    end

    % calculate B2 part:
    B2 = L0;

    B3 = zeros( rr );
    % calculate B3 part:
    for i = idx+1:X.order
        tmp = X;
        tmp.U{i} = tensorprod_ttemps( tmp.U{i}, L0, 2 );
        B3 = B3 + innerprod( X, tmp, 'RL', idx+1);
    end

    [V,E] = eig( kron( eye(rr), B1 ) + kron( B3, eye(rl) ) );
    E = diag(E);
end

function res = apply_precond( B2, V, E, rhs, sz )
    n = size(B2, 1);
    rhs = reshape( rhs, sz );
    rhs = matricize( rhs, 2 ) * V;
    Y = zeros(size(rhs));
    for i=1:length(E)
        Y(:,i) = (B2 + E(i)*speye(n)) \ rhs(:,i);
    end
    res = tensorize( Y*V', 2, sz );
    res = res(:);
end


function y = acoth(x)
%ACOTH        Implements  acoth(x)  for intervals
%
%   y = acoth(x)
%
%interval standard function implementation
%

% written  10/16/98     S.M. Rump
% modified 12/30/98     S.M. Rump  improved speed and use atanh
% modified 08/31/99     S.M. Rump  complex allowed, sparse input,
%                                  major revision, improved accuracy near 1
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 12/04/05     S.M. Rump  'realstdfctsexcptnignore' added and
%                                     some improvements, extreme values
%                                     for approximate part
% modified 09/06/07     S.M. Rump  approximate std fcts removed, exceptional arguments
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/20/08     S.M. Rump  check for zero omitted
% modified 10/18/08     S.M. Rump  StdFctsException ignore/NaN
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

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

  if x.complex
    if issparse(x.mid)
      x.mid = full(x.mid);
      x.rad = full(x.rad);
    end
    y = atanh( 1./x );
    if rndold
      setround(rndold)
    end
    return
  end

  if issparse(x.inf)
    x.inf = full(x.inf);
    x.sup = full(x.sup);
  end
  % input x real and full
  % real range of definition:  [-inf,-1] and [1,inf]
  % take care for intersection( x , (-1,1) ) nonempty
  INTLAB_STDFCTS_EXCPTN = getappdata(0,'INTLAB_STDFCTS_EXCPTN');
  Index1 = ( abs(x.inf)<1 );           % (partially) exceptional indices
  Index2 = ( abs(x.sup)<1 );
  if ~isempty(find(Index1)) | ~isempty(find(Index2))   % handle input out-of-range
    if INTLAB_STDFCTS_EXCPTN<=1 % out-of-range input handled as complex
      if INTLAB_STDFCTS_EXCPTN==1
        warning('ACOTH: Real interval input out of range changed to be complex')
      end
      y = x;
      index = Index1 | Index2;
      %VVVV  y(index) = atanh(cintval(1./x(index)));
      s.type = '()'; s.subs = {index}; y = subsasgn(y,s,atanh(cintval(1./subsref(x,s))));
      %AAAA  Matlab bug fix
      index = ~index;
      if any(index(:))
        %VVVV  y(index) = atanh(1./x(index));
        s.type = '()'; s.subs = {index}; y = subsasgn(y,s,atanh(1./subsref(x,s)));
        %AAAA  Matlab bug fix
      end
      index = ( x==0 );
      if any(index(:))
        INTLAB_STDFCTS_PI = getappdata(0,'INTLAB_STDFCTS_PI');
        iPI2 = intval(j*INTLAB_STDFCTS_PI.PI2MID,INTLAB_STDFCTS_PI.PI2RAD,'midrad');
        %VVVV  y(index) = iPI2;
        s.type = '()'; s.subs = {index}; y = subsasgn(y,s,iPI2);
        %AAAA  Matlab bug fix
      end
      if rndold
        setround(rndold)
      end
      return
    end
    setappdata(0,'INTLAB_STDFCTS_EXCPTN_',1);
    if INTLAB_STDFCTS_EXCPTN==3    % ignore input out of range (ignore-mode)
      x.inf(Index1) = 1;                  % completely exceptional indices treated below
      x.sup(Index2) = -1;
      Index = ( Index1 & Index2 );        % completely exceptional indices
    end
  else
    Index = [];                           % make sure Index is not undefined
  end

  % input x real and full
  y = x;
  wng = warning;                         % get current warning mode
  warning off

  % treat positive intervals
  index1 = ( x.inf>0 );
  if any(index1(:))
    y.inf(index1) = acoth_pos(x.sup(index1),-1);
    y.sup(index1) = acoth_pos(x.inf(index1),1);
  end

  % treat negative intervals
  index1 = ( x.sup<0 );
  if any(index1(:))
    y.inf(index1) = - acoth_pos(-x.sup(index1),1);
    y.sup(index1) = - acoth_pos(-x.inf(index1),-1);
  end

  if INTLAB_STDFCTS_EXCPTN==3      % ignore input out of range (ignore-mode)
    if ~isempty(find(Index))              % completely exceptional arguments to NaN
      y.inf(Index) = NaN;
      y.sup(Index) = NaN;
    end
  else                                    % any input out of range to NaN (NaN-mode)
    Index = Index1 | Index2;
    if ~isempty(find(Index))              % exceptional arguments to NaN
      y.inf(Index) = NaN;
      y.sup(Index) = NaN;
    end
  end

  setround(rndold)
  warning(wng)                           % restore warning mode

function y = acoth_pos(x,rnd)
% local acoth for double vector x>=1 with rounding corresponding to rnd
%

  y = x;

  index = ( x<4 );
  if any(index(:))             % 1 <= x <= 4
    setround(rnd)              % acoth(x) = log( 1 + 2/(x-1) )
    e = x(index) - 1;          % e w/o rounding error
    e = 1 + 2./e;
    y(index) = log_rnd( e , rnd ) / 2;
    y(x==1) = inf;
  end

  index = ~index;              % x >= 4, difference x-1 not necessarily exact
  if any(index(:))             % acoth(x) = atanh(1/x)
    setround(rnd)
    e = 1./x(index);
    y(index) = atanh_pos( e , rnd );
  end


%**************************************************************************
% Q Learning applied to Cart-Pole balancing problem.
% The environment of the learning system is a black box, from which it has
% several lines and a reinforcement line. Its task is to learn to give
% responses which maximize the scalar signals on its reinforcement line.
% The Q-value update is the following, if the system takes action a from
%   state s at time t, and arrives at state ss with feedback r at time t+1:
%
%   Q(t+1, s, a) = Q(t, s, a)
%                  + alpha (r + gamma max_{b}Q(t,ss, b) - Q(t, s, a))

% get_box:  Given the current state, returns a number from 1 to 162
%           designating the region of the state space encompassing the current
%           state.
%           Returns a value of -1 if a failure state is encountered.

% cart_pole: The cart and pole dynamics; given the Force and
%            current state, estimates next state

%Code written by: Savinay Nagendra
%email id:        nagsavi17@gmail.com
%**************************************************************************
clc;
clear all;
close all;
% Initialization

NUM_BOXES = 163;
ALPHA = 0.4;             % Learning rate parameter
GAMMA = 0.999;           % Discount factor for future reinf
Q = zeros(NUM_BOXES,2);  % State-Action Values
action = [30 -30];
MAX_FAILURES = 1000;
MAX_STEPS = 150000;
epsilon = 0;
steps = 0;
failures = 0;
thetaPlot = 0;
xPlot = 0;
%Pendulum state initialization
theta = 0;
thetaDot = 0;
x = 0;
xDot = 0;
box = getBox4(theta,thetaDot,x,xDot);

while(steps<=MAX_STEPS && failures<+MAX_FAILURES)
    steps = steps + 1;

    % choose either explore or exploit
    if(rand>epsilon)       % exploit
        [~,actionMax] = max(Q(box,:));
        currentAction = action(actionMax);
    else                   % explore
        currentAction = datasample(action,1);
    end

    actionIndex = find(action == currentAction); % index of chosen action
    %Apply action to the simulated cart pole
    [thetaNext,thetaDotNext,thetaacc,xNext,xDotNext] = cart_pole2(currentAction,theta,thetaDot,x,xDot);
    %Get box of state space containing the resulting state
    thetaPlot(end + 1) = thetaNext;
    xPlot(end + 1) = xNext;
    newBox = getBox4(thetaNext,thetaDotNext,xNext,xDotNext);
    theta = thetaNext;
    thetaDot = thetaDotNext;
    x = xNext;
    xDot = xDotNext;
    if(newBox==163)
        r = -1;
        Q(newBox,:) = 0;
        figure(2);
        plot((1:length(thetaPlot)),thetaPlot,'-ob');
        figure(3);
        plot((1:length(xPlot)),xPlot,'-og');

        thetaPlot = 0;
        xPlot = 0;
        theta = 0;
        thetaDot = 0;
        x = 0;
        xDot = 0;
        newBox = getBox4(theta,thetaDot,x,xDot);
        failures = failures + 1;
        fprintf('Trial %d was %d steps. \n',failures,steps);
        figure(1);
        plot(failures,steps,'-or');
        hold on;
        steps = 0;
    else
        r = 0;
    end
    Q(box,actionIndex) = Q(box,actionIndex) + ALPHA*(r + GAMMA*max(Q(newBox,:)) - Q(box,actionIndex));
    box = newBox;
end
if(failures == MAX_FAILURES)
    fprintf('Pole not balanced. Stopping after %d failures.',failures);
else
    fprintf('Pole balanced successfully for at least %d steps\n', steps);
    figure(1);
    plot(failures+1,steps,'-or');
    hold on;
    figure(2);
    plot((1:length(thetaPlot)),thetaPlot,'-ob');
    figure(3);
    plot((1:length(xPlot)),xPlot,'-og');
    figure(4);
    plot((1:301),thetaPlot(1:301),'-ob');
    hold on;
    figure(5);
    plot((1:301),xPlot(1:301),'-og');
    hold on;
end