/*************************************************************************
*  2021 Jérôme Duriez, jerome.duriez@inrae.fr                            *
*  2023 Danny van der Haven, dannyvdhaven@gmail.com                      *
*  This program is free software, see file LICENSE for details.          *
*************************************************************************/

#ifdef YADE_LS_DEM
#include <lib/computational-geometry/MarchingCube.hpp>
#include <lib/high-precision/Constants.hpp>
#include <core/Scene.hpp>
#include <pkg/levelSet/LevelSet.hpp>
#include <pkg/levelSet/ShopLS.hpp>
#include <boost/math/tools/roots.hpp>
#include <preprocessing/dem/Shop.hpp>

namespace yade {
YADE_PLUGIN((LevelSet));
CREATE_LOGGER(LevelSet);

void LevelSet::assignSurfNodes(vector<Vector3r> givenNodes)
{
	surfNodes.clear();
	surfNodes.resize(givenNodes.size());
	minRad = std::numeric_limits<Real>::infinity();
	maxRad = 0;
	Real normNode(-1);
	for (unsigned int idx = 0; idx < givenNodes.size(); idx++) {
		surfNodes[idx] = givenNodes[idx];
		normNode       = surfNodes[idx].norm();
		if (normNode < minRad) minRad = normNode;
		if (normNode > maxRad) maxRad = normNode;
	}
	postProcessNodes();
}

vector<vector<vector<int>>> LevelSet::binarize(bool surface)
{
	vector<vector<vector<int>>> boolGrid; // the 3D grid to return, full of 0 (outside) or 1
	boolGrid.resize(lsGrid->nGP[0]);
	for (int xInd = 0; xInd < lsGrid->nGP[0]; xInd++) {
		boolGrid[xInd].resize(lsGrid->nGP[1]);
		for (int yInd = 0; yInd < lsGrid->nGP[1]; yInd++) {
			boolGrid[xInd][yInd].resize(lsGrid->nGP[2]);
			for (int zInd = 0; zInd < lsGrid->nGP[2]; zInd++) {
				if (distField[xInd][yInd][zInd] == 0) boolGrid[xInd][yInd][zInd] = (surface ? 1 : 0);
				else
					boolGrid[xInd][yInd][zInd] = ((distField[xInd][yInd][zInd] < 0) ? 1 : 0);
			}
		}
	}
	return boolGrid;
}

Vector3r LevelSet::getCenter()
{
	if (!initDone) init();
	return center;
}

// // vector<int> LevelSet::getNodesInCellCube(int i, int j, int k){ Vector3i ?
// // 	return nodesIndicesInCell[Vector3i(i,j,k)];
// // }
Real LevelSet::smearedHeaviside(Real x) const
{
	// Passing smoothly (increasing sine-sort) from y = 0 to 1 when x goes from -1 to 1, see Eq. (3) of Kawamoto2016
	Real ret;
	if (x < -1.) ret = 0;
	else if (x > 1.)
		ret = 1;
	else
		ret = 0.5 * (1.0 + x + sin(Mathr::PI * x) / Mathr::PI);
	return ret;
}

Real LevelSet::volumeInsideThreshold(Real epsilon) const
{
	if (smearCoeff <= 0)
		LOG_WARN("Using volumeInsideThreshold (for surface measurement, probably) with a negative smearCoeff = " << smearCoeff << " is not expected");
	Real vol(0.);                                                // to-be-returned volume which is inside phi = epsilon
	Real phiRef(0.5 * sqrt(3.0) * lsGrid->spacing / smearCoeff); // the reference length for smoothing the Heaviside
	Real volCell(pow(lsGrid->spacing, 3));                       // lsGrid voxel volume
	for (int xIndex = 0; xIndex < lsGrid->nGP[0]; xIndex++) {
		for (int yIndex = 0; yIndex < lsGrid->nGP[1]; yIndex++) {
			for (int zIndex = 0; zIndex < lsGrid->nGP[2]; zIndex++) {
				vol += smearedHeaviside((epsilon - distField[xIndex][yIndex][zIndex]) / phiRef) * volCell;
			}
		}
	}
	return vol;
}

std::vector<Vector3r> LevelSet::rayTrace(const Vector3r& ray, const Real& nodesTol)
{
	// we launch a ray from shape's center (= 0), and go through all grid cells until the end of the grid, looking for ray-surface intersection
	// we return all found intersection points through a C++ class member nodesOnRay (maybe weird but making the method a void-return would prevent direct Python usage)
	nodesOnRay = std::vector<Vector3r>(); // will be the returned quantity, reset to empty vector for each ray, before being recursively populated below
	Vector3r pointP(
	        Vector3r::
	                Zero()), // pointP is always the point where the ray starts from, initialized to 0 instead of center for a better precision of the approach (this may lead to brutal segfaults for non-0-centered shapes)
	        point0;          // while point0 is the lowest corner of the current grid cell
	Vector3i indices(lsGrid->closestCorner(pointP)), // which grid cell do we start from ?
	        move;                                    // to control the move from one cell to another
	std::array<Real, 3>
	        kVal; // points of the ray are of the form pointP + k * ray (k>= 0). Then, this kVal will include the 3 smallest k-values that make the ray exit of the cell, considering in an independent manner the three axis. E.G. for a ray = (1,0,0) starting from point0, kVal = (spacing, infinity, infinity)
	bool    diffSign // true if and only if at least one gp has a different sign than the others, ie iff at least one gp has a diff sign than the 1st one
	        ,
	        sign(false) // binary sign of current grid point
	        ,
	        touched(false) // true iff ray touches at least once the surface (at least one node found)
	        ,
	        touchedInCell(false) // true iff ray touches the surface in a given cell (one node found in that cell)
	        ;
	Real         minOtherAxes(-1.), gpDist;
	unsigned int xInd, yInd, zInd; // the grid indices of cell gridpoints, used below
	while (true) {
		LOG_DEBUG(
		        "\nConsidering cell " << indices[0] << " " << indices[1] << " " << indices[2] << " "
		                              << " while grid itself has " << lsGrid->nGP[0] << " " << lsGrid->nGP[1] << " " << lsGrid->nGP[2]
		                              << " gridpoints");
		point0   = lsGrid->gridPoint(indices[0], indices[1], indices[2]);
		diffSign = false;
		for (unsigned int gp = 0; gp < 8; gp++) { // passing through 8 cell gridpoints to check whether they all have the same distance sign
			xInd   = (gp % 2 ? indices[0] + 1
			                 : indices[0]); // better put parenthesis: "=" and ternary have same precedence, let s not look at associativity
			yInd   = ((gp & 2) / 2 ? indices[1] + 1 : indices[1]);
			zInd   = ((gp & 4) / 4 ? indices[2] + 1 : indices[2]);
			gpDist = distField[xInd][yInd][zInd]; // distance value for current gp
			                                      //			Same sign considerations:
			if (gp == 0) sign = gpDist > 0;       // precedence OK; the binary sign of the "1st" gridpoint (third case = 0 handled otherwise)
			if (gp > 0 and (gpDist > 0) != sign)  // "and" least precedence OK
				diffSign = diffSign || true;  // diffSign is now (and will remain) true because we found one gp with a different sign
				                              // including the case of a zero distance at one cell gp
			if (gpDist == 0) {
				LOG_DEBUG("We should find one gridpoint among the boundary nodes");
				diffSign = diffSign || true; // that s also a case where we need to trigger ray tracing (if not already done)..
			}
		}
		if (diffSign) { // zero distance surface can be detected as in the cell only with at least one change in sign between the gridpoints
			touchedInCell = rayTraceInCell(ray, pointP, point0, indices, nodesTol);
			LOG_INFO(
			        "Considering cell " << indices[0] << " " << indices[1] << " " << indices[2]
			                            << " which should include the surface. rayTraceInCell actually returned " << touchedInCell);
			touched = touched
			        || touchedInCell; // just touched || rayTraceInCell(..) would not work for multiple nodes along a ray because of short circuit !
		}
		// we check whether we still have to walk along ray, or whether we reached the grid boundary (whatever boundary it is):
		if ((indices[0] == lsGrid->nGP[0] - 2) or (indices[1] == lsGrid->nGP[1] - 2) or (!twoD and (indices[2] == lsGrid->nGP[2] - 2)) or !indices[0]
		    or !indices[1] or (!twoD and !indices[2])) {
			if (!touched) LOG_WARN("Ray (" << ray[0] << " " << ray[1] << " " << ray[2] << ") did not create a boundary node");
			LOG_INFO("Ray reached grid boundary because indices = " << indices << ", ray tracing for that one is over");
			break; // this should make sense as soon as center is not already in this boundary layer (which should not happen)
		}
		// we would also stop walking along ray in case we found a (first and only) node for a starLike case:
		if (touched && starLike) break;
		// otherwise, we will now move to the next cell:
		// first, computing the path lengths (along ray) to exit our current cell, testing all three directions, and filling above-mentioned kVal
		for (int axis = 0; axis < 3; axis++) {
			if (ray[axis] > 0) kVal[axis] = (point0[axis] + lsGrid->spacing - pointP[axis]) / ray[axis]; // a positive value
			else if (ray[axis] < 0)
				kVal[axis] = (point0[axis] - pointP[axis]) / ray[axis]; // another positive value
			else
				kVal[axis] = std::numeric_limits<Real>::infinity();
		}
		//second, testing which exit is the closest and update move accordingly:
		move = Vector3i::Zero(); // after recalling the initial value here, setting -1,+1,0 in this Vector3i to update the indices below
		for (int axis = 0; axis < 3; axis++) {
			minOtherAxes = math::min(kVal[(axis + 1) % 3], kVal[(axis + 2) % 3]);
			// do we exit along axis ?
			if (kVal[axis] - minOtherAxes < Mathr::EPSILON * lsGrid->spacing) {
				//				yes: exit along axis is the quickest.
				//				We also take into account the possibility of kVal[axis] being very close from minOtherAxes (see below), be it by excess or by default
				move[axis] = int(math::sign(ray[axis]));
				for (int j = 1; j < 3; j++) { // still checking the two other axes because of finite precision
					if (math::abs(kVal[axis] - kVal[(axis + j) % 3]) < Mathr::EPSILON * lsGrid->spacing)
						move[(axis + j) % 3] = int(math::sign(ray[(axis + j) % 3])); // the exit along axis+j is actually just as close
				}
			}
		}
		// third, updating pointP, as well as cell indices for the next step in the loop (see in the above)
		pointP = pointP + *(std::min_element(kVal.begin(), kVal.end())) * ray;
		indices += move;
		// just checking finally whether we indeed changed cell:
		if (move.squaredNorm() == 0) {
			LOG_ERROR("We're stuck in the same cell !!!");
			break;
		}
	}
	return nodesOnRay;
}

bool LevelSet::rayTraceInCell(const Vector3r& ray, const Vector3r& pointP, const Vector3r& point0, const Vector3i& indices, const Real& nodesTol)
{
	// Given the level set surface described by a trilinear distance function and an oriented ray starting from pointP in some cell,
	// where the cell is (redundantly, but this is to avoid computing things already computed in rayNode) defined by point0 (lowest corner) and its indices
	// it searches for the surface points intersected by the ray in that cell, and returns true if it found one, after populating surfNodes
	int indX(indices[0]), indY(indices[1]), indZ(indices[2]);
	LOG_DEBUG("Ray tracing from pointP = " << pointP << " in cell " << indX << " " << indY << " " << indZ);
	Real     normNode(-1);
	Vector3r trialNode;
	Real     xP(pointP[0]), yP(pointP[1]), zP(pointP[2]);
	Real     ux(ray[0]), uy(ray[1]), uz(ray[2]); // the ray direction (oriented: vec is not the same as -vec)
	Real     spac = lsGrid->spacing;
	bool     touched(false);
	Real     x0(point0[0]), y0(point0[1]), z0(point0[2]);
	Real     f000(distField[indX][indY][indZ]), f111(distField[indX + 1][indY + 1][indZ + 1]), f100(distField[indX + 1][indY][indZ]),
	        f010(distField[indX][indY + 1][indZ]), f001(distField[indX][indY][indZ + 1]), f101(distField[indX + 1][indY][indZ + 1]),
	        f011(distField[indX][indY + 1][indZ + 1]), f110(distField[indX + 1][indY + 1][indZ]);
	// the coefficients of the trilinear interpolation, when written f(x,y,z) = A*x'y'z' + B*x'y' + C*y'z' + D*x'z' + E*x' + F*y' + G*z' + H (with x', .. the dimensionless coordinates, and the useless H = f000):
	Real A(f111 + f100 + f010 + f001 - f101 - f110 - f011 - f000), B(f110 - f100 - f010 + f000), C(f011 - f010 - f001 + f000), D(f101 - f100 - f001 + f000),
	        E(f100 - f000), F(f010 - f000), G(f001 - f000);
	// for the cubic polynom expression of that interpolation (when applied to a ray), we need:
	Real Ag3(A / pow(spac, 3)), Bg2(B / pow(spac, 2)), Cg2(C / pow(spac, 2)), Dg2(D / pow(spac, 2));
	// hence the polynom, in a dimensionless form:
	std::array<Real, 4> coeffs {
		{ distance(pointP) / spac,
		  Ag3 * (uz * (xP - x0) * (yP - y0) + uy * (xP - x0) * (zP - z0) + ux * (yP - y0) * (zP - z0)) + Bg2 * (ux * (yP - y0) + uy * (xP - x0))
		          + Cg2 * (uy * (zP - z0) + uz * (yP - y0)) + Dg2 * (ux * (zP - z0) + uz * (xP - x0)) + E / spac * ux + F / spac * uy + G / spac * uz,
		  (Ag3 * ((xP - x0) * uy * uz + (yP - y0) * ux * uz + (zP - z0) * ux * uy) + Bg2 * ux * uy + Cg2 * uy * uz + Dg2 * ux * uz) * spac,
		  (Ag3 * ux * uy * uz) * pow(spac, 2) }
	};
	LOG_INFO(
	        "We think finding surface-ray intersection is like finding the roots of this polynom (coeffs of increasing order):\n"
	        << coeffs[0] << " " << coeffs[1] << " " << coeffs[2] << " " << coeffs[3]);
	// Then, solving for the 1st root given by Boost root finding algorithm boost::math::tools::newton_raphson_iterate, in a concise syntax thanks to C++11 lambda notation, that replaces going through e.g. an operator() of some struct (whose instance may depend on coeffs)
	Real root(std::numeric_limits<Real>::max());
	try {
		root = boost::math::tools::newton_raphson_iterate(
		        [coeffs](Real k) {
			        return std::
			                make_tuple( // (only ?) problematic part for High Precision (HP) compatibility and REAL_* cmake options different than 64
			                        coeffs[0] + coeffs[1] * k + coeffs[2] * k * k + coeffs[3] * k * k * k,
			                        coeffs[1] + 2 * coeffs[2] * k + 3 * coeffs[3] * k * k);
		        },
		        0.,
		        -sqrt(3.),
		        sqrt(3.),
		        std::numeric_limits<Real>::
		                digits); // doc https://www.boost.org/doc/libs/1_75_0/libs/math/doc/html/math_toolkit/roots_deriv.html suggests to use 0.6 * std::numeric_limits<Real>::digits for the last "digits" argument. With std::numeric_limits<double>::digits = 53, as per standard for the significand
	} catch (
	        const std::exception&
	                e) // starting from somewhere between boost 1.71.0 and 1.74.0, a boost::math::evaluation_error is thrown if no root is found, which can be catched as a std::exception
	{
		LOG_INFO("No root actually found");
	};
	LOG_INFO(
	        "N-R root = " << root << " , leading to a dimensionless distance (through the ray cubic polynom) = "
	                      << coeffs[0] + coeffs[1] * root + coeffs[2] * root * root + coeffs[3] * root * root * root
	                      << " , using digits = " << std::numeric_limits<Real>::digits);
	if (root >= 0) trialNode = pointP + spac * root * ray;
	else if (
	        math::abs(root) <= Mathr::
	                EPSILON) // tiny negative numbers count as well. See e.g. 'superellipsoid',extents=(0.07,0.09,0.14),epsilons=(1.1,0.2),spacing= 0.01 for the -z ray
		trialNode = pointP;
	else                  // truly negative roots do not count
		return false; // we have no node from this ray
	// trialNode is not garanteed until now to be in the considered (indices) cell, to which the cubic polynom should restrict. In order to check for consistency, we no longer check the cell indices though (that would be a discontinuous test too painful to make work with numeric precision), and just look at the distance value (which is continuous at the boundary of two cells)
	if (!Shop::isInBB(trialNode, lsGrid->min, lsGrid->max())) return false; // still starting to check whether trialNodes fits into lsGrid
	Vector3i indicesPt(lsGrid->closestCorner(
	        trialNode)); // in which cell do we find trialNode (see below) ? (quite useless since Feb 2021 actually, except for log messages)
	LOG_INFO("We have a possible intersection point (" << trialNode << "): it would be in cell " << indicesPt << " whereas we considered cell " << indX
	                                                   << " " << indY << " " << indZ << endl;);
	LOG_INFO(
	        "Dimensionless distance, wrt to grid spacing, is here (from distance function) = " << distance(trialNode)
	                / spac << " ; while nodesTol*epsilon = " << nodesTol * Mathr::EPSILON);
	if (nodesOnRay.size() > 0) // wondering about a possible duplicate with previous node (along the same ray), in preparation of the actual check below
		LOG_INFO(
		        "And previous node is " << surfNodes.back() << " (which should be along same ray and equal to " << nodesOnRay.back()
		                                << ") whose comparison with gives (relative difference through norms)"
		                                << (trialNode - surfNodes.back()).norm() / trialNode.norm());
	if ((math::abs(distance(trialNode)) / lengthChar < nodesTol * Mathr::EPSILON)
	    // looks like we found a node. Root finding algorithm normally had a Mathr::EPSILON precision wrt to spac but distance() computation is not the same, hence a different precision. Which is finally estimated here from lengthChar (which matters more than spac)
	    && (nodesOnRay.size() == 0 || ((trialNode - nodesOnRay.back()).norm() / trialNode.norm() > nodesTol * Mathr::EPSILON)
	        // but we still do not want a duplicate of last node, that would come from an adjacent cell, with phi=0 on the cell surface(s)
	        // NB: the 2 above tests are somewhat contradictory in terms of nodesTol (whose value should not be neither too small (phi = 0 could not be checked and more duplicate) nor too big (phi = 0 could be false positive and false positive in duplicate test). What about a 1e-7 hardcoded ?..
	        )) {
		LOG_INFO("ONE NODE FOUND ! (" << trialNode << ")");
		nodesOnRay.push_back(trialNode); // we insert it in nodesOnRay
		touched  = true;
		normNode = trialNode.norm();
		if (normNode < minRad) minRad = normNode;
		if (normNode > maxRad) maxRad = normNode;
	}
	return touched;
}

Vector3r LevelSet::normal(const Vector3r& pt, const bool& unbound) const
{
	// Returns the normal vector at pt from distance gradient
	// Checking which cell we're in:
	Vector3i indices = lsGrid->closestCorner(pt, unbound);
	int      xInd(indices[0]), yInd(indices[1]), zInd(indices[2]);

	if (xInd < 0 || yInd < 0 || zInd < 0) { // operators precedence OK in || vs <
		LOG_ERROR("Can not compute the normal because of given negative grid indices, returning a NaN vector.");
		return Vector3r(NaN, NaN, NaN);
	}
	// Some declarations:
	Real     spac   = lsGrid->spacing;
	Vector3r corner = lsGrid->gridPoint(xInd, yInd, zInd);
	// Then, the reduced coordinates in one cell, i.e. dimensionless x,y,z expected to be in [0;1] (3., top p. 4 Kawamoto2016). Actually capped into [0;1] even for out-of-the grid points (VLS-DEM wants normal = normal(corresponding edge grid corner) in such a case):
	Real xRed = math::max(math::min((pt[0] - corner[0]) / spac, 1.0), 0.0), yRed = math::max(math::min((pt[1] - corner[1]) / spac, 1.0), 0.0),
	     zRed = math::max(math::min((pt[2] - corner[2]) / spac, 1.0), 0.0);
	Real nx(0), ny(0), nz(0); // The x, y, z components of normal, computed below
	// Computing normal as the gradient of trilinear interpolation (e.g. Eq. (2) Kawamoto2016):
	for (int indA = 0; indA < 2; indA++) {
		for (int indB = 0; indB < 2; indB++) {
			for (int indC = 0; indC < 2; indC++) {
				Real lsVal = distField[xInd + indA][yInd + indB][zInd + indC];
				nx += lsVal * (2 * indA - 1) * ((1 - indB) * (1 - yRed) + indB * yRed) * ((1 - indC) * (1 - zRed) + indC * zRed);
				ny += lsVal * (2 * indB - 1) * ((1 - indA) * (1 - xRed) + indA * xRed) * ((1 - indC) * (1 - zRed) + indC * zRed);
				nz += lsVal * (2 * indC - 1) * ((1 - indA) * (1 - xRed) + indA * xRed) * ((1 - indB) * (1 - yRed) + indB * yRed);
			}
		}
	}
	return Vector3r(nx, ny, nz).normalized(); // .normalized() useful since the FMM-output discrete phi field only approximates Eikonal equation
}

void LevelSet::rayTraceSurfNodes(const int& nSurfNodes, const int& nodesPath, const Real& nodesTol)
{
	surfNodes.clear();
	// Obtaining the boundary nodes through "ray tracing", starting from the center
	if (nSurfNodes <= 2 && nodesPath == 1 && !twoD)
		LOG_ERROR("You asked for a level set shape with no more than two boundary nodes, for contact detection purposes. This is too few and will lead "
		          "to square roots of negative numbers, then unexpected events.");
	int              nAngle(int(sqrt(nSurfNodes - 2)));    // useful only if(!twoD && case 1), but let s declare and assign it once for all
	Real             phiMult(Mathr::PI * (3. - sqrt(5.))); // useful only if(!twoD && case 2), ditto
	vector<Vector3r> tracedNodes;                          // quite redundant with class member nodesOnRay, let it be
	for (int rayIdx = 0; rayIdx < nSurfNodes; rayIdx++) {
		if (twoD) // 2D analysis in (x,y) plane
		{
			tracedNodes = rayTrace(
			        Vector3r(cos(float(rayIdx) / nSurfNodes * 2 * Mathr::PI), sin(float(rayIdx) / nSurfNodes * 2 * Mathr::PI), 0), nodesTol);
			surfNodes.insert(surfNodes.end(), tracedNodes.begin(), tracedNodes.end());
		} else {
			Real theta(0), phi(0);
			switch (nodesPath) {
				case 1: // rectangular partition method
					if (rayIdx < 2) {
						theta = (rayIdx == 0 ? 0 : Mathr::PI);
						phi   = 0; // exact value does not really matter..
					} else {
						if ((rayIdx - 2) / nAngle >= nAngle) // possible when nSurfNodes != k^2 + 2...
							LOG_ERROR(
							        "Problems may come soon, please define nSurfNodes as a squared integer + 2 if not twoD. "
							        "Otherwise you will get phi = "
							        << (rayIdx - 2) / nAngle * 2 * Mathr::PI / nAngle);
						theta = ((rayIdx - 2) % nAngle + 1) * Mathr::PI
						        / (nAngle + 1.); // won't include 0 neither pi, handled previously
						phi = (rayIdx - 2) / nAngle * 2. * Mathr::PI
						        / nAngle; // from 0 included to 2 pi excluded. The integer division as first term is intended and prevents merging the 2 nAngle denominators. Also note the left-to-right associativity of * and / which make things work as expected
					}
					break;
				case 2: // spiral points: Elias' polyhedralBall code in the spirit of Rakhmanov1994
					theta = acos(-1. + (2. * rayIdx + 1.) / nSurfNodes);
					phi   = rayIdx * phiMult;
					break;
				default:
					LOG_ERROR(
					        "You asked for locating boundary nodes according to a method #"
					        << nodesPath << " which is not implemented. Supported choices are 1 and 2.")
			}
			LOG_DEBUG(
			        "\nrayIdx (i.e., node for convex) = " << rayIdx << " ; theta = " << theta << " and phi = " << phi << " ; launching ray "
			                                              << Vector3r(1, theta, phi) << endl);
			tracedNodes = rayTrace(ShopLS::spher2cart(Vector3r(1, theta, phi)), nodesTol);
			surfNodes.insert(surfNodes.end(), tracedNodes.begin(), tracedNodes.end());
		}
	}
	if (int(surfNodes.size()) != nSurfNodes) {
		string string1(""), string2("");
		if (starLike) string1 = ", for this starLike shape: this should really not happen";
		else
			string2 = " and/or multiple nodes on a same ray (which could be physical)";
		LOG_WARN(
		        "Ray tracing gave " << surfNodes.size() << " boundary nodes instead of " << nSurfNodes << " required" << string1
		                            << ". You may want to check for unsuccessful rays (and nodesTol)" << string2 << ".");
	}
	postProcessNodes();
}

void LevelSet::init() // computes stuff (center, volume, inertia, nVoxInside) once distField exists
{
	if (initDone) LOG_WARN("How comes we run a second time init ?")
	if (!distField.size()) LOG_ERROR("You are interested into center/volume before that distField has been defined");
	Vector3i gpPerAxis = lsGrid->nGP;
	int      nGPx(gpPerAxis[0]), nGPy(gpPerAxis[1]), nGPz(gpPerAxis[2]);
	if ((int(distField.size()) != nGPx) or (int((distField[0][0]).size()) != nGPz))
		LOG_ERROR("There is a size-inconsistency between the current level set grid and shape.distField for this body! The level set grid has changed "
		          "since the creation of this body, this is not supported.");
	Real spac  = lsGrid->spacing;
	Real Vcell = pow(spac, 3);
	Real phiRef(0.);
	if (smearCoeff != 0)
		phiRef = sqrt(3.0 / 4.0) * spac
		        / smearCoeff; // Smearing parameter (reference distance value) for the smeared Heaviside step function. Equals the cell half-diagonal divided by smearCoeff.
	else
		LOG_WARN("You passed smearCoeff = 0, was that intended ? (there will be no smearing)");
	Real xMean(0.), yMean(0.), zMean(0.); // the center of the level set volume, as determined below
	nVoxInside = 0;
	volume     = 0.0;      // Initializing volume to zero
	Real     phi, dV(-1.); // Distance value and considered particle volume for the current cell (the latter can be less than Vcell due to smearing)
	Vector3r gp;
	// Particle volume is now computed below from a voxellised description which is built upon the sign of distField values. TODO: Code should be equivalent than volumeInsideThreshold(0) (but we here additionally compute *Mean for volume center)
	for (int xIndex = 0; xIndex < nGPx; xIndex++) {
		for (int yIndex = 0; yIndex < nGPy; yIndex++) {
			for (int zIndex = 0; zIndex < nGPz; zIndex++) {
				phi = distField[xIndex][yIndex][zIndex];
				if (math::abs(phi) < phiRef)
					dV = smearedHeaviside(-phi / phiRef)
					        * Vcell; // we are within the smeared boundary (avoiding smearing if smearCoeff < 0)
				else if (phi <= 0)
					dV = Vcell; // inside (more or less far from boundary depending from smearing)
				else if (phi > 0)
					dV = 0.; // outside
				if (dV > 0.) {
					nVoxInside++;
					volume += dV;
					gp = lsGrid->gridPoint(xIndex, yIndex, zIndex);
					xMean += gp[0] * dV;
					yMean += gp[1] * dV;
					zMean += gp[2] * dV;
				}
			}
		}
	}
	if (nVoxInside == 0)
		LOG_ERROR("We have a level set body with 0 voxels being considered inside (0 positive values in distField ?), this is not expected neither "
		          "supported.");

	xMean /= volume;
	yMean /= volume;
	zMean /= volume;
	center = Vector3r(xMean, yMean, zMean);
	if (center.norm() > pow(3., 0.5) * spac) // checking the possible offset wrt a grid spacing-dependent characteristic length
		LOG_ERROR(
		        "Incorrect LevelSet description: shape center is equal to " << center << " in local axes, instead of 0 (modulo a " << spac
		                                                                    << " grid spacing).");
	// Computing inertia in a 2nd loop, now that we first computed center above (in an unavoidable 1st loop):
	Real Ixx(0), Ixy(0), Ixz(0), Iyy(0), Iyz(0), Izz(0);
	Real xV, yV, zV;
	for (int xIndex = 0; xIndex < nGPx; xIndex++) {
		for (int yIndex = 0; yIndex < nGPy; yIndex++) {
			for (int zIndex = 0; zIndex < nGPz; zIndex++) {
				phi = distField[xIndex][yIndex][zIndex];
				if (math::abs(phi) < phiRef)
					dV = smearedHeaviside(-phi / phiRef)
					        * Vcell; // we are within the smeared boundary (avoiding smearing if smearCoeff < 0)
				else if (phi <= 0)
					dV = Vcell; // inside, and away from boundary
				else if (phi > 0)
					dV = 0.; // outside
				if (dV > 0.) {
					gp = lsGrid->gridPoint(xIndex, yIndex, zIndex);
					xV = gp[0];
					yV = gp[1];
					zV = gp[2];
					Ixx += (pow(yV - yMean, 2) + pow(zV - zMean, 2)) * dV;
					Iyy += (pow(xV - xMean, 2) + pow(zV - zMean, 2)) * dV;
					Izz += (pow(xV - xMean, 2) + pow(yV - yMean, 2)) * dV;
					Ixy -= (xV - xMean) * (yV - yMean) * dV;
					Ixz -= (xV - xMean) * (zV - zMean) * dV;
					Iyz -= (yV - yMean) * (zV - zMean) * dV;
				}
			}
		}
	}
	Matrix3r matI, diagI;
	matI << Ixx, Ixy, Ixz, Ixy, Iyy, Iyz, Ixz, Iyz, Izz; // full inertia matrix
	diagI << Ixx, 0, 0, 0, Iyy, 0, 0, 0, Izz;            // just its diagonal components
	Real ratio((matI - diagI).norm() / diagI.norm());    // how much non-diagonal matI is
	if (ratio > 5.e-3) // 0.005 is a convenient value for jduriez .stl data and usual grids, while being probably small enough
		LOG_ERROR(
		        "Incorrect LevelSet input: local frame is non-inertial. Indeed, Ixx = "
		        << Ixx << " ; Iyy = " << Iyy << " ; Izz = " << Izz << " ; Ixy = " << Ixy << " ; Ixz = " << Ixz << " ; Iyz = " << Iyz
		        << " for inertia matrix I, making for a " << ratio << " non-diagonality ratio.");
	inertia    = Vector3r(Ixx, Iyy, Izz);
	lengthChar = twoD
	        ? sqrt(volume / lsGrid->spacing)
	        : cbrt(volume); // measuring lengthChar here (even if surfNodes already exist and won't be recomputed below) to allow a user to satisfactory call for a rayTrace even after a O.save / O.load
	initDone   = true;
}

void LevelSet::postProcessNodes()
{
	if (n_neighborsNodes >= 0) init_neighborsNodes();
	sphericity = maxRad / minRad;
}

bool comp(const std::pair<int, Real> a, const std::pair<int, Real> b)
{
	// for use in init_neighborsNodes; arguments a and b are (node j idx ; distance from j to a reference node i) pairs
	// should return False when a carries the smallest distance to i
	// could be replaced with lambda expression in std::make_heap and std::pop_heap, but this would make duplicated code
	return (a.second > b.second);
}

void LevelSet::init_neighborsNodes()
{
	unsigned int nNodes(surfNodes.size());
	if (int(nNodes) < n_neighborsNodes)
		LOG_FATAL(
		        "We want to establish the " << n_neighborsNodes << " closest neighbors to each node while we have a total of " << nNodes
		                                    << " of those. Impossible to pursue");
	neighborsNodes.clear(); // let us to be sure to restart from scratch
	neighborsNodes.resize(nNodes);
	std::vector<std::vector<std::pair<int, Real>>>
	        d_ijList; // d_ijList[i] has a number of (j,d_ij) pairs describing the distances between a node i and other nodes j (i excluded). Note that using a std::array< .. , nNodes > would probably be impossible to compile
	Real    d_ij(-1),
	        lChar((maxRad + minRad) / 2.) // some arbitrary threshold (D/4 for a sphere) beyond which we re sure not to find any node for neighborsNodes.
	        ;
	if (lChar < 0) LOG_ERROR("Having a negative lChar = " << lChar << "; how comes ?");
	// we first fill d_ijList with (almost, see lChar test) all possible interdistance values
	d_ijList.resize(nNodes);
	for (unsigned int nodeI = 0; nodeI < nNodes; nodeI++) {
		d_ijList[nodeI].reserve(nNodes); // this reserve will be useful for only half of the push_back commands below, but it probably can not hurt
		for (unsigned int nodeJ = nodeI + 1; nodeJ < nNodes; nodeJ++) {
			d_ij = (surfNodes[nodeI] - surfNodes[nodeJ]).norm();
			if (d_ij < lChar) {
				d_ijList[nodeI].push_back(std::make_pair(nodeJ, d_ij));
				d_ijList[nodeJ].push_back(std::make_pair(nodeI, d_ij));
			}
		}
	}
	// we now pick from d_ijList[i] the n closest cases (thanks to a heap sort procedure), to store in neighborsNodes[i]:
	for (unsigned int nodeI = 0; nodeI < nNodes; nodeI++) {
		if (int(d_ijList[nodeI].size()) < n_neighborsNodes) {
			LOG_ERROR(
			        "We are establishing the list of the " << n_neighborsNodes << " closest nodes to node " << nodeI << " while only "
			                                               << d_ijList[nodeI].size() << " interdistance could be established !");
		}
		neighborsNodes[nodeI].reserve(n_neighborsNodes + 1);
		neighborsNodes[nodeI].push_back(nodeI);
		std::make_heap(d_ijList[nodeI].begin(), d_ijList[nodeI].end(), comp);
		for (int neighbrIdx = 0; neighbrIdx < n_neighborsNodes;
		     neighbrIdx++) { // neighbrIdx not "unsigned" to have a signed-valid comparison with n_neighborsNodes
			neighborsNodes[nodeI].push_back((d_ijList[nodeI].front()).first);
			std::pop_heap(d_ijList[nodeI].begin(), d_ijList[nodeI].end(), comp);
			d_ijList[nodeI].pop_back();
		}
	}
	LOG_DEBUG("init_neighborsNodes() all done !");
}

Real LevelSet::distance(const Vector3r& pt, const bool& unbound) const
{
	// We work here in the "reference configuration" or local axes
	Vector3i indices = lsGrid->closestCorner(pt, unbound);
	int      xInd(indices[0]), yInd(indices[1]), zInd(indices[2]);
	Real     dist;

	if (!unbound) { // Points outside the grid are NOT allowed.
		dist = distanceInterpolation(pt, xInd, yInd, zInd);
	} else { // Points outside the grid are allowed
		Vector3i gpPerAxis = lsGrid->nGP;
		int      nGPx(gpPerAxis[0]), nGPy(gpPerAxis[1]), nGPz(gpPerAxis[2]);
		// Check if we pass either the lower or upper bound of the grid
		if (xInd == 0 || yInd == 0 || zInd == 0 || xInd == (nGPx - 2) || yInd == (nGPy - 2) || zInd == (nGPz - 2)) {
			// Do grid extrapolation.
			Vector3r cornerC = lsGrid->gridPoint(xInd, yInd, zInd); // Get the closest point on the grid.
			Real     nx(0), ny(0), nz(0);                           // The x, y, z components of normal, computed below
			// Faster version of LevelSet::normal() for points exactly on the grid,
			// the dimensionless x, y, z of top p. 4 Kawamoto2016 are all zero here.
			for (int indA = 0; indA < 2; indA++) {
				for (int indB = 0; indB < 2; indB++) {
					for (int indC = 0; indC < 2; indC++) {
						Real lsVal = distField[xInd + indA][yInd + indB][zInd + indC];
						nx += lsVal * (2 * indA - 1) * (1 - indB) * (1 - indC);
						ny += lsVal * (2 * indB - 1) * (1 - indA) * (1 - indC);
						nz += lsVal * (2 * indC - 1) * (1 - indA) * (1 - indB);
					}
				}
			}
			Vector3r normalC   = Vector3r(nx, ny, nz).normalized();
			Real     distanceC = distField[xInd][yInd][zInd];
			Vector3r projectC  = cornerC - distanceC * normalC; // Project corner onto the object surface.
			dist               = (projectC - pt).norm();        // Take the distance between the projected point and pt.
		} else
			dist = distanceInterpolation(pt, xInd, yInd, zInd); // Fall back to classical grid interpolation for this inside point.
	}

	return dist;
}

Real LevelSet::distanceInterpolation(const Vector3r& pt, const int& xInd, const int& yInd, const int& zInd) const
{
	// Trilinear interpolation of distance value for pt being inside the (xInd,yInd,zInd) grid cell
	if (xInd < 0 || yInd < 0 || zInd < 0) { // operators precedence OK in || vs <
		LOG_ERROR("Can not compute the distance, returning NaN.");
		return (NaN);
	}
	Real                               f0yz(NaN), f1yz(NaN); // distance values at the same y and z as pt and for a x-value just before (resp. after) pt
	std::array<Real, 2>                yzCoord = { pt[1], pt[2] };
	std::array<Real, 2>                yExtr   = { lsGrid->gridPoint(xInd, yInd, zInd)[1], lsGrid->gridPoint(xInd, yInd + 1, zInd)[1] };
	std::array<Real, 2>                zExtr   = { lsGrid->gridPoint(xInd, yInd, zInd)[2], lsGrid->gridPoint(xInd, yInd, zInd + 1)[2] };
	std::array<std::array<Real, 2>, 2> knownValx0;
	knownValx0[0][0] = distField[xInd][yInd][zInd];
	knownValx0[0][1] = distField[xInd][yInd][zInd + 1];
	knownValx0[1][0] = distField[xInd][yInd + 1][zInd];
	knownValx0[1][1] = distField[xInd][yInd + 1][zInd + 1];
	std::array<std::array<Real, 2>, 2> knownValx1;
	knownValx1[0][0] = distField[xInd + 1][yInd][zInd];
	knownValx1[0][1] = distField[xInd + 1][yInd][zInd + 1];
	knownValx1[1][0] = distField[xInd + 1][yInd + 1][zInd];
	knownValx1[1][1] = distField[xInd + 1][yInd + 1][zInd + 1];
	f0yz             = ShopLS::biInterpolate(yzCoord, yExtr, zExtr, knownValx0);
	f1yz             = ShopLS::biInterpolate(yzCoord, yExtr, zExtr, knownValx1);
	return (pt[0] - lsGrid->gridPoint(xInd, yInd, zInd)[0]) / lsGrid->spacing * (f1yz - f0yz) + f0yz;
}

Real LevelSet::getVolumeDep()
{
	LOG_WARN("LevelSet.volume() has been deprecated in September 2025 and shall be replaced in user-scripts with LevelSet.getVolume()");
	return getVolume();
}

Real LevelSet::getVolume()
{
	if (!initDone) init();
	return volume;
}

Vector3r LevelSet::getInertia()
{
	if (!initDone) init();
	return inertia;
}

Real LevelSet::getSurface_epsilon(Real epsilon) const // to avoid code duplication in getSurface
{
	Real volExcess(volumeInsideThreshold(epsilon)), volDefault(volumeInsideThreshold(-epsilon));
	if (volExcess == volDefault) // may happen in case of failed iterative search in getSurface and an epsilon really fading to 0
		LOG_WARN(
		        "Measuring twice the same volume when using epsilon = " << epsilon << " for a grid spacing of " << lsGrid->spacing
		                                                                << ", we will obtain a zero surface");
	return (volExcess - volDefault) / (2 * epsilon);
}
Real LevelSet::getSurface(Real epsilon) const
{
	unsigned int cptr(0), cptrMax(100);
	Real         surfOld(getSurface_epsilon(epsilon));
	Real         surfNew(-1), relChange(-1);
	while (cptr < cptrMax) {
		epsilon   = epsilon / 2;
		surfNew   = (getSurface_epsilon(epsilon));
		relChange = math::abs(surfOld - surfNew) / surfOld;
		LOG_INFO(
		        "During the iteration nbr " << cptr + 1 << " (with new epsilon = " << epsilon << "), it is computed " << surfNew
		                                    << " for new surface value, to compare with " << surfOld << " for old surface value, ie a " << relChange
		                                    << " relative change");
		if (relChange < 1.e-7) // we converged to a limit
			break;
		else { // we go for another round
			surfOld = surfNew;
			cptr++;
		}
	}
	if (cptr == cptrMax) LOG_ERROR("We reached " << cptrMax << " iterations wo converging to a limit surface value");
	return surfNew;
}
void LevelSet::computeMarchingCubes()
{
	// Clear possible existing marching cube data
	// (might matter later if the level set grid changes size)
	marchingCubesData.triangles.clear();
	marchingCubesData.normals.clear();

	// Compute the Marching Cubes triangulation
	MarchingCube mc;
	Vector3i     gpPerAxis = lsGrid->nGP;
	int          nGPx(gpPerAxis[0]), nGPy(gpPerAxis[1]), nGPz(gpPerAxis[2]);
	mc.init(nGPx, nGPy, nGPz, lsGrid->min, lsGrid->max());
	mc.computeTriangulation(distField, 0);

	// Removing garbage values
	vector<Vector3r> obtTriangles(mc.getTriangles());
	obtTriangles.resize(3 * mc.getNbTriangles());
	vector<Vector3r> obtNormals(mc.getNormals());
	obtNormals.resize(3 * mc.getNbTriangles());

	// Store newly computed marching cube data
	marchingCubesData.triangles   = obtTriangles;
	marchingCubesData.normals     = obtNormals;
	marchingCubesData.nbTriangles = mc.getNbTriangles();

	initDoneMarchingCubes = true;
}

vector<Vector3r> LevelSet::getMarchingCubeTriangles()
{
	if (!initDone) init();
	if (!initDoneMarchingCubes) computeMarchingCubes();
	return marchingCubesData.triangles;
}

vector<Vector3r> LevelSet::getMarchingCubeNormals()
{
	if (!initDone) init();
	if (!initDoneMarchingCubes) computeMarchingCubes();
	return marchingCubesData.normals;
}

int LevelSet::getMarchingCubeNbTriangles()
{
	if (!initDone) init();
	if (!initDoneMarchingCubes) computeMarchingCubes();
	return marchingCubesData.nbTriangles;
}

} // namespace yade
#endif //YADE_LS_DEM