#include "LevelSet.h"

#include <iostream>

#include "tbb/tbb.h"

#include <Eigen/Dense>
#include <Eigen/SVD>

namespace FluidSim2D::SurfaceTrackers
{

void LevelSet::drawGrid(Renderer& renderer, bool doOnlyNarrowBand) const
{
	if (doOnlyNarrowBand)
	{
		forEachVoxelRange(Vec2i(0), size(), [&](const Vec2i& cell)
		{
			if (std::fabs(myPhiGrid(cell)) < myNarrowBand)
				myPhiGrid.drawGridCell(renderer, cell);
		});
	}
	else myPhiGrid.drawGrid(renderer);
}

void LevelSet::drawMeshGrid(Renderer& renderer) const
{
	Transform xform(myPhiGrid.dx(), myPhiGrid.offset() + Vec2f(0.5) * myPhiGrid.dx());
	ScalarGrid<int> tempGrid(xform, myPhiGrid.size());
	tempGrid.drawGrid(renderer);
}

void LevelSet::drawSupersampledValues(Renderer& renderer, float radius, int samples, float sampleSize) const
{
	myPhiGrid.drawSupersampledValues(renderer, radius, samples, sampleSize);
}
void LevelSet::drawNormals(Renderer& renderer, const Vec3f& colour, float length) const
{
	myPhiGrid.drawSampleGradients(renderer, colour, length);
}

void LevelSet::drawSurface(Renderer& renderer, const Vec3f& colour, float lineWidth) const
{
	EdgeMesh surface = buildMSMesh();
	surface.drawMesh(renderer, colour, lineWidth);
}

void LevelSet::drawDCSurface(Renderer& renderer, const Vec3f& colour, float lineWidth) const
{
	EdgeMesh surface = buildDCMesh();
	surface.drawMesh(renderer, colour, lineWidth);
}

// Find the nearest point on the interface starting from the index position.
// If the position falls outside of the narrow band, there isn't a defined gradient
// to use. In this case, the original position will be returned.

Vec2f LevelSet::findSurface(const Vec2f& worldPoint, int iterationLimit) const
{
	assert(iterationLimit >= 0);

	float phi = myPhiGrid.biLerp(worldPoint);

	float epsilon = 1E-2 * dx();
	Vec2f tempPoint = worldPoint;

	int iterationCount = 0;
	if (std::fabs(phi) < myNarrowBand)
	{
		while (std::fabs(phi) > epsilon && iterationCount < iterationLimit)
		{
			tempPoint -= phi * normal(tempPoint);
			phi = myPhiGrid.biCubicInterp(tempPoint);
			++iterationCount;
		}
	}

	return tempPoint;
}

Vec2f LevelSet::findSurfaceIndex(const Vec2f& indexPoint, int iterationLimit) const
{
	Vec2f worldPoint = indexToWorld(indexPoint);
	worldPoint = findSurface(worldPoint, iterationLimit);
	return worldToIndex(worldPoint);
}

void LevelSet::reinit(bool rebuildWithFIM)
{
	UniformGrid<VisitedCellLabels> reinitializedCells(size(), VisitedCellLabels::UNVISITED_CELL);

	// Find the zero crossings, update their distances and flag as source cells
	ScalarGrid<float> tempPhiGrid = myPhiGrid;

	tbb::parallel_for(tbb::blocked_range<int>(0, voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int> &range)
	{
		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = reinitializedCells.unflatten(cellIndex);

			// Check for a zero crossing
			bool isAtZeroCrossing = false;
			for (int axis = 0; axis < 2 && !isAtZeroCrossing; ++axis)
				for (int direction : {0, 1})
				{
					Vec2i adjacentCell = cellToCell(cell, axis, direction);

					if (adjacentCell[axis] < 0 || adjacentCell[axis] >= size()[axis]) continue;

					if ((myPhiGrid(cell) <= 0 && myPhiGrid(adjacentCell) > 0) ||
						(myPhiGrid(cell) > 0 && myPhiGrid(adjacentCell) <= 0))
					{
						isAtZeroCrossing = true;

						Vec2f worldPoint = indexToWorld(Vec2f(cell));
						Vec2f interfacePoint = findSurface(worldPoint, 5);

						float distance = dist(worldPoint, interfacePoint);

						tempPhiGrid(cell) = myPhiGrid(cell) < 0. ? -distance : distance;
						reinitializedCells(cell) = VisitedCellLabels::FINISHED_CELL;

						break;
					}
				}

			// Set unvisited grid cells to background value, using old grid for inside/outside sign
			if (!isAtZeroCrossing)
			{
				assert(reinitializedCells(cell) == VisitedCellLabels::UNVISITED_CELL);
				tempPhiGrid(cell) = myPhiGrid(cell) < 0. ? -myNarrowBand : myNarrowBand;
			}
		}
	});

	//std::swap(myPhiGrid, tempPhiGrid);
	myPhiGrid = tempPhiGrid;

	if (rebuildWithFIM)
		reinitFastIterative(reinitializedCells);
	else
		reinitFastMarching(reinitializedCells);
}

void LevelSet::reinitFastIterative(UniformGrid<VisitedCellLabels>& reinitializedCells)
{
	assert(reinitializedCells.size() == size());

	//
	// Before starting the iterations, we want to construct the active list of voxels
	// to reinitialize.
	//

	tbb::enumerable_thread_specific<std::vector<Vec2i>> parallelActiveCellList;

	tbb::parallel_for(tbb::blocked_range<int>(0, voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		std::vector<Vec2i>& localActiveCellList = parallelActiveCellList.local();

		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = reinitializedCells.unflatten(cellIndex);

			if (reinitializedCells(cell) == VisitedCellLabels::FINISHED_CELL)
			{
				// Add neighbours to the list
				for (int axis : {0, 1})
					for (int direction : {0, 1})
					{
						Vec2i adjacentCell = cellToCell(cell, axis, direction);

						if (adjacentCell[axis] < 0 || adjacentCell[axis] >= size()[axis]) continue;

						if (reinitializedCells(adjacentCell) == VisitedCellLabels::UNVISITED_CELL)
						{
							localActiveCellList.push_back(adjacentCell);
							reinitializedCells(adjacentCell) = VisitedCellLabels::VISITED_CELL;
						}
					}
			}
		}
	});

	std::vector<Vec2i> activeCellList;
	mergeLocalThreadVectors(activeCellList, parallelActiveCellList);

	parallelActiveCellList.clear();

	auto vecCompare = [](const Vec2i& a, const Vec2i& b) -> bool
	{
		if (a[0] < b[0]) return true;
		else if (a[0] == b[0] && a[1] < b[1]) return true;
		return false;
	};

	tbb::parallel_sort(activeCellList.begin(), activeCellList.end(), vecCompare);

	float dx = myPhiGrid.dx();

	// Now that the correct distances and signs have been recorded at the interface,
	// it's important to flood fill that the signed distances outwards into the entire grid.
	// We use the Eikonal equation here to build this outward.

	auto solveEikonal = [&](const Vec2i& idx) -> float
	{
		float Ul = (idx[0] == 0) ? std::numeric_limits<float>::max() : myPhiGrid(idx[0] - 1, idx[1]);
		float Ur = (idx[0] == myPhiGrid.size()[0] - 1) ? std::numeric_limits<float>::max() : myPhiGrid(idx[0] + 1, idx[1]);

		float Ub = (idx[1] == 0) ? std::numeric_limits<float>::max() : myPhiGrid(idx[0], idx[1] - 1);
		float Ut = (idx[1] == myPhiGrid.size()[1] - 1) ? std::numeric_limits<float>::max() : myPhiGrid(idx[0], idx[1] + 1);

		float u = std::fabs(myPhiGrid(idx[0], idx[1]));

		int count = 0;

		float a = std::min(std::fabs(Ul), std::fabs(Ur));
		if (u - a <= 0.) a = std::numeric_limits<float>::max();
		else ++count;

		float b = std::min(std::fabs(Ub), std::fabs(Ut));

		if (u - b <= 0.) b = std::numeric_limits<float>::max();
		else ++count;

		if (a > b) std::swap(a, b);

		if (count == 1) u = a + dx;
		else if (count == 2)
		{
			float temp = -sqr(a) - sqr(b) + 2. * a * b + 2. * sqr(dx);
			if (temp < 0.) u = a + dx;
			else u = .5 * (a + b + sqrt(temp));
			assert(std::isfinite(u));
		}
		// There shouldn't be a case where count is 0 but it seems to be happenning..

		return u;

	};

	ScalarGrid<float> tempPhiGrid = myPhiGrid;

	float tolerance = dx * 1E-5;
	bool stillActiveCells = true;

	int activeCellCount = activeCellList.size();

	int iteration = 0;
	int maxIterations = 5 * myNarrowBand / dx;

	while (activeCellCount > 0 && iteration < maxIterations)
	{
		tbb::parallel_for(tbb::blocked_range<int>(0, activeCellCount, tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			auto& localActiveCellList = parallelActiveCellList.local();

			Vec2i oldCell(-1);

			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i newCell = activeCellList[cellIndex];

				if (oldCell == newCell)
					continue;

				oldCell = newCell;

				assert(reinitializedCells(newCell) == VisitedCellLabels::VISITED_CELL);

				float newPhi = solveEikonal(newCell);

				// If we hit the narrow band, we don't need to make any changes
				if (newPhi > myNarrowBand) continue;

				tempPhiGrid(newCell) = myPhiGrid(newCell) < 0 ? -newPhi : newPhi;

				// Check if new phi is converged
				float oldPhi = myPhiGrid(newCell);

				// If the cell is converged, load up the neighbours that aren't currently being VISITED
				if (std::fabs(newPhi - std::fabs(oldPhi)) < tolerance)
				{
					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i adjacentCell = cellToCell(newCell, axis, direction);

							if (adjacentCell[axis] < 0 || adjacentCell[axis] >= size()[axis]) continue;

							if (reinitializedCells(adjacentCell) == VisitedCellLabels::UNVISITED_CELL)
							{
								float adjacentNewPhi = solveEikonal(adjacentCell);

								if (adjacentNewPhi > myNarrowBand) continue;

								// Check if new phi is less than the current value
								float adjacentOldPhi = std::fabs(myPhiGrid(adjacentCell));

								if ((adjacentNewPhi < adjacentOldPhi) && (std::fabs(adjacentNewPhi - adjacentOldPhi) > tolerance))
								{
									tempPhiGrid(adjacentCell) = myPhiGrid(adjacentCell) < 0 ? -adjacentNewPhi : adjacentNewPhi;

									localActiveCellList.push_back(adjacentCell);
								}
							}

						}
				}
				// If the cell hasn't converged, toss it back into the list
				else
					localActiveCellList.push_back(newCell);
			}
		});

		// Turn off VISITED labels for current list
		tbb::parallel_for(tbb::blocked_range<int>(0, activeCellCount, tbbLightGrainSize), [&](const tbb::blocked_range<int> &range)
		{
			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i cell = activeCellList[cellIndex];
				assert(reinitializedCells(cell) != VisitedCellLabels::FINISHED_CELL);
				reinitializedCells(cell) = VisitedCellLabels::UNVISITED_CELL;
			}
		});

		activeCellList.clear();

		mergeLocalThreadVectors(activeCellList, parallelActiveCellList);

		parallelActiveCellList.clear();

		tbb::parallel_sort(activeCellList.begin(), activeCellList.end(), vecCompare);

		activeCellCount = activeCellList.size();

		// Turn on VISITED labels for new list
		tbb::parallel_for(tbb::blocked_range<int>(0, activeCellCount, tbbLightGrainSize), [&](const tbb::blocked_range<int> &range)
		{
			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i cell = activeCellList[cellIndex];
				assert(reinitializedCells(cell) != VisitedCellLabels::FINISHED_CELL);
				reinitializedCells(cell) = VisitedCellLabels::VISITED_CELL;
			}
		});

		//std::swap(tempPhiGrid, myPhiGrid);
		myPhiGrid = tempPhiGrid;

		++iteration;
	}
}

