function [ ci, bc, bcerr, abserr ] = findFEMCell( m, p, hint )
%[ ci, bc, bcerr, abserr ] = findFEMCell( m, p, hint )
%   Find the FEM cell which p is closest to lying inside, and the barycentric
%   coordinates of p in that cell.  err is the difference between the
%   resulting point and p.  hint is a list of FEM cells to try first.  If p
%   does not appear to be in any of them (i.e. its barycentric coordinates
%   are not all positive), then all cells sharing any vertexes with the
%   hint cells are tried.

    global findFEMCellStats
    findFEMCellStats(1) = findFEMCellStats(1) + 1;
    statsindex = 2;

    if (nargin < 3) || isempty(hint);
        [mindsq,mini] = min( sum( (m.nodes - repmat( p, size(m.nodes,1), 1 )).^2, 2 ) );
        if ~isempty(mini)
            nce = m.nodecelledges{mini(1)};
            hint = nce(2,:);
            hint = hint(hint>0);
            for i=2:length(mini)
                nce = m.nodecelledges{mini(i)};
                morehints = nce(2,:);
                morehints = morehints(morehints>0);
                hint = [ hint, morehints ];
            end
%             fprintf( 1, '%s: starting from closest FEs:', mfilename );
%             fprintf( 1, ' %d', hint );
%             fprintf( 1, '\n' );
        else
            hint = 1:size(m.tricellvxs,1);
        end
    end
    findFEMCell_BCTOLERANCE = 0.02;
    findFEMCell_ABSTOLERANCE = 0.1 * sqrt(max(m.cellareas(hint)));

    % Try the hint cells.
    [ ci, bc, bcerr, abserr ] = findFEMCell1( m, p, hint );
    attempt = 1;
%    return;
    if (bcerr >= -findFEMCell_BCTOLERANCE) && (abserr <= findFEMCell_ABSTOLERANCE)
      % fprintf( 1, '%s %d: bcerr %f abserr %f\n', mfilename(), attempt, bcerr, abserr );
        findFEMCellStats(statsindex) = findFEMCellStats(statsindex) + 1;
        return;
    end
    statsindex = statsindex+1;

    % Try the hint cells and all the cells that share a vertex with them.
    othercells = cellVxNbs( m, hint );
    if isempty(othercells)
        return;
    end
    [ ci, bc, bcerr, abserr ] = findFEMCell1( m, p, othercells );
    attempt = 2;
%    return;
    if (bcerr >= -findFEMCell_BCTOLERANCE) && (abserr <= findFEMCell_ABSTOLERANCE)
      % fprintf( 1, '%s %d: bcerr %f abserr %f\n', mfilename(), attempt, bcerr, abserr );
        findFEMCellStats(statsindex) = findFEMCellStats(statsindex) + 1;
        return;
    end
    statsindex = statsindex+1;

    bestci = ci;
    bestbc = bc;
    bestbcerr = bcerr;
    bestabserr = abserr;
    bestattempt = attempt;
    
    % Try cells sharing vertexes with cells sharing vertexes with the hint
    % cells.
    seencells = false( size(m.tricellvxs,1), 1 );
    seencells(othercells) = true;
    othercells = cellVxNbs( m, othercells );
    othercells = othercells( ~seencells( othercells ) );
    if isempty(othercells)
        return;
    end
    [ ci, bc, bcerr, abserr ] = findFEMCell1( m, p, othercells );
    attempt = 3;
    if (bcerr >= -findFEMCell_BCTOLERANCE) && (abserr <= findFEMCell_ABSTOLERANCE)
      % fprintf( 1, '%s %d: bcerr %f abserr %f\n', mfilename(), attempt, bcerr, abserr );
        findFEMCellStats(statsindex) = findFEMCellStats(statsindex) + 1;
        return;
    end
    statsindex = statsindex+1;

    if abserr < bestabserr
        bestci = ci;
        bestbc = bc;
        bestbcerr = bcerr;
        bestabserr = abserr;
        bestattempt = attempt;
    end

    seencells(othercells) = true;

    % Try all other cells.
    othercells = find( ~seencells );
    if isempty(othercells)
        return;
    end
    [ ci, bc, bcerr, abserr ] = findFEMCell1( m, p, othercells );
    attempt = 4;
    if (bcerr >= -findFEMCell_BCTOLERANCE) && (abserr <= findFEMCell_ABSTOLERANCE)
      % fprintf( 1, '%s %d: bcerr %f abserr %f\n', mfilename(), attempt, bcerr, abserr );
        findFEMCellStats(statsindex) = findFEMCellStats(statsindex) + 1;
        return;
    end
    statsindex = statsindex+1;

    if abserr < bestabserr
        bestci = ci;
        bestbc = bc;
        bestbcerr = bcerr;
        bestabserr = abserr;
        bestattempt = attempt;
    end

    if false
        [ ci, bc, bcerr, abserr ] = findFEMCell2( m, p, hint );
        attempt = 5;
        if (bcerr >= -findFEMCell_BCTOLERANCE) && (abserr <= findFEMCell_ABSTOLERANCE)
          % fprintf( 1, '%s %d: bcerr %f abserr %f\n', mfilename(), attempt, bcerr, abserr );
            findFEMCellStats(statsindex) = findFEMCellStats(statsindex) + 1;
            return;
        end

        if abserr < bestabserr
            bestci = ci;
            bestbc = bc;
            bestbcerr = bcerr;
            bestabserr = abserr;
            bestattempt = attempt;
        end
    end
    statsindex = statsindex+1;
    
    ci = bestci;
    bc = bestbc;
    bcerr = bestbcerr;
    abserr = bestabserr;
    attempt = bestattempt;

    findFEMCellStats(statsindex) = findFEMCellStats(statsindex) + 1;
    fprintf( 1, '%s (%d,%d): last resort attempt %d ci %d [%.3f %.3f %.3f] bcerr %.8f abserr %.8f.\n', ...
        mfilename(), findFEMCellStats(1), findFEMCellStats(end), ...
        attempt, ci, bc, bcerr, abserr );