void LevelSet::initFromMesh(const EdgeMesh& initialMesh, bool doResizeGrid)
{
	if (doResizeGrid)
	{
		// Determine the bounding box of the mesh to build the underlying grids
		Vec2f minBoundingBox(std::numeric_limits<float>::max());
		Vec2f maxBoundingBox(std::numeric_limits<float>::lowest());

		for (int vertexIndex = 0; vertexIndex < initialMesh.vertexCount(); ++vertexIndex)
			updateMinAndMax(minBoundingBox, maxBoundingBox, initialMesh.vertex(vertexIndex).point());

		// Just for nice whole numbers, let's clamp the bounding box to be an integer
		// offset in index space then bring it back to world space
		float maxNarrowBand = 10.;
		maxNarrowBand = std::min(myNarrowBand / dx(), maxNarrowBand);

		minBoundingBox = dx() * (Vec2f(floor(minBoundingBox / dx())) - 2 * Vec2f(maxNarrowBand));
		maxBoundingBox = dx() * (Vec2f(ceil(maxBoundingBox / dx())) + 2 * Vec2f(maxNarrowBand));

		clear();
		Transform xform(dx(), minBoundingBox);
		// Since we know how big the mesh is, we know how big our grid needs to be (wrt to grid spacing)
		myPhiGrid = ScalarGrid<float>(xform, Vec2i((maxBoundingBox - minBoundingBox) / dx()), myNarrowBand);
	}
	else
		myPhiGrid.resize(size(), myNarrowBand);

	// We want to track which cells in the level set contain valid distance information.
	// The first pass will set cells close to the mesh as FINISHED.
	UniformGrid<VisitedCellLabels> reinitializedCells(size(), VisitedCellLabels::UNVISITED_CELL);
	UniformGrid<int> meshCellParities(size(), 0);

	for (const auto& edge : initialMesh.edges())
	{
		// It's easier to work in our index space and just scale the distance later.
		const Vec2f& startPoint = worldToIndex(initialMesh.vertex(edge.vertex(0)).point());
		const Vec2f& endPoint = worldToIndex(initialMesh.vertex(edge.vertex(1)).point());

		// Record mesh-grid intersections between cell nodes (i.e. on grid edges)
		// Since we only cast rays *left-to-right* for inside/outside checking, we don't
		// need to know if the mesh intersects y-aligned grid edges
		Vec2f vmin, vmax;
		minAndMax(vmin, vmax, startPoint, endPoint);

		Vec2i edgeCeilMin = Vec2i(ceil(vmin));
		Vec2i edgeFloorMin = Vec2i(floor(vmin)) - Vec2i(1);
		Vec2i edgeFloorMax = Vec2i(floor(vmax));

		for (int j = edgeCeilMin[1]; j <= edgeFloorMax[1]; ++j)
			for (int i = edgeFloorMax[0]; i >= edgeFloorMin[0]; --i)
			{
				Vec2f gridNode(i, j);
				IntersectionLabels intersectionResult = exactEdgeIntersect(startPoint, endPoint, gridNode, Axis::XAXIS);

				// TODO: remove once test complete
				if (gridNode[0] < 0 || gridNode[1] < 0 || gridNode[0] >= myPhiGrid.size()[0] ||
					gridNode[1] >= myPhiGrid.size()[1])
				{
					std::cout << "Caught out of bounds. Node: " << gridNode[0] << " " << gridNode[1] << std::endl;
				}

				if (intersectionResult == IntersectionLabels::NO) continue;

				// Increment the parity since the grid_node is
				// "left" of the mesh-edge crossing the grid-edge.
				// This indicates a negative normal in the x-direction
				// and means we're entering into the material.
				int parityChange = -1;
				if (startPoint[1] < endPoint[1])
					parityChange = 1;

				if (intersectionResult == IntersectionLabels::YES)
					meshCellParities(i + 1, j) += parityChange;
				// If the grid node is explicitly on the mesh-edge, set distance to zero
				// since it might not be exactly zero due to floating point error above.
				else
				{
					assert(intersectionResult == IntersectionLabels::ON);
					// Technically speaking, the zero isocountour means we're inside
					// the surface. So we should change the parity at the node that
					// is intersected even though it's zero and the sign is meaningless.
					reinitializedCells(i, j) = VisitedCellLabels::FINISHED_CELL;
					myPhiGrid(i, j) = 0.;
					meshCellParities(i, j) += parityChange;
				}

				break;
			}
	}

	// Now that all the x-axis edge crossings have been found, we can compile the parity changes
	// and label grid nodes that are at the interface
	for (int j = 0; j < size()[1]; ++j)
	{
		int parity = myIsBackgroundNegative ? 1 : 0;

		// We loop x-major because that's how we've set up our edge intersection.
		for (int i = 0; i < size()[0]; ++i) // TODO: double check that I've resized right
		{
			// Update parity before changing sign since the parity values above used the convention
			// of putting the change on the "far" node (i.e. after the mesh-grid intersection).

			Vec2i cell(i, j);
			parity += meshCellParities(cell);
			meshCellParities(cell) = parity;

			// Set inside cells to negative
			if (parity > 0) myPhiGrid(cell) = -std::fabs(myPhiGrid(cell));
		}

		assert(myIsBackgroundNegative ? parity == 1 : parity == 0);
	}

	// With the parity assigned, loop over the grid once more and label nodes that have an implied sign change
	// with neighbouring nodes (this means parity goes from -'ve (and zero) to +'ve or vice versa).
	forEachVoxelRange(Vec2i(1), size() - Vec2i(1), [&](const Vec2i& cell)
	{
		bool isCellInside = meshCellParities(cell) > 0;

		for (int axis : {0, 1})
			for (int direction : {0, 1})
			{
				Vec2i adjacentCell = cellToCell(cell, axis, direction);

				bool isAdjacentCellInside = meshCellParities(adjacentCell) > 0;

				if (isCellInside != isAdjacentCellInside)
					reinitializedCells(cell) = VisitedCellLabels::FINISHED_CELL;
			}
	});

	// Loop over all the edges in the mesh. Level set grid cells labelled as VISITED will be
	// updated with the distance to the surface if it happens to be shorter than the current
	// distance to the surface.
	for (const auto& edge : initialMesh.edges())
	{
		// Using the vertices of the edge, we can update distance values for cells
		// within the bounding box of the mesh. It's easier to work in our index space
		// and just scale the distance later.
		const Vec2f& startPoint = worldToIndex(initialMesh.vertex(edge.vertex(0)).point());
		const Vec2f& endPoint = worldToIndex(initialMesh.vertex(edge.vertex(1)).point());

		// Build bounding box

		Vec2i minBoundingBox = Vec2i(floor(minUnion(startPoint, endPoint))) - Vec2i(2);
		minBoundingBox = maxUnion(minBoundingBox, Vec2i(0));

		Vec2i maxBoundingBox = Vec2i(ceil(maxUnion(startPoint, endPoint))) + Vec2i(2);
		Vec2i top = size() - Vec2i(1);
		maxBoundingBox = minUnion(maxBoundingBox, top);

		// Update distances to the mesh at grid cells within the bounding box
		assert(minBoundingBox[0] >= 0 && minBoundingBox[1] >= 0 && maxBoundingBox[0] < size()[0] && maxBoundingBox[1] < size()[1]);

		forEachVoxelRange(minBoundingBox, maxBoundingBox + Vec2i(1), [&](const Vec2i& cell)
		{
			if (reinitializedCells(cell) != VisitedCellLabels::UNVISITED_CELL)
			{
				Vec2f cellPoint(cell);
				Vec2f vec0 = cellPoint - startPoint;
				Vec2f vec1 = endPoint - startPoint;

				float s = dot(vec0, vec1) / dot(vec1, vec1); // Find projection along edge.
				s = clamp(s, float(0), float(1));

				// Remove on-edge projection to get vector from closest point on edge to cell point.
				float surfaceDistance = mag(vec0 - s * vec1) * dx();

				// Update if the distance to this edge is shorter than previous values.
				if (surfaceDistance < std::fabs(myPhiGrid(cell)))
				{
					// If the parity says the node is inside, set it to be negative
					myPhiGrid(cell) = (meshCellParities(cell) > 0) ? -surfaceDistance : surfaceDistance;
				}
			}
		});
	}

	reinitFastIterative(reinitializedCells);
	//reinitFastMarching(reinitializedCells);
}

void LevelSet::reinitFastMarching(UniformGrid<VisitedCellLabels>& reinitializedCells)
{
	assert(reinitializedCells.size() == size());

	// Now that the correct distances and signs have been recorded at the interface,
	// it's important to flood fill that the signed distances outwards into the entire grid.
	// We use the Eikonal equation here to build this outward
	auto solveEikonal = [&](const Vec2i& cell) -> float
	{
		float max = std::numeric_limits<float>::max();

		float U_bx = (cell[0] > 0) ? std::fabs(myPhiGrid(cell[0] - 1, cell[1])) : max;
		float U_fx = (cell[0] < size()[0] - 1) ? std::fabs(myPhiGrid(cell[0] + 1, cell[1])) : max;

		float U_by = (cell[1] > 0) ? std::fabs(myPhiGrid(cell[0], cell[1] - 1)) : max;
		float U_fy = (cell[1] < size()[1] - 1) ? std::fabs(myPhiGrid(cell[0], cell[1] + 1)) : max;

		float Uh = std::min(U_bx, U_fx);
		float Uv = std::min(U_by, U_fy);
		float U;

		if (std::fabs(Uh - Uv) >= dx())
			U = std::min(Uh, Uv) + dx();
		else
			// Quadratic equation from the Eikonal
			U = (Uh + Uv) / 2. + .5 * std::sqrt(pow(Uh + Uv, 2.) - 2. * (sqr(Uh) + sqr(Uv) - sqr(dx())));

		return U;
	};

	// Load up the BFS queue with the unvisited cells next to the finished ones
	using Node = std::pair<Vec2i, float>;
	auto cmp = [](const Node& a, const Node& b) -> bool { return std::fabs(a.second) > std::fabs(b.second); };
	std::priority_queue<Node, std::vector<Node>, decltype(cmp)> marchingQ(cmp);

	forEachVoxelRange(Vec2i(0), size(), [&](const Vec2i& cell)
	{
		if (reinitializedCells(cell) == VisitedCellLabels::FINISHED_CELL)
		{
			for (int axis : {0, 1})
				for (int direction : {0, 1})
				{
					Vec2i adjacentCell = cellToCell(cell, axis, direction);

					if (adjacentCell[axis] < 0 || adjacentCell[axis] >= size()[axis]) continue;

					if (reinitializedCells(adjacentCell) == VisitedCellLabels::UNVISITED_CELL)
					{
						float dist = solveEikonal(adjacentCell);
						assert(dist >= 0);

						myPhiGrid(adjacentCell) = (myPhiGrid(adjacentCell) < 0.) ? -dist : dist;

						Node node(adjacentCell, dist);

						marchingQ.push(node);
						reinitializedCells(adjacentCell) = VisitedCellLabels::VISITED_CELL;
					}
				}
		}
	});

	while (!marchingQ.empty())
	{
		Node localNode = marchingQ.top();
		Vec2i localCell = localNode.first;
		marchingQ.pop();

		// Since you can't just update parts of the priority queue,
		// it's possible that a cell has been solidified at a smaller distance
		// and an older insert if floating around.
		if (reinitializedCells(localCell) == VisitedCellLabels::FINISHED_CELL)
		{
			// Make sure that the distance assigned to the cell is smaller than
			// what is floating around
			assert(std::fabs(myPhiGrid(localCell)) <= std::fabs(localNode.second));
			continue;
		}
		assert(reinitializedCells(localCell) == VisitedCellLabels::VISITED_CELL);

		if (std::fabs(myPhiGrid(localCell)) < myNarrowBand)
		{
			// Debug check that there is indeed a FINISHED cell next to it
			bool foundFinishedCell = false;

			// Loop over the neighbouring cells and load the unvisited cells
			// and update the visited cells
			for (int axis : {0, 1})
				for (int direction : {0, 1})
				{
					Vec2i adjacentCell = cellToCell(localCell, axis, direction);

					if (adjacentCell[axis] < 0 || adjacentCell[axis] >= reinitializedCells.size()[axis])
						continue;

					if (reinitializedCells(adjacentCell) == VisitedCellLabels::FINISHED_CELL)
						foundFinishedCell = true;
					else // If visited, then we'll update it
					{
						float dist = solveEikonal(adjacentCell);
						assert(dist >= 0);

						if (dist > myNarrowBand) dist = myNarrowBand;

						if (reinitializedCells(adjacentCell) == VisitedCellLabels::VISITED_CELL && dist > std::fabs(myPhiGrid(adjacentCell)))
							continue;

						myPhiGrid(adjacentCell) = myPhiGrid(adjacentCell) < 0 ? -dist : dist;

						Node node(adjacentCell, dist);

						marchingQ.push(node);
						reinitializedCells(adjacentCell) = VisitedCellLabels::VISITED_CELL;
					}
				}

			//Check that a marked cell was indeed visited
			assert(foundFinishedCell);
		}
		// Clamp to narrow band
		else myPhiGrid(localCell) = myPhiGrid(localCell) < 0 ? -myNarrowBand : myNarrowBand;

		// Solidify cell now that we've handled all it's neighbours
		reinitializedCells(localCell) = VisitedCellLabels::FINISHED_CELL;
	}
}

// Extract a mesh representation of the interface. Useful for rendering but not much
// else since there will be duplicate vertices per grid edge in the current implementation.

EdgeMesh LevelSet::buildMSMesh() const
{
	std::vector<Vec2f> verts;
	std::vector<Vec2i> edges;

	// Run marching squares loop
	forEachVoxelRange(Vec2i(0), size() - Vec2i(1), [&](const Vec2i& cell)
	{
		int mcKey = 0;

		for (int direction = 0; direction < 4; ++direction)
		{
			Vec2i node = cellToNodeCCW(cell, direction);
			if (myPhiGrid(node) <= 0.) mcKey += (1 << direction);
		}

		// Connect edges using the marching squares template
		for (int edgeIndex = 0; edgeIndex < 4 && marchingSquaresTemplate[mcKey][edgeIndex] >= 0; edgeIndex += 2)
		{
			// Find first vertex
			int edge = marchingSquaresTemplate[mcKey][edgeIndex];

			Vec3i faceMap = cellToFaceCCW(cell, edge);

			Vec2i face(faceMap[0], faceMap[1]);
			int axis = faceMap[2];
			Vec2i startNode = faceToNode(face, axis, 0);
			Vec2i endNode = faceToNode(face, axis, 1);

			Vec2f startPoint = interpolateInterface(startNode, endNode);

			// Find second vertex
			edge = marchingSquaresTemplate[mcKey][edgeIndex + 1];
			faceMap = cellToFaceCCW(cell, edge);

			face = Vec2i(faceMap[0], faceMap[1]);
			axis = faceMap[2];

			startNode = faceToNode(face, axis, 0);
			endNode = faceToNode(face, axis, 1);

			Vec2f endPoint = interpolateInterface(startNode, endNode);

			// Store vertices
			Vec2f worldStartPoint = indexToWorld(startPoint);
			Vec2f worldEndPoint = indexToWorld(endPoint);

			verts.push_back(worldStartPoint);
			verts.push_back(worldEndPoint);

			edges.emplace_back(verts.size() - 2, verts.size() - 1);
		}
	});

	return EdgeMesh(edges, verts);
}

// Extract a mesh representation of the interface using dual contouring
EdgeMesh LevelSet::buildDCMesh() const
{
	std::vector<Vec2f> verts;
	std::vector<Vec2i> edges;

	// Create grid to store index to dual contouring point. Note that phi is
	// center sampled so the DC grid must be node sampled and one cell shorter
	// in each dimension
	UniformGrid<int> dcPointIndex(size() - Vec2i(1), -1);

	// Run dual contouring loop
	forEachVoxelRange(Vec2i(0), dcPointIndex.size(), [&](const Vec2i& cell)
	{
		std::vector<Vec2f> qefPoints;
		std::vector<Vec2f> qefNormals;

		for (int axis : {0, 1})
			for (int direction : {0, 1})
			{
				Vec2i face = cellToFace(cell, axis, direction);

				Vec2i backwardNode = faceToNode(face, axis, 0);
				Vec2i forwardNode = faceToNode(face, axis, 1);

				if ((myPhiGrid(backwardNode) <= 0 && myPhiGrid(forwardNode) > 0) ||
					(myPhiGrid(backwardNode) > 0 && myPhiGrid(forwardNode) <= 0))
				{
					// Find interface point
					Vec2f interfacePoint = interpolateInterface(backwardNode, forwardNode);
					qefPoints.push_back(interfacePoint);

					// Find associated surface normal
					Vec2f surfaceNormal = normal(indexToWorld(interfacePoint));
					qefNormals.push_back(surfaceNormal);
				}
			}

		if (qefPoints.size() > 0)
		{
			Eigen::MatrixXd A(qefPoints.size(), 2);
			Eigen::VectorXd b(qefPoints.size());
			Eigen::VectorXd pointCOM = Eigen::VectorXd::Zero(2);

			assert(qefPoints.size() > 1);

			for (int pointIndex = 0; pointIndex < qefPoints.size(); ++pointIndex)
			{
				A(pointIndex, 0) = qefNormals[pointIndex][0];
				A(pointIndex, 1) = qefNormals[pointIndex][1];

				b(pointIndex) = dot(qefNormals[pointIndex], qefPoints[pointIndex]);

				pointCOM[0] += qefPoints[pointIndex][0];
				pointCOM[1] += qefPoints[pointIndex][1];
			}

			pointCOM /= float(qefPoints.size());

			Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
			svd.setThreshold(1E-2);

			Eigen::VectorXd dcPoint = pointCOM + svd.solve(b - A * pointCOM);

			Vec2f vecCOM(pointCOM[0], pointCOM[1]);

			Vec2f boundingBoxMin = floor(vecCOM);
			Vec2f boundingBoxMax = ceil(vecCOM);

			if (dcPoint[0] < boundingBoxMin[0] ||
				dcPoint[1] < boundingBoxMin[1] ||
				dcPoint[0] > boundingBoxMax[0] ||
				dcPoint[1] > boundingBoxMax[1])
				dcPoint = pointCOM;

			verts.push_back(indexToWorld(Vec2f(dcPoint[0], dcPoint[1])));
			dcPointIndex(cell) = verts.size() - 1;
		}
	});

	for (int axis : {0, 1})
	{
		Vec2i start(0); ++start[axis];
		Vec2i end(dcPointIndex.size());

		forEachVoxelRange(start, end, [&](const Vec2i& face)
		{
			Vec2i backwardNode = faceToNode(face, axis, 0);
			Vec2i forwardNode = faceToNode(face, axis, 1);

			if ((myPhiGrid(backwardNode) <= 0 && myPhiGrid(forwardNode) > 0) ||
				(myPhiGrid(backwardNode) > 0 && myPhiGrid(forwardNode) <= 0))
			{
				Vec2i backwardCell = faceToCell(Vec2i(face), axis, 0);
				Vec2i forwardCell = faceToCell(Vec2i(face), axis, 1);

				assert(dcPointIndex(backwardCell) >= 0 && dcPointIndex(forwardCell) >= 0);

				Vec2i edge;
				if (myPhiGrid(backwardNode) <= 0.)
				{
					if (axis == 0)
						edge = Vec2i(dcPointIndex(backwardCell), dcPointIndex(forwardCell));
					else
						edge = Vec2i(dcPointIndex(forwardCell), dcPointIndex(backwardCell));
				}
				else
				{
					if (axis == 0)
						edge = Vec2i(dcPointIndex(forwardCell), dcPointIndex(backwardCell));
					else
						edge = Vec2i(dcPointIndex(backwardCell), dcPointIndex(forwardCell));
				}

				edges.push_back(edge);
			}
		});
	}

	return EdgeMesh(edges, verts);
}

Vec2f LevelSet::interpolateInterface(const Vec2i& startPoint, const Vec2i& endPoint) const
{
	assert((myPhiGrid(startPoint[0], startPoint[1]) <= 0 && myPhiGrid(endPoint[0], endPoint[1]) > 0) ||
			(myPhiGrid(startPoint[0], startPoint[1]) > 0 && myPhiGrid(endPoint[0], endPoint[1]) <= 0));

	//Find weight to zero isosurface
	float s = myPhiGrid(startPoint) / (myPhiGrid(startPoint) - myPhiGrid(endPoint));
	s = clamp(s, float(0), float(1));

	Vec2f dx = Vec2f(endPoint) - Vec2f(startPoint);
	return Vec2f(startPoint) + s * dx;
}

void LevelSet::unionSurface(const LevelSet& unionPhi)
{
	assert(isGridMatched(unionPhi));

	tbb::parallel_for(tbb::blocked_range<int>(0, voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = myPhiGrid.unflatten(cellIndex);
			if (unionPhi(cell) < 2 * unionPhi.dx())
				myPhiGrid(cell) = std::min(myPhiGrid(cell), unionPhi(cell));
		}
	});

	reinitMesh();
}

}


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Ioan Sucan, Jonathan Gammell*/

#include "ompl/util/RandomNumbers.h"
#include "ompl/util/Exception.h"
#include "ompl/util/Console.h"
#include <mutex>
#include <memory>
#include <boost/math/constants/constants.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/random/uniform_on_sphere.hpp>
#include <boost/random/variate_generator.hpp>
#include <Eigen/Core>

/// @cond IGNORE
namespace
{
    /// We use a different random number generator for the seeds of the
    /// other random generators. The root seed is from the number of
    /// nano-seconds in the current time, or given by the user.
    class RNGSeedGenerator
    {
    public:
        RNGSeedGenerator()
          : firstSeed_(std::chrono::duration_cast<std::chrono::microseconds>(
                           std::chrono::system_clock::now() - std::chrono::system_clock::time_point::min())
                           .count())
          , sGen_(firstSeed_)
          , sDist_(1, 1000000000)
        {
        }

        std::uint_fast32_t firstSeed()
        {
            std::lock_guard<std::mutex> slock(rngMutex_);
            return firstSeed_;
        }

        void setSeed(std::uint_fast32_t seed)
        {
            std::lock_guard<std::mutex> slock(rngMutex_);
            if (seed > 0)
            {
                if (someSeedsGenerated_)
                {
                    OMPL_ERROR("Random number generation already started. Changing seed now will not lead to "
                               "deterministic sampling.");
                }
                else
                {
                    // In this case, since no seeds have been generated yet, so we remember this seed as the first one.
                    firstSeed_ = seed;
                }
            }
            else
            {
                if (someSeedsGenerated_)
                {
                    OMPL_WARN("Random generator seed cannot be 0. Ignoring seed.");
                    return;
                }
                OMPL_WARN("Random generator seed cannot be 0. Using 1 instead.");
                seed = 1;
            }
            sGen_.seed(seed);
        }

        std::uint_fast32_t nextSeed()
        {
            std::lock_guard<std::mutex> slock(rngMutex_);
            someSeedsGenerated_ = true;
            return sDist_(sGen_);
        }

    private:
        bool someSeedsGenerated_{false};
        std::uint_fast32_t firstSeed_;
        std::mutex rngMutex_;
        std::ranlux24_base sGen_;
        std::uniform_int_distribution<> sDist_;
    };

    std::once_flag g_once;
    boost::scoped_ptr<RNGSeedGenerator> g_RNGSeedGenerator;

    void initRNGSeedGenerator()
    {
        g_RNGSeedGenerator.reset(new RNGSeedGenerator());
    }

    RNGSeedGenerator &getRNGSeedGenerator()
    {
        std::call_once(g_once, &initRNGSeedGenerator);
        return *g_RNGSeedGenerator;
    }
}  // namespace
/// @endcond

/// @cond IGNORE
class ompl::RNG::SphericalData
{
public:
    /** \brief The container type for the variate generators. */
    using container_type_t = std::vector<double>;

    /** \brief The uniform_on_sphere distribution type. */
    using spherical_dist_t = boost::uniform_on_sphere<double, container_type_t>;

    /** \brief The resulting variate generator type. */
    using variate_generator_t = boost::variate_generator<std::mt19937 *, spherical_dist_t>;

    /** \brief Constructor */
    SphericalData(std::mt19937 *generatorPtr) : generatorPtr_(generatorPtr){};

    /** \brief The generator for a specified dimension. Will create if not existent */
    container_type_t generate(unsigned int dim)
    {
        // Assure that the dimension is in the range of the vector.
        growVector(dim);

        // Assure that the dimension is allocated:
        allocateDimension(dim);

        // Return the generator
        return (*dimVector_.at(dim).second)();
    };

    /** \brief Iterate over all the dimensions and reset the generators that exist. */
    void reset()
    {
        // Iterate over each dimension
        for (auto &i : dimVector_)
            // Check if the variate_generator is allocated
            if (bool(i.first))
                // It is, reset THE DATA (not the pointer)
                i.first->reset();
        // No else, this is an uninitialized dimension.
    };

private:
    /** \brief The pair of distribution and variate generator. */
    using dist_gen_pair_t = std::pair<std::shared_ptr<spherical_dist_t>, std::shared_ptr<variate_generator_t>>;

    /** \brief A vector distribution and variate generators (as pointers) indexed on dimension. */
    std::vector<dist_gen_pair_t> dimVector_;

    /** \brief A pointer to the generator owned by the outer class. Needed for creating new variate_generators */
    std::mt19937 *generatorPtr_;

    /** \brief Grow the vector until it contains an (empty) entry for the specified dimension. */
    void growVector(unsigned int dim)
    {
        // Iterate until the index associated with this dimension is in the vector
        while (dim >= dimVector_.size())
            // Create a pair of empty pointers:
            dimVector_.emplace_back();
    };

    /** \brief Assure that a distribution/generator is allocated for the specified index. */
    void allocateDimension(unsigned int dim)
    {
        // Only do this if unallocated, so check that:
        if (dimVector_.at(dim).first == nullptr)
        {
            // It is not allocated, so....
            // First construct the distribution
            dimVector_.at(dim).first = std::make_shared<spherical_dist_t>(dim);
            // Then the variate generator
            dimVector_.at(dim).second = std::make_shared<variate_generator_t>(generatorPtr_, *dimVector_.at(dim).first);
        }
        // No else, the pointer is already allocated.
    };
};
/// @endcond

std::uint_fast32_t ompl::RNG::getSeed()
{
    return getRNGSeedGenerator().firstSeed();
}