end

function nbcis = cellVxNbs( m, cis )
%nbcis = cellVxNbs( m, cis )
%   Find all cells that share at least one vertex with any cell in cis.

    vxs = unique(m.tricellvxs( cis, : ));
    nces = [ m.nodecelledges{vxs} ];
    nbcis = unique( nces(2,:) );
    if nbcis(1)==0
        nbcis = nbcis(2:end);
    end
end

function re = residualError( m, p, ci, bc )
    re = norm(p-bc*m.nodes(m.tricellvxs(ci,:),:));
end

function [ ci, bc, bcerr, abserr ] = findFEMCell1( m, p, cells )
%[ ci, bc, bcerr, abserr ] = findFEMCell1( m, p, cells )
%   Find the FEM cell which p is closest to lying inside, and the barycentric
%   coordinates of p in that cell.  bcerr and abserr are the difference between the
%   resulting point and p, in barycentric coordinate and Euclidean distance
%   respectively.  cells is a list of FEM cells to try.

    if isempty( cells )
        ci = 0;
        bc = [];
        bcerr = 0;
        abserr = 0;
        return;
    end
    
    
    numcells = length(cells);
  % fprintf( 1, '%s: numcells %d\n', mfilename(), numcells );
  % ci = zeros(numcells,1);
    bcs = zeros(numcells,3);
    bcerrs = zeros(numcells,1);
    abserrs = zeros(numcells,1);
    
  % xbcs = zeros(numcells,3);
  % xabserrs = zeros(numcells,1);
  % for i=1:numcells
  %     xbcs(i,:) = cellBaryCoords( m, cells(i), p );
  % end
  % xbcerrs = min( 0, min(xbcs,[],1) );
  % xbcs = normaliseBaryCoords( xbcs );
  % for i=1:numcells
  %     xabserrs(i) = residualError( m, p, cells(i), xbcs(i,:) );
  % end
    for i=1:numcells
        bc = cellBaryCoords( m, cells(i), p );
        bcerrs(i) = min( 0, min(bc) );
        bc = normaliseBaryCoords( bc );
        abserrs(i) = residualError( m, p, cells(i), bc );
        bcs(i,:) = bc;
    end
  % xabserrs-abserrs
    [abserr,i] = min( abserrs );
    ci = cells(i);
    bc = bcs(i,:);
    bcerr = bcerrs(i);
end

function [ ci, bc, bcerr, abserr ] = findFEMCell2( m, p, hint )
    d = zeros(1,length(hint));
    for i = 1:length(hint)
        d(i) = pointPlaneDistance( m.nodes( m.tricellvxs(hint(i),:), :), p );
    end
    [mind,mini] = min(d);
    ci = hint(mini);
    bc = cellBaryCoords( m, ci, p );
    bcerr = min( bc );
    bc = normaliseBaryCoords( bc );
    abserr = residualError( m, p, ci, bc );
    fprintf( 1, 'findFEMCell2( [%.3f %.3f %.3f] ) = ci %d, mind %.3f bc [%.3f %.3f %.3f], bcerr %.3f, abserr %.3f\n', ...
        p, ci, mind, bc, bcerr, abserr );
end