void ompl::RNG::setSeed(std::uint_fast32_t seed)
{
    getRNGSeedGenerator().setSeed(seed);
}

ompl::RNG::RNG()
  : localSeed_(getRNGSeedGenerator().nextSeed())
  , generator_(localSeed_)
  , sphericalDataPtr_(std::make_shared<SphericalData>(&generator_))
{
}

ompl::RNG::RNG(std::uint_fast32_t localSeed)
  : localSeed_(localSeed), generator_(localSeed_), sphericalDataPtr_(std::make_shared<SphericalData>(&generator_))
{
}

void ompl::RNG::setLocalSeed(std::uint_fast32_t localSeed)
{
    // Store the seed
    localSeed_ = localSeed;

    // Change the generator's seed
    generator_.seed(localSeed_);

    // Reset the distributions used by the variate generators, as they can cache values
    uniDist_.reset();
    normalDist_.reset();
    sphericalDataPtr_->reset();
}

double ompl::RNG::halfNormalReal(double r_min, double r_max, double focus)
{
    assert(r_min <= r_max);

    const double mean = r_max - r_min;
    double v = gaussian(mean, mean / focus);

    if (v > mean)
        v = 2.0 * mean - v;
    double r = v >= 0.0 ? v + r_min : r_min;
    return r > r_max ? r_max : r;
}

int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus)
{
    auto r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
    return (r > r_max) ? r_max : r;
}

// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
//       pg. 124-132
void ompl::RNG::quaternion(double value[4])
{
    double x0 = uniDist_(generator_);
    double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
    double t1 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_),
           t2 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_);
    double c1 = cos(t1), s1 = sin(t1);
    double c2 = cos(t2), s2 = sin(t2);
    value[0] = s1 * r1;
    value[1] = c1 * r1;
    value[2] = s2 * r2;
    value[3] = c2 * r2;
}

// From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
void ompl::RNG::eulerRPY(double value[3])
{
    value[0] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
    value[1] = acos(1.0 - 2.0 * uniDist_(generator_)) - boost::math::constants::pi<double>() / 2.0;
    value[2] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
}

void ompl::RNG::uniformNormalVector(std::vector<double> &v)
{
    // Generate a random value, the variate_generator is returning a shallow_array_adaptor, which will modify the value
    // array:
    v = sphericalDataPtr_->generate(v.size());
}

// See: http://math.stackexchange.com/a/87238
void ompl::RNG::uniformInBall(double r, std::vector<double> &v)
{
    // Draw a random point on the unit sphere
    uniformNormalVector(v);

    // Draw a random radius scale
    double radiusScale = r * std::pow(uniformReal(0.0, 1.0), 1.0 / static_cast<double>(v.size()));

    // Scale the point on the unit sphere
    std::transform(v.begin(), v.end(), v.begin(), [radiusScale](double x) { return radiusScale * x; });
}

void ompl::RNG::uniformProlateHyperspheroidSurface(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr,
                                                   double value[])
{
    // Variables
    // The spherical point as a std::vector
    std::vector<double> sphere(phsPtr->getDimension());

    // Get a random point on the sphere
    uniformNormalVector(sphere);

    // Transform to the PHS
    phsPtr->transform(&sphere[0], value);
}

void ompl::RNG::uniformProlateHyperspheroid(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[])
{
    // Variables
    // The spherical point as a std::vector
    std::vector<double> sphere(phsPtr->getDimension());

    // Get a random point in the sphere
    uniformInBall(1.0, sphere);

    // Transform to the PHS
    phsPtr->transform(&sphere[0], value);
}


#include <iostream>
#include <vector>
#include <string>

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/preprocessor/seq/for_each.hpp>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/static_matrix.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/block_matrix.hpp>

#if defined(SOLVER_BACKEND_VEXCL)
#  include <amgcl/backend/vexcl.hpp>
#  include <amgcl/backend/vexcl_static_matrix.hpp>
#elif defined(SOLVER_BACKEND_CUDA)
#  include <amgcl/backend/cuda.hpp>
#  include <amgcl/relaxation/cusparse_ilu0.hpp>
#else
#  ifndef SOLVER_BACKEND_BUILTIN
#    define SOLVER_BACKEND_BUILTIN
#  endif
#endif

#include <amgcl/mpi/util.hpp>
#include <amgcl/mpi/make_solver.hpp>
#include <amgcl/mpi/preconditioner.hpp>
#include <amgcl/mpi/solver/runtime.hpp>

#include <amgcl/io/mm.hpp>
#include <amgcl/io/binary.hpp>
#include <amgcl/profiler.hpp>

#ifndef AMGCL_BLOCK_SIZES
#  define AMGCL_BLOCK_SIZES (3)(4)
#endif

namespace amgcl {
    profiler<> prof;
}

namespace math = amgcl::math;

//---------------------------------------------------------------------------
ptrdiff_t assemble_poisson3d(amgcl::mpi::communicator comm,
        ptrdiff_t n, int block_size,
        std::vector<ptrdiff_t> &ptr,
        std::vector<ptrdiff_t> &col,
        std::vector<double>    &val,
        std::vector<double>    &rhs)
{
    ptrdiff_t n3 = n * n * n;

    ptrdiff_t chunk = (n3 + comm.size - 1) / comm.size;
    if (chunk % block_size != 0) {
        chunk += block_size - chunk % block_size;
    }
    ptrdiff_t row_beg = std::min(n3, chunk * comm.rank);
    ptrdiff_t row_end = std::min(n3, row_beg + chunk);
    chunk = row_end - row_beg;

    ptr.clear(); ptr.reserve(chunk + 1);
    col.clear(); col.reserve(chunk * 7);
    val.clear(); val.reserve(chunk * 7);

    rhs.resize(chunk);
    std::fill(rhs.begin(), rhs.end(), 1.0);

    const double h2i = (n - 1) * (n - 1);
    ptr.push_back(0);

    for (ptrdiff_t idx = row_beg; idx < row_end; ++idx) {
        ptrdiff_t k = idx / (n * n);
        ptrdiff_t j = (idx / n) % n;
        ptrdiff_t i = idx % n;

        if (k > 0)  {
            col.push_back(idx - n * n);
            val.push_back(-h2i);
        }

        if (j > 0)  {
            col.push_back(idx - n);
            val.push_back(-h2i);
        }

        if (i > 0) {
            col.push_back(idx - 1);
            val.push_back(-h2i);
        }

        col.push_back(idx);
        val.push_back(6 * h2i);

        if (i + 1 < n) {
            col.push_back(idx + 1);
            val.push_back(-h2i);
        }

        if (j + 1 < n) {
            col.push_back(idx + n);
            val.push_back(-h2i);
        }

        if (k + 1 < n) {
            col.push_back(idx + n * n);
            val.push_back(-h2i);
        }

        ptr.push_back( col.size() );
    }

    return chunk;
}

//---------------------------------------------------------------------------
ptrdiff_t read_matrix_market(
        amgcl::mpi::communicator comm,
        const std::string &A_file, const std::string &rhs_file, int block_size,
        std::vector<ptrdiff_t> &ptr,
        std::vector<ptrdiff_t> &col,
        std::vector<double>    &val,
        std::vector<double>    &rhs)
{
    amgcl::io::mm_reader A_mm(A_file);
    ptrdiff_t n = A_mm.rows();

    ptrdiff_t chunk = (n + comm.size - 1) / comm.size;
    if (chunk % block_size != 0) {
        chunk += block_size - chunk % block_size;
    }

    ptrdiff_t row_beg = std::min(n, chunk * comm.rank);
    ptrdiff_t row_end = std::min(n, row_beg + chunk);

    chunk = row_end - row_beg;

    A_mm(ptr, col, val, row_beg, row_end);

    if (rhs_file.empty()) {
        rhs.resize(chunk);
        std::fill(rhs.begin(), rhs.end(), 1.0);
    } else {
        amgcl::io::mm_reader rhs_mm(rhs_file);
        rhs_mm(rhs, row_beg, row_end);
    }

    return chunk;
}

//---------------------------------------------------------------------------
ptrdiff_t read_binary(
        amgcl::mpi::communicator comm,
        const std::string &A_file, const std::string &rhs_file, int block_size,
        std::vector<ptrdiff_t> &ptr,
        std::vector<ptrdiff_t> &col,
        std::vector<double>    &val,
        std::vector<double>    &rhs)
{
    ptrdiff_t n = amgcl::io::crs_size<ptrdiff_t>(A_file);

    ptrdiff_t chunk = (n + comm.size - 1) / comm.size;
    if (chunk % block_size != 0) {
        chunk += block_size - chunk % block_size;
    }

    ptrdiff_t row_beg = std::min(n, chunk * comm.rank);
    ptrdiff_t row_end = std::min(n, row_beg + chunk);

    chunk = row_end - row_beg;

    amgcl::io::read_crs(A_file, n, ptr, col, val, row_beg, row_end);

    if (rhs_file.empty()) {
        rhs.resize(chunk);
        std::fill(rhs.begin(), rhs.end(), 1.0);
    } else {
        ptrdiff_t rows, cols;
        amgcl::io::read_dense(rhs_file, rows, cols, rhs, row_beg, row_end);
    }

    return chunk;
}

//---------------------------------------------------------------------------
template <class Backend, class Matrix>
std::shared_ptr< amgcl::mpi::distributed_matrix<Backend> >
partition(amgcl::mpi::communicator comm, const Matrix &Astrip,
        typename Backend::vector &rhs, const typename Backend::params &bprm,
        amgcl::runtime::mpi::partition::type ptype, int block_size = 1)
{
    typedef typename Backend::value_type val_type;
    typedef typename amgcl::math::rhs_of<val_type>::type rhs_type;
    typedef amgcl::mpi::distributed_matrix<Backend> DMatrix;

    using amgcl::prof;

    auto A = std::make_shared<DMatrix>(comm, Astrip);

    if (comm.size == 1 || ptype == amgcl::runtime::mpi::partition::merge)
        return A;

    prof.tic("partition");
    boost::property_tree::ptree prm;
    prm.put("type", ptype);
    amgcl::runtime::mpi::partition::wrapper<Backend> part(prm);

    auto I = part(*A, block_size);
    auto J = transpose(*I);
    A = product(*J, *product(*A, *I));

#if defined(SOLVER_BACKEND_BUILTIN)
    amgcl::backend::numa_vector<rhs_type> new_rhs(J->loc_rows());
#elif defined(SOLVER_BACKEND_VEXCL)
    vex::vector<rhs_type> new_rhs(bprm.q, J->loc_rows());
#elif defined(SOLVER_BACKEND_CUDA)
    thrust::device_vector<rhs_type> new_rhs(J->loc_rows());
#endif

    J->move_to_backend(bprm);

    amgcl::backend::spmv(1, *J, rhs, 0, new_rhs);
    rhs.swap(new_rhs);
    prof.toc("partition");

    return A;
}

//---------------------------------------------------------------------------
#if defined(SOLVER_BACKEND_BUILTIN) || defined(SOLVER_BACKEND_VEXCL)
template <int B>
void solve_block(
        amgcl::mpi::communicator comm,
        ptrdiff_t chunk,
        const std::vector<ptrdiff_t>      &ptr,
        const std::vector<ptrdiff_t>      &col,
        const std::vector<double>         &val,
        const boost::property_tree::ptree &prm,
        const std::vector<double>         &f,
        amgcl::runtime::mpi::partition::type ptype
        )
{
    typedef amgcl::static_matrix<double, B, B> val_type;
    typedef amgcl::static_matrix<double, B, 1> rhs_type;

#if defined(SOLVER_BACKEND_BUILTIN)
    typedef amgcl::backend::builtin<val_type> Backend;
#elif defined(SOLVER_BACKEND_VEXCL)
    typedef amgcl::backend::vexcl<val_type> Backend;
#endif

    typedef
        amgcl::mpi::make_solver<
            amgcl::runtime::mpi::preconditioner<Backend>,
            amgcl::runtime::mpi::solver::wrapper<Backend>
            >
        Solver;

    using amgcl::prof;

    typename Backend::params bprm;

#if defined(SOLVER_BACKEND_BUILTIN)
    amgcl::backend::numa_vector<rhs_type> rhs(
            reinterpret_cast<const rhs_type*>(&f[0]),
            reinterpret_cast<const rhs_type*>(&f[0]) + chunk / B
            );
#elif defined(SOLVER_BACKEND_VEXCL)
    vex::Context ctx(vex::Filter::Env);
    bprm.q = ctx;

    vex::scoped_program_header header(ctx,
            amgcl::backend::vexcl_static_matrix_declaration<double,B>());

    if (comm.rank == 0) std::cout << ctx << std::endl;

    vex::vector<rhs_type> rhs(ctx, chunk / B, reinterpret_cast<const rhs_type*>(&f[0]));
#endif

    prof.tic("setup");
    std::shared_ptr<Solver> solve;
    if (ptype) {
        auto A = partition<Backend>(comm,
                amgcl::adapter::block_matrix<val_type>(std::tie(chunk, ptr, col, val)),
                rhs, bprm, ptype, prm.get("precond.coarsening.aggr.block_size", 1));

        solve = std::make_shared<Solver>(comm, A, prm, bprm);
        chunk = A->loc_rows();
    } else {
        solve = std::make_shared<Solver>(comm,
                amgcl::adapter::block_matrix<val_type>(std::tie(chunk, ptr, col, val)),
                prm, bprm);
    }
    prof.toc("setup");

    if (comm.rank == 0) {
        std::cout << *solve << std::endl;
    }

#if defined(SOLVER_BACKEND_BUILTIN)
    amgcl::backend::numa_vector<rhs_type> x(chunk);
#elif defined(SOLVER_BACKEND_VEXCL)
    vex::vector<rhs_type> x(ctx, chunk);
    x = math::zero<rhs_type>();
#endif

    int    iters;
    double error;

    prof.tic("solve");
    std::tie(iters, error) = (*solve)(rhs, x);
    prof.toc("solve");

    if (comm.rank == 0) {
        std::cout
            << "Iterations: " << iters << std::endl
            << "Error:      " << error << std::endl
            << prof << std::endl;
    }
}
#endif

//---------------------------------------------------------------------------
void solve_scalar(
        amgcl::mpi::communicator comm,
        ptrdiff_t chunk,
        const std::vector<ptrdiff_t> &ptr,
        const std::vector<ptrdiff_t> &col,
        const std::vector<double> &val,
        const boost::property_tree::ptree &prm,
        const std::vector<double> &f,
        amgcl::runtime::mpi::partition::type ptype
        )
{
#if defined(SOLVER_BACKEND_BUILTIN)
    typedef amgcl::backend::builtin<double> Backend;
#elif defined(SOLVER_BACKEND_VEXCL)
    typedef amgcl::backend::vexcl<double> Backend;
#elif defined(SOLVER_BACKEND_CUDA)
    typedef amgcl::backend::cuda<double> Backend;
#endif

    typedef
        amgcl::mpi::make_solver<
            amgcl::runtime::mpi::preconditioner<Backend>,
            amgcl::runtime::mpi::solver::wrapper<Backend>
            >
        Solver;

    using amgcl::prof;

    typename Backend::params bprm;

#if defined(SOLVER_BACKEND_BUILTIN)
    amgcl::backend::numa_vector<double> rhs(f);
#elif defined(SOLVER_BACKEND_VEXCL)
    vex::Context ctx(vex::Filter::Env);
    bprm.q = ctx;

    if (comm.rank == 0) std::cout << ctx << std::endl;

    vex::vector<double> rhs(ctx, f);
#elif defined(SOLVER_BACKEND_CUDA)
    cusparseCreate(&bprm.cusparse_handle);
    thrust::device_vector<double> rhs(f);
#endif

    prof.tic("setup");
    std::shared_ptr<Solver> solve;
    if (ptype) {
        auto A = partition<Backend>(comm,
                std::tie(chunk, ptr, col, val), rhs, bprm, ptype,
                prm.get("precond.coarsening.aggr.block_size", 1));

        solve = std::make_shared<Solver>(comm, A, prm, bprm);
        chunk = A->loc_rows();
    } else {
        solve = std::make_shared<Solver>(comm, std::tie(chunk, ptr, col, val), prm, bprm);
    }
    prof.toc("setup");

    if (comm.rank == 0) {
        std::cout << *solve << std::endl;
    }

#if defined(SOLVER_BACKEND_BUILTIN)
    amgcl::backend::numa_vector<double> x(chunk);
#elif defined(SOLVER_BACKEND_VEXCL)
    vex::vector<double> x(ctx, chunk);
    x = 0.0;
#elif defined(SOLVER_BACKEND_CUDA)
    thrust::device_vector<double> x(chunk, 0.0);
#endif

    int    iters;
    double error;

    prof.tic("solve");
    std::tie(iters, error) = (*solve)(rhs, x);
    prof.toc("solve");

    if (comm.rank == 0) {
        std::cout
            << "Iterations: " << iters << std::endl
            << "Error:      " << error << std::endl
            << prof << std::endl;
    }
}

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    amgcl::mpi::init_thread mpi(&argc, &argv);
    amgcl::mpi::communicator comm(MPI_COMM_WORLD);

    if (comm.rank == 0)
        std::cout << "World size: " << comm.size << std::endl;

    using amgcl::prof;

    // Read configuration from command line
    namespace po = boost::program_options;
    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        ("matrix,A",
         po::value<std::string>(),
         "System matrix in the MatrixMarket format. "
         "When not specified, a Poisson problem in 3D unit cube is assembled. "
        )
        (
         "rhs,f",
         po::value<std::string>()->default_value(""),
         "The RHS vector in the MatrixMarket format. "
         "When omitted, a vector of ones is used by default. "
         "Should only be provided together with a system matrix. "
        )
        (
         "Ap",
         po::value< std::vector<std::string> >()->multitoken(),
         "Pre-partitioned matrix (single file per MPI process)"
        )
        (
         "fp",
         po::value< std::vector<std::string> >()->multitoken(),
         "Pre-partitioned RHS (single file per MPI process)"
        )
        (
         "binary,B",
         po::bool_switch()->default_value(false),
         "When specified, treat input files as binary instead of as MatrixMarket. "
         "It is assumed the files were converted to binary format with mm2bin utility. "
        )
        (
         "block-size,b",
         po::value<int>()->default_value(1),
         "The block size of the system matrix. "
         "When specified, the system matrix is assumed to have block-wise structure. "
         "This usually is the case for problems in elasticity, structural mechanics, "
         "for coupled systems of PDE (such as Navier-Stokes equations), etc. "
        )
        (
         "partitioner,r",
         po::value<amgcl::runtime::mpi::partition::type>()->default_value(
#if defined(AMGCL_HAVE_SCOTCH)
             amgcl::runtime::mpi::partition::ptscotch
#elif defined(AMGCL_HAVE_PARMETIS)
             amgcl::runtime::mpi::partition::parmetis
#else
             amgcl::runtime::mpi::partition::merge
#endif
             ),
         "Repartition the system matrix"
        )
        (
         "size,n",
         po::value<ptrdiff_t>()->default_value(128),
         "domain size"
        )
        ("prm-file,P",
         po::value<std::string>(),
         "Parameter file in json format. "
        )
        (
         "prm,p",
         po::value< std::vector<std::string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        ;

    po::positional_options_description p;
    p.add("prm", -1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
        if (comm.rank == 0) std::cout << desc << std::endl;
        return 0;
    }

    boost::property_tree::ptree prm;
    if (vm.count("prm-file")) {
        read_json(vm["prm-file"].as<std::string>(), prm);
    }

    if (vm.count("prm")) {
        for(const std::string &v : vm["prm"].as<std::vector<std::string> >()) {
            amgcl::put(prm, v);
        }
    }

    ptrdiff_t n;
    std::vector<ptrdiff_t> ptr;
    std::vector<ptrdiff_t> col;
    std::vector<double>    val;
    std::vector<double>    rhs;

    int block_size = vm["block-size"].as<int>();
    int aggr_block = prm.get("precond.coarsening.aggr.block_size", 1);

    bool binary = vm["binary"].as<bool>();
    amgcl::runtime::mpi::partition::type ptype = vm["partitioner"].as<amgcl::runtime::mpi::partition::type>();

    if (vm.count("matrix")) {
        prof.tic("read");
        if (binary) {
            n = read_binary(comm,
                    vm["matrix"].as<std::string>(),
                    vm["rhs"].as<std::string>(),
                    block_size * aggr_block, ptr, col, val, rhs);
        } else {
            n = read_matrix_market(comm,
                    vm["matrix"].as<std::string>(),
                    vm["rhs"].as<std::string>(),
                    block_size * aggr_block, ptr, col, val, rhs);
        }
        prof.toc("read");
    } else if (vm.count("Ap")) {
        prof.tic("read");
        ptype = static_cast<amgcl::runtime::mpi::partition::type>(0);

        std::vector<std::string> Aparts = vm["Ap"].as<std::vector<std::string>>();
        comm.check(Aparts.size() == static_cast<size_t>(comm.size),
                "--Ap should have single entry per MPI process");

        if (binary) {
            amgcl::io::read_crs(Aparts[comm.rank], n, ptr, col, val);
        } else {
            ptrdiff_t m;
            std::tie(n, m) = amgcl::io::mm_reader(Aparts[comm.rank])(ptr, col, val);
        }

        if (vm.count("fp")) {
            std::vector<std::string> fparts = vm["fp"].as<std::vector<std::string>>();
            comm.check(fparts.size() == static_cast<size_t>(comm.size),
                    "--fp should have single entry per MPI process");

            ptrdiff_t rows;
            ptrdiff_t cols;

            if (binary) {
                amgcl::io::read_dense(fparts[comm.rank], rows, cols, rhs);
            } else {
                std::tie(rows, cols) = amgcl::io::mm_reader(fparts[comm.rank])(rhs);
            }

            comm.check(rhs.size() == static_cast<size_t>(n), "Wrong RHS size");
        } else {
            rhs.resize(n, 1);
        }
        prof.toc("read");
    } else {
        prof.tic("assemble");
        n = assemble_poisson3d(comm,
                vm["size"].as<ptrdiff_t>(),
                block_size * aggr_block, ptr, col, val, rhs);
        prof.toc("assemble");
    }

    switch(block_size) {

#if defined(SOLVER_BACKEND_BUILTIN) || defined(SOLVER_BACKEND_VEXCL)
#  define AMGCL_CALL_BLOCK_SOLVER(z, data, B)                        \
        case B:                                                      \
            solve_block<B>(comm, n, ptr, col, val, prm, rhs, ptype); \
            break;

        BOOST_PP_SEQ_FOR_EACH(AMGCL_CALL_BLOCK_SOLVER, ~, AMGCL_BLOCK_SIZES)

#  undef AMGCL_CALL_BLOCK_SOLVER
#endif

        case 1:
            solve_scalar(comm, n, ptr, col, val, prm, rhs, ptype);
            break;
        default:
            if (comm.rank == 0)
                std::cout << "Unsupported block size!" << std::endl;
    }
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2012-2012 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/**
* \file
* \author Martin Weiser
* This file contains an implementation of an additive hierarchical basis preconditioner.
*/

#ifndef HB_HH
#define HB_HH

#include <cassert>
#include <cmath>
#include <iostream>
#include <vector>

#include <boost/utility/result_of.hpp>

#include "dune/grid/common/grid.hh"
#include "dune/istl/preconditioners.hh"

#include "fem/barycentric.hh"

namespace Kaskade {

  /**
  * \ingroup linalgsolution
  * \brief A hierarchical basis preconditioner.
  *
  * This preconditioner realizes an approximative inverse of a Laplace operator with constant diffusion coefficient
  * discretized by linear finite elements in nodal basis over the leaf view of the provided grid.
  *
  * This implementation is geometry- and coefficient-agnostic, i.e. it assumes that the diagonal of the stiffness matrix is
  * just the identity. The advantage of this approach are its extreme simplicity, both in terms of interface and implementation,
  * a minimal memory footprint, and very cheap set-up phase and application. The drawback is, of course, a worse resulting
  * condition number in case non-uniform coarse grids, anisotropic coarse grids, or spatially varying or anisotropic
  * diffusion coefficients are to be treated.
  *
  * Extending the implementation to take more structure (stiffness matrices, non-uniform grids) into account is probably
  * not worth the while, as the iteration count grows anyway in proportion to the number of refinement levels in 2D
  * or even faster in 3D. If the simplicity of the PrecondType::HB preconditioner does'nt pay off, have a look at \ref MultigridSolver
  * or \ref BPXYPreconditioner.
  *
  * We refer to Deuflhard/Weiser Chapter 7.3.
  *
  * \tparam Grid the grid on which the Laplace operator is discretized. This implementation assumes that the grid
  *              is purely simplicial and that on refinement new nodes are created only at edge midpoints.
  * \tparam Domain the type of domain space vectors
  * \tparam Range the type of range space vectors
  */

  template <class Grid, class Domain, class Range>
  class HierarchicalBasisPreconditioner: public Dune::Preconditioner<Domain,Range> {
    private:
      typedef typename Grid::LevelGridView::IndexSet::IndexType Index;
      typedef std::vector<Index>                                NodeSet;

      static int const dim = Grid::dimension;

    public:
      enum { category = Dune::SolverCategory::sequential };

      /**
      * \brief Constructor.
      * \param grid the grid
      * \param mat_ a matrix containing (at least) the diagonal of the stiffness matrix
      */
      explicit HierarchicalBasisPreconditioner(Grid const& grid)
      {
	// make sure the grid satisfies our assumptions: a purely simplicial grid
	assert(grid.leafIndexSet().geomTypes(0).size()==1 && grid.leafIndexSet().geomTypes(0)[0].isSimplex()
	      && "HierarchicalBasisPreconditioner currently requires purely simplicial grids");

	// extract and store relevant sizes
	j = grid.maxLevel();
	n = grid.leafIndexSet().size(dim);

	typedef typename Grid::template Codim<0>::LevelIterator  CellLevelIterator;
	typedef typename Grid::template Codim<dim>::LevelIterator LeafVertexIterator;

	// extract the coarse grid nodes
	nodesOnLevel.resize(j+1);
	for (LeafVertexIterator i=grid.template lbegin<dim>(0); i!=grid.template lend<dim>(0); ++i)
	  nodesOnLevel[0].push_back(grid.leafIndexSet().index(*i));

	// extract the coarse grid elements' vertices
	coarseElement.reserve(grid.levelIndexSet(0).size(0));
	for (CellLevelIterator ci=grid.template lbegin<0>(0); ci!=grid.template lend<0>(0); ++ci)
	{
	  Dune::FieldVector<Index,dim+1> idx;
	  for (int i=0; i<=dim; ++i)
	    idx[i] = grid.leafIndexSet().index(*(ci->template subEntity<dim>(i)));
	  coarseElement.push_back(idx);
	}

	// compute the parents of each node on level >0
	parents.resize(n,std::make_pair(n,n)); // invalid sentinel (n,n) denotes not yet processed nodes.
	for (int k=1; k<=j; ++k)
	{
	  // On each level, look out where nodes of this level are located in their father cell. Their barycentric coordinates
	  // should have exactly two nonzero entries of size 0.5. The corresponding corners of the father cell are the parents.
	  for (CellLevelIterator ci=grid.template lbegin<0>(k); ci!=grid.template lend<0>(k); ++ci)
	  {
	    typedef typename Grid::template Codim<0>::EntityPointer CellPointer;
	    CellPointer father = ci->father();

	    // Extract indices of father's vertices
	    // @TODO Variable is never read! Remove or keep for future extension of the preconditioner??
//            Index fatherCornerIndex[dim+1];
//            for (int i=0; i<=dim; ++i)
//              fatherCornerIndex[i] = grid.leafIndexSet().index(*(father->template subEntity<dim>(i)));

	    // consider each vertex of current cell in turn
	    for (int i=0; i<=dim; ++i)
	    {
	      typename Grid::template Codim<dim>::EntityPointer vertexPointer = ci->template subEntity<dim>(i);
	      Index idx = grid.leafIndexSet().index(*vertexPointer);

	      // Edge midpoints can be reached from multiple cells depending on the spatial dimension. The parent nodes
	      // are independent of from which cell we look. Thus we check wether we've already done the work.
	      if (parents[idx].first==n && vertexPointer->level()>0) // not yet processed (but has parents)
	      {
		// Obtain barycentric coordinates of child corner in father. For child nodes located on an edge,
		// there will be exactly two nonzero entries associated to the vertices. Note that the barycentric
		// coordinates as implemented in fem/barycentric.hh are shifted by index 1 compared to the
		// reference element vertex numbering.
		Dune::FieldVector<typename Grid::ctype,dim+1> relativeCoord = barycentric(ci->geometryInFather().corner(i));

		// Extract the two coordinates with value 0.5
		int coords[dim+1];
		int pos = 0;
		for (int m=0; m<=dim; ++m)
		  if (relativeCoord[m] > 0.4)
		    coords[pos++] = (m+1)%(dim+1);

		if (pos==2)
		{
		  // that's an edge midpoint: obtain the leaf indices of those father corners
		  parents[idx].first  = grid.leafIndexSet().index(*(father->template subEntity<dim>(coords[0])));
		  parents[idx].second = grid.leafIndexSet().index(*(father->template subEntity<dim>(coords[1])));

		  // write down that this node appeared first on level k
		  nodesOnLevel[k].push_back(idx);
		}
	      }
	    }
	  }
	}

	// perform sanity checks
	#ifndef NDEBUG
	// check that each level >0 node has parents
	int count = nodesOnLevel[0].size();
	for (int k=1; k<=j; ++k)
	{
	  for (typename NodeSet::const_iterator i=nodesOnLevel[k].begin(); i!=nodesOnLevel[k].end(); ++i)
	    assert(parents[*i].first<n && parents[*i].second<n);
	  count += nodesOnLevel[k].size();
	}
	assert(count==n);
	#endif
      }

      /**
      * \brief Has to be called before the first call to apply
      */
      virtual void pre (Domain& x, Range& b)
      {
	// actually, does nothing :)
      }

      /**
      * \brief applies the preconditioner to the residual \arg d, which results in the correction \arg v
      */
      virtual void apply (Domain& v, const Range& d)
      {
	using namespace boost::fusion;
      // We modify the residual (in-place), hence we have to copy it here.
	Range r = d;

	// apply smoother and recursively restrict the residual downwards through the mesh hierarchy
	for (int k=j; k>0; --k)
	{
	  // correct scaling for diagonal coefficients
	  typename Domain::field_type a = std::pow(2.0,(Grid::dimension-2.0)*(k-j));

	  //  for all nodes on level k
	  for (typename NodeSet::const_iterator i=nodesOnLevel[k].begin(); i!=nodesOnLevel[k].end(); ++i)
	  {
	    // apply one Jacobi step, care for correct scaling of lower level hierarchical basis functions
	    // Beispiel at_c<0>(estSol.data)[*j];

	    at_c<0>(v.data)[*i] = at_c<0>(r.data)[*i];
	    at_c<0>(v.data)[*i] /= a;

	    // restrict the residual
	    at_c<0>(r.data)[parents[*i].first]  += 0.5 * at_c<0>(r.data)[*i];
	    at_c<0>(r.data)[parents[*i].second] += 0.5 * at_c<0>(r.data)[*i];
	  }
	}

	// "solve" on coarse grid.
	coarseGridSolution(v,r);

	// prolongate the correction recursively upwards through the mesh hierarchy, adding up
	// all the corrections from different levels
	for (int k=1; k<=j; ++k)
	  // the nodes on level k get contributions of 0.5 from each of their parent nodes - that's all
	  for (typename NodeSet::const_iterator i=nodesOnLevel[k].begin(); i!=nodesOnLevel[k].end(); ++i)
	    at_c<0>(v.data)[*i] += 0.5*(at_c<0>(v.data)[parents[*i].first]+at_c<0>(v.data)[parents[*i].second]);
      }

      /**
      * \brief Has to be called after the last call to apply
      */
      virtual void post (Domain& x)
      {
	// actually, does nothing :)
      }

    private:
      std::vector<NodeSet>                       nodesOnLevel;   // hierarchical basis node sets
      std::vector<std::pair<Index,Index> >       parents;        // parent nodes of child nodes on level >0
      int                                        j;              // maximum grid level
      Index                                      n;              // total number of nodes
      std::vector<Dune::FieldVector<Index,dim+1> > coarseElement;

      // This coarse grid solution employs a simple Jacobi iteration, where the
      // matrix is simply patched together from identical elemental stiffness matrices.
      // TODO: maybe it would be better to explicitly form the coarse grid matrix and use a direct solver
      void coarseGridSolution(Domain& v, Range& r) const {
	using namespace boost::fusion;
	// get the correct scaling for the coarse grid
	typename Domain::field_type a = std::pow(2.0,-(Grid::dimension-2.0));

	// Initialize correction to zero
	for (typename NodeSet::const_iterator i=nodesOnLevel[0].begin(); i!=nodesOnLevel[0].end(); ++i)
	  at_c<0>(v.data)[*i] = 0;
	Domain dv = v; // TODO: that's a looooong vector (overkill)

	// perform a couple of Jacobi iterations
	for (int k=0; k<20; ++k)
	{
	  // add correction: solve with diagonal and set residual to zero
	  for (typename NodeSet::const_iterator i=nodesOnLevel[0].begin(); i!=nodesOnLevel[0].end(); ++i)
	  {
	    at_c<0>(dv.data)[*i] = at_c<0>(r.data)[*i];
	    at_c<0>(v.data)[*i] += at_c<0>(dv.data)[*i] / a;
	  }

	  // update the residual. We are geometry agnostic here and use a rough approximation of the Laplacian: On each
	  // coarse grid element we assume we have the following element matrices in 1D, 2D, 3D, respectively:
	  // [ 1 -1 ]       [ 2 -1 -1 ]        [  3 -1 -1 -1 ]
	  // [ -1 1 ], 1/12 [-1  2 -1 ], 1/60  [ -1  3 -1 -1 ]
	  //                [-1 -1  2 ]        [ -1 -1  3 -1 ]
	  //                                   [ -1 -1 -1  3 ]
	  // Obviously, the matrixes are just  s*(-e*e^T + (d+1)*I), where e is the vector with all entries zero. In order
	  // to have an invertible matrix, we add a little bit more of the identity, i.e. we end up with element matrices
	  // s*(-e*e^T + (d+1.1)*I) with s=1, 1/12, 1/60 for d=1,2,3, respectively. In this form, multiplication with the
	  // total stiffness matrix is just summing up the contributions of the elemental matrices that can efficiently
	  // been computed.
	  typename Dune::Preconditioner<Domain,Range>::field_type s = dim==1? 1.0: dim==2? 1/12.0 : 1/60.0;
	  for (int i=0; i<coarseElement.size(); ++i)
	  {
	    //typename Domain::block_type eTdv = 0;
	    double eTdv = 0;
	    for (int m=0; m<=dim; ++m)
	      eTdv += at_c<0>(dv.data)[coarseElement[i][m]];
	    for (int m=0; m<=dim; ++m)
	      at_c<0>(r.data)[coarseElement[i][m]] -= a*s*(-eTdv+(dim+1.1)*at_c<0>(dv.data)[coarseElement[i][m]]);
	  }
	}
      }
  };

}

#endif


#include <iostream>
#include <cstdio>
#include <algorithm>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/connected_components.hpp>
// #include <boost/unordered_set.hpp>
#include <ctime>
#include <stack>
#include <queue>
#include <fstream>
// #include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/functional/hash.hpp>
#include <random>
#include <unordered_set>
#include <unordered_map>
// using namespace std;
using namespace boost;

typedef std::vector < std::vector <int> > vvi;

typedef adjacency_list< listS, vecS, undirectedS, no_property, property<edge_weight_t, float> > Graph;
// typedef adjacency_list< listS,vecS, undirectedS > Graph;
typedef graph_traits < Graph> ::vertex_descriptor vertex_descriptor;
typedef graph_traits < Graph> ::edge_descriptor edge_descriptor;
typedef Graph::vertex_iterator vertex_iterator;
typedef Graph::edge_iterator edge_iterator;
typedef std::pair <int, int> Edge;
typedef graph_traits < Graph> ::adjacency_iterator my_adjacency_iterator;
property_map<Graph, edge_weight_t>::type weight;

std::unordered_set<int> union_(std::unordered_set<int> &set1, std::unordered_set<int> &set2)
{
	// returns the union of two unordered sets of integers
	std::unordered_set<int> new_set = set1;

	for(auto thing : set2)
		new_set.insert(thing);
	return new_set;
}

std::vector< std::vector <int> > sample(std::vector <int> &things, int &k)
{
	// Returns the set of sampled nodes needed for the whole process of spanner construction as vector of integer vectors
	std::vector< std::vector<int> > master_v;
	std::vector<int> v = things;

	int i = 1, n = v.size(), s;
	float f;

	std::vector<int> counts;

	for(i = 1; i < k; i ++)
	{
		f = i / (float) k;
		s = ceil(pow(n, 1 - f));
		counts.push_back(s);
	}

	master_v.push_back(v);

	std::default_random_engine engine(std::random_device {}());
	std::shuffle(master_v[0].begin(), master_v[0].end(), engine);

	for(i = 1; i < k; i ++)
	{
		std::vector<int> temp;
		for(int j = 0; j < counts[i - 1]; j ++)
			temp.push_back(master_v[i - 1][j]);
		std::shuffle(temp.begin(), temp.end(), engine);
		master_v.push_back(temp);
	}
	return master_v;
}

tuple<Graph, Graph, Graph> read_graphs(std::string filename)
{
	// Reads the graphs from the edgelist. At first, the graphs G, G_comm and G_prime are all the same
	std::ifstream fp(filename);
	Graph G, G_comm, G_prime;
	int u, v;
	float w;

	while(fp >> u >> v >> w)
	{
		add_edge(u, v, w, G);
		add_edge(u, v, w, G_comm);
		add_edge(u, v, w, G_prime);
	}
	fp.close();
	return make_tuple(G, G_comm, G_prime);
}

Graph make_spanner(std::string filename, int k)
{
	// constructs the (2k - 1)-spanner using Baswana and Sen's randomized algorithm
	Graph G, G_comm, G_prime;
	Graph G_spanner;

	tie(G, G_comm, G_prime) = read_graphs(filename);

	std::unordered_set <std::pair<int, int>, hash< std::pair<int, int> > > old_Ei;
	std::unordered_set <std::pair<int, int>, hash< std::pair<int, int> > > new_Ei;

	std::unordered_set <int> v_prime_flag;
	std::unordered_map <int, int> membership_oldc;
	std::unordered_map <int, int> membership_newc;
	std::unordered_map <int, int> neighborhood;

	std::unordered_map <int, std::unordered_set <int> > old_c;

	std::pair <vertex_iterator, vertex_iterator> vp;

	std::vector <int> all_nodes;

	for(vp = vertices(G); vp.first != vp.second; ++vp.first) // iterating over all the nodes
	{
		auto node = *vp.first;
		old_c[node].insert(node);
		v_prime_flag.insert(node);
		membership_oldc[node] = node;
		membership_newc[node] = -1;
		all_nodes.push_back(node);
	}

	my_adjacency_iterator start, end;

	std::vector < std::vector<int> > R_vector; //this stores all the randomly sampled cluster heads for all iterations

	R_vector = sample(all_nodes, k); // R_vector stores the set of sampled nodes required at each iteration

	for(int i = 1; i < k; i ++)
	{
		/*****1. Forming a sample of clusters **********************/

		std::unordered_set<int> R_i(R_vector[i].begin(), R_vector[i].end()); // R_vector[i] gives the randomly picked cluster heads for ith iteration
		std::unordered_map <int, std::unordered_set <int> > new_c;

		for(auto item : old_c)
		{
			int v = item.first;

			if(R_i.find(v) != R_i.end())
				new_c[v] = old_c[v];
		}

		std::unordered_set <int> sampled_nodes;

		for(auto v : R_i)
			sampled_nodes = union_(sampled_nodes, old_c[v]);

		std::unordered_set <int> unsampled_nodes;

		for(vp = vertices(G_prime); vp.first != vp.second; ++vp.first)
		{
			int node = *(vp.first);
			if(sampled_nodes.find(node) == sampled_nodes.end())
				unsampled_nodes.insert(node);
		}

		neighborhood.clear();
		for(auto node : unsampled_nodes)
			neighborhood[node] = -1;

		if(i == 1)
			for(auto v : R_i)
				membership_newc[v] = v;

		else
		{
			std::pair <vertex_iterator, vertex_iterator> vp;
			for(vp = vertices(G_prime); vp.first != vp.second; ++ vp.first)
			{
				auto v = *(vp.first);
				if(membership_newc[v] != -1)
					if(R_i.find(membership_newc[v]) == R_i.end())
						membership_newc[v] = -1;
			}
		}

		new_Ei = old_Ei;
		int u, v;

		for(auto edge : old_Ei)
		{
			tie(u, v) = edge;
			if(R_i.find(membership_newc[u]) == R_i.end() || R_i.find(membership_newc[v]) == R_i.end())
			{
				std::pair<int, int> e = std::make_pair(u, v);
				new_Ei.erase(e);
			}
		}

		/***********************end of step 1**************************/

		/********2. finding nearest neighboring sampled cluster*********/

		for(int v : unsampled_nodes)
		{
			if(v_prime_flag.find(v) == v_prime_flag.end()) // if v is not in G_prime, we don't consider it
				continue;

			std::unordered_map <int, std::pair<float, int> > min_table;
			edge_descriptor e;
			vertex_descriptor src, dest;

			for(std::tie(start, end) = adjacent_vertices(v, G_prime); start != end; ++ start)
			{
				int neighbor = *start;
				float wt;
				src = vertex(v, G);
				dest = vertex(neighbor, G);

				std::tie(e, std::ignore) = edge(src, dest, G);

				wt = get(weight, e);

				if(membership_newc[neighbor] != -1) // checking if the neighbor is sampled
				{
					if(min_table.find(membership_newc[neighbor]) == min_table.end())
						min_table[membership_newc[neighbor]] = std::make_pair(wt, neighbor);

					else if(wt < min_table[membership_newc[neighbor]].first)
					{
						min_table[membership_newc[neighbor]].first = wt;
						min_table[membership_newc[neighbor]].second = neighbor;
					}
				}
			}

			float min_ = INT_MAX;
			int nearest_neighbor = -1;

			for(auto item : min_table)
			{
				if(item.second.first  < min_)
				{
					min_ = item.second.first;
					nearest_neighbor = item.second.second;
				}
			}

			neighborhood[v] = nearest_neighbor;
		}

		/************end of step 2 ********************************/

		/* **********3. adding edges to spanner ******************/

		for(int v : unsampled_nodes)
		{
			if(v_prime_flag.find(v) == v_prime_flag.end())
				continue;

			if(neighborhood[v] == -1) //v is not adjacent to any sampled nodes
			{
				std::unordered_map <int, std::pair<float, int> > min_table;

				edge_descriptor e;
				vertex_descriptor src, dest;
				std::vector <std::pair<int, int> > edges_to_be_removed;
				my_adjacency_iterator start, end;

				for(std::tie(start, end) = adjacent_vertices(v, G_prime); start != end; ++ start)
				{
					int neighbor = *start;
					float wt;

					src = vertex(v, G);
					dest = vertex(neighbor, G);

					std::tie(e, std::ignore) = edge(src, dest, G);

					wt = get(weight, e);

					if(membership_oldc[neighbor] != -1)
					{
						if(min_table.find(neighbor) == min_table.end())
							min_table[neighbor] = std::make_pair(wt, neighbor);

						if(wt <= min_table[neighbor].first)
						{
							min_table[neighbor].first = wt;
							min_table[neighbor].second = neighbor;

							edges_to_be_removed.push_back(std::make_pair(v, neighbor));
						}
					}
				}

				for(auto edge : edges_to_be_removed)
					remove_edge(edge.first, edge.second, G_prime);

				for(auto item : min_table)
				{
					add_edge(v, item.second.second, item.second.first, G_spanner);
					remove_edge(v, item.second.second, G_comm);
				}
			}

			else
			{
				my_adjacency_iterator start, end;
				float wt;

				edge_descriptor e;
				std::tie(e, std::ignore) = edge(v, neighborhood[v], G);
				wt = get(weight, e);

				add_edge(v, neighborhood[v], wt, G_spanner);
				remove_edge(v, neighborhood[v], G_comm);

				new_Ei.insert(std::make_pair(v, neighborhood[v]));

				std::vector <int> neighbors;
				for(std::tie(start, end) = adjacent_vertices(v, G_prime); start != end; ++ start)
				{
					int neighbor = *start;
					neighbors.push_back(neighbor);
				}

				for(auto neighbor : neighbors)
				{
					if(membership_newc[neighbor] == membership_newc[neighborhood[v]])
						remove_edge(v, neighbor, G_prime);
				}

				std::unordered_map <int, std::pair<float, int> > min_table;
				edge_descriptor e1;
				vertex_descriptor src, dest;

				for(auto neighbor : neighbors)
				{
					float wt1, wt2;
					src = vertex(v, G);
					dest = vertex(neighbor, G);

					std::tie(e1, std::ignore) = edge(src, dest, G);
					wt1 = get(weight, e1);

					dest = vertex(neighborhood[v], G);
					std::tie(e1, std::ignore) = edge(src, dest, G);
					wt2 = get(weight, e1);

					if(wt1 < wt2)
					{
						if(min_table.find(membership_oldc[neighbor]) == min_table.end())
							min_table[membership_oldc[neighbor]] = std::make_pair(wt1, neighbor);

						else if(wt1 < min_table[membership_oldc[neighbor]].first)
						{
							min_table[membership_oldc[neighbor]].first = wt2;
							min_table[membership_oldc[neighbor]].second = neighbor;
							remove_edge(v, neighbor, G_prime);
						}
					}
				}

				for(auto item : min_table)
				{
					add_edge(v, item.second.second, item.second.first, G_spanner);
					remove_edge(v, item.second.second, G_comm);
					remove_edge(v, item.second.second, G_prime);
				}

			}

		}

		/************end of step 3*****************************************/

		/******4. removing intra cluster edges ***********************/

		std::vector< std::pair <int, int> > intracluster_edges;

		for(auto item : new_Ei)
		{
			int u = item.first;
			int v = item.second;

			if(membership_newc[u] != -1)
			{
				membership_newc[v] = membership_newc[u];
				new_c[membership_newc[u]].insert(v);
			}

			else if(membership_newc[v] != -1)
			{
				membership_newc[u] = membership_newc[v];
				new_c[membership_newc[v]].insert(u);
			}
		}

		edge_iterator e_start, e_end;

		for(std::tie(e_start, e_end) = edges(G_prime); e_start != e_end; ++ e_start)
		{
			int u = source(*e_start, G_prime), v = target(*e_start, G_prime);

			if(membership_newc[u] == membership_newc[v])
				intracluster_edges.push_back(std::make_pair(u, v));
		}

		for(auto edge : intracluster_edges)
		{
			// // std:: cout << "\n(" << edge.first << ", " << edge.second << ")";
			remove_edge(edge.first, edge.second, G_prime);
		}

		/*************end of step 4*************************************************/

		/**** updations needed at the end of iteration******************************/

		v_prime_flag.clear();

		int count = 0;
		// // std:: cout << "E_prime: \n";
		for(std::tie(e_start, e_end) = edges(G_prime); e_start != e_end; ++ e_start)
		{
			int u = source(*e_start, G_prime), v = target(*e_start, G_prime);
			v_prime_flag.insert(u);
			v_prime_flag.insert(v);
			count ++;
		}

		for(auto item : new_Ei)
		{
			v_prime_flag.insert(item.first);
			v_prime_flag.insert(item.second);
		}

		old_c = new_c;
		membership_oldc = membership_newc;

		/***********The end of iteration i *****************************/
	}

	/******* Phase 2 **************************/

	vertex_iterator v_start, v_end;

	for(std::tie(v_start, v_end) = vertices(G_prime); v_start != v_end; ++ v_start)
	{
		int v = *v_start;
		if(v_prime_flag.find(v) == v_prime_flag.end())
			continue;

		std::unordered_map <int, std::pair<float, int> > min_table;
		edge_descriptor e;
		vertex_descriptor src, dest;

		std::vector <std::pair<int, int> > edges_to_be_removed;

		for(std::tie(start, end) = adjacent_vertices(v, G_prime); start != end; ++ start)
		{
			int neighbor = *start;
			float wt;
			src = vertex(v, G);
			dest = vertex(neighbor, G);

			std::tie(e, std::ignore) = edge(src, dest, G);

			wt = get(weight, e);

			if(membership_newc[neighbor] == -1)
				continue;

			if(min_table.find(membership_newc[neighbor]) == min_table.end())
				min_table[membership_newc[neighbor]] = std::make_pair(wt, neighbor);

			if(wt <= min_table[membership_newc[neighbor]].first)
			{
				min_table[membership_newc[neighbor]].first = wt;
				min_table[membership_newc[neighbor]].second = neighbor;

				edges_to_be_removed.push_back(std::make_pair(v, neighbor));
			}

			if(min_table.find(membership_newc[neighbor]) == min_table.end())
				edges_to_be_removed.push_back(std::make_pair(v, neighbor));
		}

		for(auto edge : edges_to_be_removed)
			remove_edge(edge.first, edge.second, G_prime);

		for(auto item : min_table)
		{
			add_edge(v, item.second.second, item.second.first, G_spanner);
			remove_edge(v, item.second.second, G_comm);
		}
	}
	return G_comm;
}

float THRESHOLD = 0.66;

std::stack <int> Broker_Stack;
std::queue <int> Community_Queue;
std::unordered_set <int> isolated;
std::unordered_set <int> influenced;
std::unordered_map<int, int> labels;
std::unordered_set <int> traversed;
std::unordered_set <int> brokers;
std::unordered_set <int> marked;
std::unordered_map<int, float> scores;

float INS_score(Graph &g, int node)
{
	int count = 0;
	int total_count = 0;

	my_adjacency_iterator start, end;
	vertex_descriptor v = vertex(node, g);

	for(std::tie(start, end) = adjacent_vertices(v, g); start != end; ++ start)
	{
		auto neighbor = *start;
		if(influenced.find(neighbor) != influenced.end())
			count += 1;
		total_count += 1;
	}

	return count * 1.0 / total_count;
}

void INS(Graph &g, int starting_node)
{
	vertex_iterator v_st, v_end;
	influenced.insert(starting_node);
	traversed.insert(starting_node);
	Broker_Stack.push(starting_node);
	brokers.insert(starting_node);
	scores[starting_node] = 0;
	labels[starting_node] = starting_node;

	my_adjacency_iterator start, end;

	while(Broker_Stack.size() + Community_Queue.size() > 0)
	{
		int node;
		if(! Community_Queue.empty())
		{
			node = Community_Queue.front();
			Community_Queue.pop();
		}
		else
		{
			node = Broker_Stack.top();
			Broker_Stack.pop();
		}

		isolated.erase(node);

		vertex_descriptor v = vertex(node, g);

		for(std::tie(start, end) = adjacent_vertices(v, g); start != end; ++ start)
			influenced.insert(*start);

		for(std::tie(start, end) = adjacent_vertices(v, g); start != end; ++ start)
		{
			int neighbor = *start;

			if(traversed.find(neighbor) != traversed.end())
				continue;
			else
			{
				traversed.insert(neighbor);
			}

			auto score = INS_score(g, neighbor);
			// std:: cout << "node: " << neighbor << " score: " << score << std:: endl;
			scores[neighbor] = score;

			if(score < THRESHOLD) //broker
			{
				labels[neighbor] = neighbor;
				Broker_Stack.push(neighbor);
				brokers.insert(neighbor);
			}

			else
			{
				if(marked.find(neighbor) == marked.end())
				{
					labels[neighbor] = labels[node];
					marked.insert(neighbor);
				}

				isolated.erase(neighbor);
				if(score != 1)
					Community_Queue.push(neighbor);
			}
		}
	}
}

void run_INS(Graph &G, std::string filename)
{
	for(unsigned int i = 0; i < num_vertices(G); i ++)
		isolated.insert(i);

	std::vector<int> component(num_vertices(G));
	int num = connected_components(G, &component[0]);

	vvi comp_lst(num);
	for(unsigned int i = 0; i != component.size(); ++i)
		comp_lst[component[i]].push_back(i);

	std::vector <int> starting_nodes;
	int min_deg = INT_MAX;
	int starting_node, deg;

	for(int i = 0; i < num; i ++)
	{
		starting_node = -1;
		min_deg = INT_MAX;
		for(auto node : comp_lst[i])
		{
			vertex_descriptor v = vertex(node, G);
			deg = degree(v, G);
			if(deg == 1)
			{
				starting_node = node;
				min_deg = deg;
				break;
			}
			else if(deg < min_deg)
			{
				starting_node = node;
				min_deg = deg;
			}
		}
		starting_nodes.push_back(starting_node);
	}

	for(auto start_node : starting_nodes)
		INS(G, start_node);

	std::string st = filename;
	std::ofstream fout(st + "_cover.part");

	for(auto it = labels.begin(); it != labels.end(); ++ it)
	{
		fout << it->first << " " << it->second << std::endl;
	}

	for(auto thing : isolated)
	{
		fout << thing << " " << thing << std::endl;
	}
	fout.close();

}

int main(int argc, char const *argv[])
{
	if(argc < 3)
	{
		std::cout << "Enter filename and k";
		return 0;
	}
	Graph G_comm;
	G_comm = make_spanner(argv[1], std::stoi(argv[2]));

	run_INS(G_comm, argv[1]);
	return 0;
}


/*
 * WaveEquationBase.cpp
 *
 *  Created on: 23.07.2017
 *      Author: thies
 */

#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/numerics/matrix_tools.h>
#include <forward/MatrixCreator.h>
#include <forward/WaveEquationBase.h>

namespace wavepi {
namespace forward {

using namespace dealii;
using namespace wavepi::base;

template <int dim>
void WaveEquationBase<dim>::fill_matrices(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx,
                                          SparseMatrix<double> &dst_A, SparseMatrix<double> &dst_B,
                                          SparseMatrix<double> &dst_C) {
  //   const double time = mesh->get_time(time_idx);

  // this helps only a bit because each of the operations is already parallelized
  // tests show about 20%-30% (depending on dim) speedup on my Intel i5 4690
  // this assembling could be done even more efficient, by looping through the mesh once and assembling all matrices.

  //   Threads::TaskGroup<void> task_group;

  if (!rho_time_dependent || time_idx == mesh->length() - 1) {
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_A, *this, mesh, time_idx, dst_A);
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_B, *this, mesh, time_idx, dst_B);
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_C, *this, mesh, time_idx, dst_C);

    this->fill_A(mesh, time_idx, dst_A);
    this->fill_B(mesh, time_idx, dst_B);
    this->fill_C(mesh, time_idx, dst_C);

    matrix_C_intermediate.clear();
    matrix_D_intermediate.clear();
  } else {
    // possible here because fill_*_intermediate can access different time steps of ρ in parallel
    // (ρ is discretized and this is exploited by assembly tasks)

    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_A, *this, mesh, time_idx, dst_A);
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_B, *this, mesh, time_idx, dst_B);
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_C, *this, mesh, time_idx, dst_C);

    this->fill_A(mesh, time_idx, dst_A);
    this->fill_B(mesh, time_idx, dst_B);
    this->fill_C(mesh, time_idx, dst_C);

    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_C_intermediate, *this, mesh, time_idx);
    //      task_group += Threads::new_task(&WaveEquationBase<dim>::fill_D_intermediate, *this, mesh, time_idx);

    this->fill_C_intermediate(mesh, time_idx);
    this->fill_D_intermediate(mesh, time_idx);
  }

  //   task_group.join_all();
}

// let dst <- (D^n)^{-1} D^{n-1} M^{-1} src
// ( i.e. dst <- src for time-independent D)
template <int dim>
void WaveEquationBase<dim>::vmult_D_intermediate(const SparseMatrix<double> &mass_matrix, Vector<double> &dst,
                                                 const Vector<double> &src, double tolerance) const {
  if (rho_time_dependent) {
    Vector<double> tmp(src.size());

    SolverControl solver_control(2000, tolerance * src.l2_norm());
    SolverCG<> cg(solver_control);

    // PreconditionIdentity precondition = PreconditionIdentity();
    PreconditionSSOR<SparseMatrix<double>> precondition;
    precondition.initialize(mass_matrix, PreconditionSSOR<SparseMatrix<double>>::AdditionalData(1.0));

    cg.solve(mass_matrix, tmp, src, precondition);

    AssertThrow(matrix_D_intermediate.n() > 0, ExcInternalError("matrix_D_intermediate is missing"));
    matrix_D_intermediate.vmult(dst, tmp);
  } else {
    dst.equ(1.0, src);
  }
}

// let dst <- M^{-1} (D^n)^{-1} D^{n-1} src
// ( i.e. dst <- src for time-independent D)
template <int dim>
void WaveEquationBase<dim>::vmult_D_intermediate_transpose(const SparseMatrix<double> &mass_matrix, Vector<double> &dst,
                                                           const Vector<double> &src, double tolerance) const {
  if (rho_time_dependent) {
    Vector<double> tmp(src.size());

    AssertThrow(matrix_D_intermediate.n() > 0, ExcInternalError("matrix_D_intermediate is missing"));
    matrix_D_intermediate.vmult(tmp, src);

    SolverControl solver_control(2000, tolerance * tmp.l2_norm());
    SolverCG<> cg(solver_control);

    // PreconditionIdentity precondition = PreconditionIdentity();
    PreconditionSSOR<SparseMatrix<double>> precondition;
    precondition.initialize(mass_matrix, PreconditionSSOR<SparseMatrix<double>>::AdditionalData(1.0));

    cg.solve(mass_matrix, dst, tmp, precondition);
  } else {
    dst.equ(1.0, src);
  }
}

// before mesh change, let dst <- (D^n)^{-1} C^{n-1} src
// ( i.e. dst <- matrix_C * src for time-independent D)
template <int dim>
void WaveEquationBase<dim>::vmult_C_intermediate(const SparseMatrix<double> &matrix_C, Vector<double> &dst,
                                                 const Vector<double> &src) const {
  if (rho_time_dependent) {
    AssertThrow(matrix_C_intermediate.n() > 0, ExcInternalError("matrix_C_intermediate is missing"));
    matrix_C_intermediate.vmult(dst, src);
  } else {
    matrix_C.vmult(dst, src);
  }
}

template <int dim>
void WaveEquationBase<dim>::fill_A(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx,
                                   SparseMatrix<double> &destination) {
  const double time = mesh->get_time(time_idx);
  auto dof_handler  = mesh->get_dof_handler(time_idx);

  if ((!param_rho_disc && !param_q_disc) || !using_special_assembly(mesh))
    MatrixCreator<dim>::create_A_matrix(dof_handler, mesh->get_quadrature(), destination, param_rho, param_q, time);
  else if (param_rho_disc && !param_q_disc)
    MatrixCreator<dim>::create_A_matrix(dof_handler, mesh->get_quadrature(), destination,
                                        param_rho_disc->get_function_coefficients_by_time(time), param_q, time);
  else if (!param_rho_disc && param_q_disc)
    MatrixCreator<dim>::create_A_matrix(dof_handler, mesh->get_quadrature(), destination, param_rho,
                                        param_q_disc->get_function_coefficients_by_time(time), time);
  else
    // (param_rho_disc && param_q_disc)
    MatrixCreator<dim>::create_A_matrix(dof_handler, mesh->get_quadrature(), destination,
                                        param_rho_disc->get_function_coefficients_by_time(time),
                                        param_q_disc->get_function_coefficients_by_time(time));
}

template <int dim>
void WaveEquationBase<dim>::fill_B(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx,
                                   SparseMatrix<double> &destination) {
  const double time = mesh->get_time(time_idx);
  auto dof_handler  = mesh->get_dof_handler(time_idx);

  if (param_nu_disc && using_special_assembly(mesh))
    MatrixCreator<dim>::create_mass_matrix(dof_handler, mesh->get_quadrature(), destination,
                                           param_nu_disc->get_function_coefficients_by_time(time));
  else
    MatrixCreator<dim>::create_mass_matrix(dof_handler, mesh->get_quadrature(), destination, param_nu, time);
}

template <int dim>
void WaveEquationBase<dim>::fill_C(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx,
                                   SparseMatrix<double> &destination) {
  const double time = mesh->get_time(time_idx);
  auto dof_handler  = mesh->get_dof_handler(time_idx);

  if ((!param_rho_disc && !param_c_disc) || !using_special_assembly(mesh))
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), destination, param_rho, param_c, time,
                                        time);
  else if (param_rho_disc && !param_c_disc)
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), destination,
                                        param_rho_disc->get_function_coefficients_by_time(time), param_c, time);
  else if (!param_rho_disc && param_c_disc)
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), destination, param_rho,
                                        param_c_disc->get_function_coefficients_by_time(time), time);
  else
    // (param_rho_disc && param_c_disc)
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), destination,
                                        param_rho_disc->get_function_coefficients_by_time(time),
                                        param_c_disc->get_function_coefficients_by_time(time));
}

template <int dim>
void WaveEquationBase<dim>::fill_C_intermediate(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx) {
  // fill with (D^{n+1})^{-1} C^n, n = time_idx on the mesh of time_idx
  // -> needs to be able to change the time in ρ to the next time if continuous ρ is used

  matrix_C_intermediate.reinit(*mesh->get_sparsity_pattern(time_idx));
  double current_time = mesh->get_time(time_idx);
  double next_time    = mesh->get_time(time_idx + 1);  // does range checking in debug mode
  auto dof_handler    = mesh->get_dof_handler(time_idx);

  if ((!param_rho_disc && !param_c_disc) || !using_special_assembly(mesh))
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), matrix_C_intermediate, param_rho, param_c,
                                        next_time, current_time);
  else if (param_rho_disc && !param_c_disc)
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), matrix_C_intermediate,
                                        param_rho_disc->get_function_coefficients_by_time(next_time), param_c,
                                        current_time);

  else if (!param_rho_disc && param_c_disc)
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), matrix_C_intermediate, param_rho,
                                        param_c_disc->get_function_coefficients_by_time(current_time), next_time);
  else
    MatrixCreator<dim>::create_C_matrix(dof_handler, mesh->get_quadrature(), matrix_C_intermediate,
                                        param_rho_disc->get_function_coefficients_by_time(next_time),
                                        param_c_disc->get_function_coefficients_by_time(current_time));
}

template <int dim>
void WaveEquationBase<dim>::fill_D_intermediate(std::shared_ptr<SpaceTimeMesh<dim>> mesh, size_t time_idx) {
  // fill with (D^{n+1})^{-1} D^n, n = time_idx on the mesh of time_idx
  // -> needs to be able to change the time in ρ to the next time if continuous ρ is used

  matrix_D_intermediate.reinit(*mesh->get_sparsity_pattern(time_idx));
  double current_time = mesh->get_time(time_idx);
  double next_time    = mesh->get_time(time_idx + 1);  // does range checking in debug mode
  auto dof_handler    = mesh->get_dof_handler(time_idx);

  if (!param_rho_disc || !using_special_assembly(mesh))
    MatrixCreator<dim>::create_D_intermediate_matrix(dof_handler, mesh->get_quadrature(), matrix_D_intermediate,
                                                     param_rho, current_time, next_time);
  else
    MatrixCreator<dim>::create_D_intermediate_matrix(dof_handler, mesh->get_quadrature(), matrix_D_intermediate,
                                                     param_rho_disc->get_function_coefficients_by_time(current_time),
                                                     param_rho_disc->get_function_coefficients_by_time(next_time));
}

template class WaveEquationBase<1>;
template class WaveEquationBase<2>;
template class WaveEquationBase<3>;

} /* namespace forward */
} /* namespace wavepi */