#include "GridUtils.h"

#include <queue>
#include <Eigen/SparseCore>
#include <Eigen/IterativeLinearSolvers>
#include <cassert>
#include <LBFGS.h>

#include "GradientDescent.h"

ar::real ar::GridUtils2D::getLinearClampedF(const real* linear, real x, real y, int width, int height)
{
    int ix = static_cast<int>(x);
    int iy = static_cast<int>(y);
    real fx = x - ix;
    real fy = y - iy;
    real v00 = getLinearClamped(linear, ix, iy, width, height);
    real v10 = getLinearClamped(linear, ix + 1, iy, width, height);
    real v01 = getLinearClamped(linear, ix, iy + 1, width, height);
    real v11 = getLinearClamped(linear, ix + 1, iy + 1, width, height);
    real v0 = v00 + fx * (v01 - v00); //correct???
    real v1 = v10 + fx * (v11 - v10);
    return v0 + fy * (v1 - v0);
}

const ar::real& ar::GridUtils2D::getClamped(const grid_t& grid, int x, int y)
{
    int width = (int)grid.rows();
    int height = (int)grid.cols();
    clampCoord(x, y, width, height);
    return grid(x, y);
}

ar::real& ar::GridUtils2D::atClamped(grid_t& grid, int x, int y)
{
    int width = (int)grid.rows();
    int height = (int)grid.cols();
    clampCoord(x, y, width, height);
    return grid(x, y);
}

ar::real ar::GridUtils2D::getClampedF(const grid_t& grid, real x, real y)
{
    int ix = static_cast<int>(x);
    int iy = static_cast<int>(y);
    real fx = x - ix;
    real fy = y - iy;
    real v00 = getClamped(grid, ix, iy);
    real v10 = getClamped(grid, ix + 1, iy);
    real v01 = getClamped(grid, ix, iy + 1);
    real v11 = getClamped(grid, ix + 1, iy + 1);
    real v0 = v00 + fy * (v01 - v00);
    real v1 = v10 + fy * (v11 - v10);
    return v0 + fx * (v1 - v0);
}

ar::GridUtils2D::grad_t ar::GridUtils2D::getGradientLinear(const real* linear, int x, int y, int width, int height)
{
    grad_t grad;
    //x
    if (x == 0)
        grad.x() = getLinear(linear, x + 1, y, width) - getLinear(linear, x, y, width); //forward
    else if (x == width - 1)
        grad.x() = getLinear(linear, x, y, width) - getLinear(linear, x - 1, y, width); //backward
    else
        grad.x() = 0.5 * (getLinear(linear, x + 1, y, width) - getLinear(linear, x - 1, y, width)); //central

    //y
    if (y == 0)
        grad.y() = getLinear(linear, x, y + 1, width) - getLinear(linear, x, y, width); //forward
    else if (y == height - 1)
        grad.y() = getLinear(linear, x, y, width) - getLinear(linear, x, y - 1, width); //backward
    else
        grad.y() = 0.5 * (getLinear(linear, x, y + 1, width) - getLinear(linear, x, y - 1, width)); //central

    return grad;
}

ar::GridUtils2D::grad_t ar::GridUtils2D::getGradient(const grid_t& grid, Eigen::Index i, Eigen::Index j)
{
	grad_t grad;
	//x
	if (i == 0)
		grad.x() = grid(i + 1, j) - grid(i, j); //forward
	else if (i == grid.rows() - 1)
		grad.x() = grid(i, j) - grid(i - 1, j); //backward
	else
		grad.x() = 0.5 * (grid(i + 1, j) - grid(i - 1, j)); //central

	//y
	if (j == 0)
		grad.y() = grid(i, j + 1) - grid(i, j); //forward
	else if (j == grid.cols() - 1)
		grad.y() = grid(i, j) - grid(i, j - 1); //backward
	else
		grad.y() = 0.5 * (grid(i, j + 1) - grid(i, j - 1)); //central

	return grad;
}

ar::real ar::GridUtils2D::getGradientPosLinear(const real* linear, int x, int y, int width, int height)
{
    real val = real(0);
    //x
    if (x == 0)
        val += ar::utils::sqr(getLinear(linear, x + 1, y, width) - getLinear(linear, x, y, width));
    else if (x == width - 1)
        val += ar::utils::sqr(getLinear(linear, x, y, width) - getLinear(linear, x - 1, y, width));
    else
        val += ar::utils::sqr(std::max(real(0), getLinear(linear, x, y, width) - getLinear(linear, x - 1, y, width)))
            + ar::utils::sqr(std::min(real(0), getLinear(linear, x + 1, y, width) - getLinear(linear, x, y, width)));

    //y
    if (y == 0)
        val += ar::utils::sqr(getLinear(linear, x, y + 1, width) - getLinear(linear, x, y, width));
    else if (y == height - 1)
        val += ar::utils::sqr(getLinear(linear, x, y, width) - getLinear(linear, x, y - 1, width));
    else
        val += ar::utils::sqr(std::max(real(0), getLinear(linear, x, y, width) - getLinear(linear, x, y - 1, width)))
            + ar::utils::sqr(std::min(real(0), getLinear(linear, x, y + 1, width) - getLinear(linear, x, y, width)));

    return std::sqrt(val);
}

ar::real ar::GridUtils2D::getGradientNegLinear(const real* linear, int x, int y, int width, int height)
{
    real val = real(0);
    //x
    if (x == 0)
        val += ar::utils::sqr(getLinear(linear, x + 1, y, width) - getLinear(linear, x, y, width));
    else if (x == width - 1)
        val += ar::utils::sqr(getLinear(linear, x, y, width) - getLinear(linear, x - 1, y, width));
    else
        val += ar::utils::sqr(std::min(real(0), getLinear(linear, x, y, width) - getLinear(linear, x - 1, y, width)))
            + ar::utils::sqr(std::max(real(0), getLinear(linear, x + 1, y, width) - getLinear(linear, x, y, width)));

    //y
    if (y == 0)
        val += ar::utils::sqr(getLinear(linear, x, y + 1, width) - getLinear(linear, x, y, width));
    else if (y == height - 1)
        val += ar::utils::sqr(getLinear(linear, x, y, width) - getLinear(linear, x, y - 1, width));
    else
        val += ar::utils::sqr(std::min(real(0), getLinear(linear, x, y, width) - getLinear(linear, x, y - 1, width)))
            + ar::utils::sqr(std::max(real(0), getLinear(linear, x, y + 1, width) - getLinear(linear, x, y, width)));

    return std::sqrt(val);
}

ar::GridUtils2D::grad_t ar::GridUtils2D::getGradientLinearF(const real* linear, real x, real y, int width, int height)
{
    grad_t grad;
    grad.x() = getLinearClampedF(linear, x + 0.5f, y, width, height) - getLinearClampedF(
        linear, x - 0.5f, y, width, height);
    grad.y() = getLinearClampedF(linear, x, y + 0.5f, width, height) - getLinearClampedF(
        linear, x, y - 0.5f, width, height);
    return grad;
}

ar::real ar::GridUtils2D::getLaplacianLinear(const real* linear, int x, int y, int width, int height)
{
    real laplacian = real(0);
    //x
    if (x == 0)
        laplacian += getLinear(linear, x, y, width) - 2 * getLinear(linear, x + 1, y, width) + getLinear(
            linear, x + 2, y, width);
    else if (x == width - 1)
        laplacian += getLinear(linear, x - 2, y, width) - 2 * getLinear(linear, x - 1, y, width) + getLinear(
            linear, x, y, width);
    else
        laplacian += getLinear(linear, x - 1, y, width) - 2 * getLinear(linear, x, y, width) + getLinear(
            linear, x + 1, y, width);

    //y
    if (y == 0)
        laplacian += getLinear(linear, x, y, width) - 2 * getLinear(linear, x, y + 1, width) + getLinear(
            linear, x, y + 2, width);
    else if (y == height - 1)
        laplacian += getLinear(linear, x, y - 2, width) - 2 * getLinear(linear, x, y - 1, width) + getLinear(
            linear, x, y, width);
    else
        laplacian += getLinear(linear, x, y - 1, width) - 2 * getLinear(linear, x, y, width) + getLinear(
            linear, x, y + 1, width);

    return laplacian;
}

ar::real ar::GridUtils2D::getLaplacian(const grid_t & grid, Eigen::Index i, Eigen::Index j)
{
	real laplacian = real(0);

	//x
	if (i == 0)
		laplacian += grid(i, j) - 2 * grid(i + 1, j) + grid(i + 2, j);
	else if (i == grid.rows() - 1)
		laplacian += grid(i - 2, j) - 2 * grid(i - 1, j) + grid(i, j);
	else
		laplacian += grid(i - 1, j) - 2 * grid(i, j) + grid(i + 1, j);

	//j
	if (j == 0)
		laplacian += grid(i, j) - 2 * grid(i, j + 1) + grid(i, j + 2);
	else if (j == grid.cols() - 1)
		laplacian += grid(i, j - 2) - 2 * grid(i, j - 1) + grid(i, j);
	else
		laplacian += grid(i, j - 1) - 2 * grid(i, j) + grid(i, j + 1);

	return laplacian;
}

std::pair<ar::GridUtils2D::grid_t, ar::GridUtils2D::grid_t> ar::GridUtils2D::getFullGradient(const grid_t & v)
{
	Eigen::Index rows = v.rows();
	Eigen::Index cols = v.cols();
	grid_t dx(rows, cols);
	grid_t dy(rows, cols);

	for (Eigen::Index j = 0; j<cols; ++j)
	{
		for (Eigen::Index i = 0; i<rows; ++i)
		{
			grad_t g = getGradient(v, i, j);
			dx(i, j) = g.x();
			dy(i, j) = g.y();
		}
	}

	return std::make_pair(dx, dy);
}

ar::GridUtils2D::grid_t ar::GridUtils2D::getFullLaplacian(const grid_t & v)
{
	Eigen::Index rows = v.rows();
	Eigen::Index cols = v.cols();
	grid_t laplacian(rows, cols);

	for (Eigen::Index j = 0; j<cols; ++j)
	{
		for (Eigen::Index i = 0; i<rows; ++i)
		{
			laplacian(i, j) = getLaplacian(v, i, j);
		}
	}

	return laplacian;
}

ar::GridUtils2D::grid_t ar::GridUtils2D::fillGrid(const grid_t& input, const bgrid_t& valid)
{
    int width = (int)input.rows();
    int height = (int)input.cols();
    grid_t grids[2] = {input, grid_t(width, height)};
    bgrid_t filled[2] = {valid, bgrid_t(width, height)};
    for (int i = 0; ; ++i)
    {
        bool hasChanges = false;
        int ci = i % 2;
        int ni = 1 - ci;
        for (int y = 0; y < height; ++y)
        {
            for (int x = 0; x < width; ++x)
            {
                if (filled[ci](x, y))
                {
                    filled[ni](x, y) = true;
                    grids[ni](x, y) = grids[ci](x, y);
                    continue;
                }
                real avg = real(0);
                int count = 0;
                if (x - 1 >= 0 && filled[ci](x - 1, y))
                {
                    avg += grids[ci](x - 1, y);
                    count++;
                }
                if (x + 1 < width && filled[ci](x + 1, y))
                {
                    avg += grids[ci](x + 1, y);
                    count++;
                }
                if (y - 1 >= 0 && filled[ci](x, y - 1))
                {
                    avg += grids[ci](x, y - 1);
                    count++;
                }
                if (y + 1 < height && filled[ci](x, y + 1))
                {
                    avg += grids[ci](x, y + 1);
                    count++;
                }
                if (count > 0)
                {
                    filled[ni](x, y) = true;
                    grids[ni](x, y) = avg / count;
                    hasChanges = true;
                }
                else
                    filled[ni](x, y) = false;
            }
        }
        if (!hasChanges)
        {
            return grids[ni];
        }
    }
}

ar::GridUtils2D::grid_t ar::GridUtils2D::fillGridDiffusion(const grid_t& input, const bgrid_t& valid)
{
    //find number of unknowns
    int width = (int)input.rows();
    int height = (int)input.cols();
	Eigen::Index numEmpty = 0;
	igrid_t indices = igrid_t::Constant(width, height, -1);
	for (int y = 0; y < height; ++y)
	{
		for (int x = 0; x < width; ++x)
		{
			if (!valid(x, y)) {
				indices(x, y) = numEmpty;
				numEmpty++;
			}
		}
	}

    //build matrix
    std::vector<Eigen::Triplet<real>> entries;
	entries.reserve(5 * numEmpty);
	VectorX rhs = VectorX::Zero(numEmpty);
	const int neighborsX[] = { -1, 1, 0, 0 };
	const int neighborsY[] = { 0, 0, -1, 1 };
	for (int y = 0; y < height; ++y)
	{
		for (int x = 0; x < width; ++x)
		{
			if (!valid(x, y)) {
				int row = indices(x, y);
				int c = 0;
				for (int i = 0; i < 4; ++i) {
					int ix = x + neighborsX[i];
					int iy = y + neighborsY[i];
					if (ix < 0 || ix >= width || iy < 0 || iy >= height) continue; //outside, Neumann boundary
					if (valid(ix, iy)) {
						//dirichlet boundary
						rhs[indices(x, y)] += input(ix, iy);
					}
					else {
						//inside
						entries.emplace_back(row, indices(ix, iy), -1);
					}
					c++;
				}
				entries.emplace_back(row, row, c);
			}
		}
	}
	Eigen::SparseMatrix<real> M(numEmpty, numEmpty);
	M.setFromTriplets(entries.begin(), entries.end());

    //Solve it
	Eigen::ConjugateGradient<Eigen::SparseMatrix<real>> cg(M);
	VectorX r = cg.solve(rhs);

	//Map back
	grid_t result(width, height);
	for (int y = 0; y < height; ++y)
	{
		for (int x = 0; x < width; ++x)
		{
			if (!valid(x, y))
				result(x, y) = r[indices(x, y)];
			else
				result(x, y) = input(x, y);
		}
	}
	return result;
}

void ar::GridUtils2D::fillGridDiffusionAdjoint(grid_t& adjInput, const grid_t& adjOutput, const grid_t& input,
    const bgrid_t& valid)
{
    //find number of unknowns
    int width = (int)input.rows();
    int height = (int)input.cols();
    Eigen::Index numEmpty = 0;
    igrid_t indices = igrid_t::Constant(width, height, -1);
    for (int y = 0; y < height; ++y)
    {
        for (int x = 0; x < width; ++x)
        {
            if (!valid(x, y)) {
                indices(x, y) = numEmpty;
                numEmpty++;
            }
        }
    }

    //build matrix
    std::vector<Eigen::Triplet<real>> entries;
    entries.reserve(5 * numEmpty);
    VectorX rhs = VectorX::Zero(numEmpty);
    const int neighborsX[] = { -1, 1, 0, 0 };
    const int neighborsY[] = { 0, 0, -1, 1 };
    for (int y = 0; y < height; ++y)
    {
        for (int x = 0; x < width; ++x)
        {
            if (!valid(x, y)) {
                int row = indices(x, y);
                int c = 0;
                for (int i = 0; i < 4; ++i) {
                    int ix = x + neighborsX[i];
                    int iy = y + neighborsY[i];
                    if (ix < 0 || ix >= width || iy < 0 || iy >= height) continue; //outside, Neumann boundary
                    if (valid(ix, iy)) {
                        //dirichlet boundary
                        rhs[indices(x, y)] += adjOutput(ix, iy);
                    }
                    else {
                        //inside
                        entries.emplace_back(indices(ix, iy), row, -1);
                    }
                    c++;
                }
                entries.emplace_back(row, row, c);
            }
        }
    }
    Eigen::SparseMatrix<real> M(numEmpty, numEmpty);
    M.setFromTriplets(entries.begin(), entries.end());

    //Solve it
    Eigen::ConjugateGradient<Eigen::SparseMatrix<real>> cg(M);
    VectorX r = cg.solve(rhs);

    //Map back
    grid_t result(width, height);
    for (int y = 0; y < height; ++y)
    {
        for (int x = 0; x < width; ++x)
        {
            if (!valid(x, y))
                adjInput(x, y) += r[indices(x, y)];
        }
    }
}

void ar::GridUtils2D::invertDisplacementDirectShepard(const grid_t& inputX, const grid_t& inputY, grid_t& outputX,
                                                      grid_t& outputY, real h)
{
    int width = (int)inputX.rows();
    int height = (int)inputX.cols();
    //find max displacement -> radius
    real maxDisp = sqrt((inputX * inputX + inputY * inputY).maxCoeff()) + 2 * h;
    int radius = static_cast<int>(ceil(maxDisp / h));
    //run the shepard interpolation
    //#pragma omp parallel for schedule(dynamic,8)
    for (int y = 0; y < height; ++y)
    {
        for (int x = 0; x < width; ++x)
        {
            vec2_t v(0, 0);
            real w = 0;
            vec2_t p1(x * h, y * h);
            for (int iy = std::max(0, y - radius); iy <= std::min(height - 1, y + radius); iy++)
            {
                for (int ix = std::max(0, x - radius); ix <= std::min(width - 1, x + radius); ix++)
                {
                    vec2_t u(inputX(ix, iy), inputY(ix, iy));
                    vec2_t p2 = vec2_t(ix * h, iy * h) + u;
                    real d = (p1 - p2).norm() + 0.000000001;
                    if (d <= maxDisp)
                    {
                        real wi = (1 / d - 1 / maxDisp);
                        wi *= wi;
                        v += wi * u;
                        w += wi;
                    }
                }
            }
            if (w > 0)
            {
                v /= w;
            }
            outputX(x, y) = v.x();
            outputY(x, y) = v.y();
        }
    }
}

void ar::GridUtils2D::invertDisplacementDirectShepardAdjoint(
    grid_t& adjInputX, grid_t& adjInputY,
    const grid_t& adjOutputX, const grid_t& adjOutputY,
    const grid_t& inputX, const grid_t& inputY, const grid_t& outputX, const grid_t& outputY,
    real h)
{
    int width = (int)inputX.rows();
    int height = (int)inputX.cols();
    //find max displacement -> radius
    real maxDisp = sqrt((inputX * inputX + inputY * inputY).maxCoeff()) + 2 * h;
    int radius = static_cast<int>(ceil(maxDisp / h));
    //compute the adjoint of the shepard interpolation
    for (int y = height - 1; y >= 0; --y)
    {
        for (int x = width - 1; x>=0; --x)
        {
            //compute terms that are needed from the forward pass
            real w = 0;
            vec2_t p1(x * h, y * h);
            for (int iy = std::max(0, y - radius); iy <= std::min(height - 1, y + radius); iy++)
            {
                for (int ix = std::max(0, x - radius); ix <= std::min(width - 1, x + radius); ix++)
                {
                    vec2_t u(inputX(ix, iy), inputY(ix, iy));
                    vec2_t p2 = vec2_t(ix * h, iy * h) + u;
                    real d = (p1 - p2).norm() + 0.000000001;
                    if (d <= maxDisp)
                    {
                        real wi = square(1 / d - 1 / maxDisp);
                        w += wi;
                    }
                }
            }
            assert(w > 0);

            vec2_t adjOutput(adjOutputX(x, y), adjOutputY(x, y));
            vec2_t adjV = adjOutput / w;
            real adjW = -(vec2_t(outputX(x, y), outputY(x, y)) / (w*w)).dot(adjV);

            //compute adjoint, add it to the gradient of each component
            for (int iy = std::min(height - 1, y + radius); iy >= std::max(0, y - radius); --iy)
            {
                for (int ix = std::min(width - 1, x + radius); ix >= std::max(0, x - radius); --ix)
                {
                    vec2_t u(inputX(ix, iy), inputY(ix, iy));
                    vec2_t p2 = vec2_t(ix * h, iy * h) + u;
                    real d = (p1 - p2).norm() + 0.000000001;
                    if (d <= maxDisp)
                    {
                        real wi = square(1 / d - 1 / maxDisp);
                        real adjWi = adjW;
                        vec2_t adjU = wi * adjV;
                        adjWi += u.dot(adjV);
                        real adjD = -(2 / square(d)) * (1 / d - 1 / maxDisp) * adjWi;

                        adjU += (p2 - p1) / d * adjD;
                        adjInputX(ix, iy) += adjU.x();
                        adjInputY(ix, iy) += adjU.y();
                    }
                }
            }
        }
    }
}

void ar::GridUtils2D::invertDisplacement(const grid_t& inputX, const grid_t& inputY, grid_t& outputX, grid_t& outputY,
                                         real h)
{
    invertDisplacementDirectShepard(inputX, inputY, outputX, outputY, h);
    //TODO: global optimization
}

ar::GridUtils2D::grid_t ar::GridUtils2D::advectGridSemiLagrange(const grid_t& input, const grid_t& dispX, const grid_t& dispY,
                                                    real step)
{
    int width = (int)input.rows();
    int height = (int)input.cols();
    grid_t output(width, height);
    //#pragma omp parallel for
    for (int y = 0; y < height; ++y)
    {
        for (int x = 0; x < width; ++x)
        {
            vec2_t v(dispX(x, y), dispY(x, y));
            output(x, y) = getClampedF(input, x + step * v.x(), y + step * v.y());
        }
    }
    return output;
}

void ar::GridUtils2D::advectGridSemiLagrangeAdjoint(grid_t& adjInput, grid_t& adjDispX, grid_t& adjDispY,
    real step, const grid_t& adjOutput, const grid_t& dispX, const grid_t& dispY, const grid_t& input)
{
    int width = (int)adjOutput.rows();
    int height = (int)adjOutput.cols();
    assert(adjInput.rows() == width); assert(adjInput.cols() == height);
    assert(adjDispX.rows() == width); assert(adjDispX.cols() == height);
    assert(adjDispY.rows() == width); assert(adjDispY.cols() == height);

    for (int ky = height-1; ky >= 0; --ky)
    {
        for (int kx = width-1; kx >= 0; --kx)
        {
            //variables from the forward pass
            vec2_t v(dispX(kx, ky), dispY(kx, ky));
            real x = kx + step * v.x();
            real y = ky + step * v.y();
            int ix = static_cast<int>(x);
            int iy = static_cast<int>(y);
            real fx = x - ix;
            real fy = y - iy;
            real v00 = getClamped(input, ix, iy);
            real v10 = getClamped(input, ix + 1, iy);
            real v01 = getClamped(input, ix, iy + 1);
            real v11 = getClamped(input, ix + 1, iy + 1);
            real v0 = v00 + fy * (v01 - v00);
            real v1 = v10 + fy * (v11 - v10);
            //adjoint of the input
            real adjB = adjOutput(kx, ky);
            real adjV0 = (1 - fx) * adjB;
            real adjV1 = fx * adjB;
            real adjFx = (v1 - v0) * adjB;
            real adjV10 = (1 - fy) * adjV1;
            real adjV11 = fy * adjV1;
            real adjFy = (v11 - v10) * adjV1;
            real adjV00 = (1 - fy) * adjV0;
            real adjV01 = fy * adjV0;
            adjFy += (v01 - v00) * adjV0;
            atClamped(adjInput, ix, iy) += adjV00;
            atClamped(adjInput, ix + 1, iy) += adjV10;
            atClamped(adjInput, ix, iy + 1) += adjV01;
            atClamped(adjInput, ix + 1, iy + 1) += adjV11;
            real adjIx = -adjFx; real adjX = adjFx;
            real adjIy = -adjFy; real adjY = adjFy;
            real adjUx = step * adjX;
            real adjUy = step * adjY;
            adjDispX(kx, ky) += adjUx;
            adjDispY(kx, ky) += adjUy;
        }
    }
}

const ar::real ar::GridUtils2D::directForwardKernelRadiusIn = 1.5;
const ar::real ar::GridUtils2D::directForwardKernelRadiusOut = 4.0;
const ar::real ar::GridUtils2D::directForwardOuterKernelWeight = 1e-5;
const ar::real ar::GridUtils2D::directForwardOuterSdfThreshold = 1.01;
const ar::real ar::GridUtils2D::directForwardOuterSdfWeight = 1e-10;

ar::GridUtils2D::grid_t ar::GridUtils2D::advectGridDirectForward(const grid_t& input, const grid_t& dispX,
    const grid_t& dispY, real step, grid_t* outputWeights)
{
    int width = (int)input.rows();
    int height = (int)input.cols();
    grid_t weights = grid_t::Constant(width, height, 0);
    grid_t output = grid_t::Zero(width, height);
    real kernelDenom = 1.0 / (2 * square(directForwardKernelRadiusIn / 3)); //98% of the Gaussian kernel is within kernelRadius
    //blend into output grid
    for (int y=0; y<height; ++y)
    {
        for (int x=0; x<width; ++x)
        {
            real value = input(x, y);
			real extraWeight = 1;
			if (value <= -directForwardOuterSdfThreshold || value >= directForwardOuterSdfThreshold)
				extraWeight = std::max(directForwardOuterSdfWeight, -std::abs(value)+ directForwardOuterSdfThreshold);
            vec2_t v(dispX(x, y), dispY(x, y));
            vec2_t p = vec2_t(x, y) - step * v;
            for (int ix = std::max(0, (int)std::floor(p.x() - directForwardKernelRadiusOut)); ix <= std::min(width-1, (int)std::ceil(p.x() + directForwardKernelRadiusOut)); ++ix)
            {
                for (int iy = std::max(0, (int)std::floor(p.y() - directForwardKernelRadiusOut)); iy <= std::min(height - 1, (int)std::ceil(p.y() + directForwardKernelRadiusOut)); ++iy)
                {
                    real d = (vec2_t(ix, iy) - p).squaredNorm();
                    if (d <= square(directForwardKernelRadiusIn))
                    {
                        real w = exp(-d * kernelDenom) * extraWeight;
                        output(ix, iy) += w * value;
                        weights(ix, iy) += w;
                    } else if (d <= square(directForwardKernelRadiusOut))
                    {
						real w = exp(-d * kernelDenom) * directForwardOuterKernelWeight * extraWeight;
						output(ix, iy) += w * value;
						weights(ix, iy) += w;
                    }
                }
            }
        }
    }
    //normalize
    //output /= weights;
	output = grid_t::NullaryExpr(width, height, [&output, &input, &weights](Eigen::Index row, Eigen::Index col)->real
    {
		const real weight = weights(row, col);
		//return weight == 0 ? input(row, col) : output(row, col) / weight;
		return weight == 0 ? (weights.rows()+weights.cols()) : output(row, col) / weight; //well outside if weight=0
	});
	//TODO: change this also in the adjoint versions

	//place output weights
	if (outputWeights)
		*outputWeights = weights;
    //done
    return output;
}

//TODO: implement changes to the forward problem (different blur kernels and weights) also in the adjoint

void ar::GridUtils2D::advectGridDirectForwardAdjoint(grid_t& adjInput, grid_t& adjDispX, grid_t& adjDispY,
    const grid_t& adjOutput, const grid_t& dispX, const grid_t& dispY, const grid_t& input, const grid_t& output,
    real step, const grid_t& weights)
{
    int width = (int)input.rows();
    int height = (int)input.cols();
	real kernelDenom = 1.0 / (2 * square(directForwardKernelRadiusIn / 3)); //98% of the Gaussian kernel is within kernelRadius

    /*
    output = grid_t::NullaryExpr(width, height, [&output, &input, &weights](Eigen::Index row, Eigen::Index col)->real
    {
		const real weight = weights(row, col);
		//return weight == 0 ? input(row, col) : output(row, col) / weight;
		return weight == 0 ? (weights.rows()+weights.cols()) : output(row, col) / weight; //well outside if weight=0
	});
     */
    //adjoint: normalize
    grid_t adjOut = grid_t::NullaryExpr(width, height, [&adjOutput, &weights](Eigen::Index row, Eigen::Index col)->real
    {
        const real weight = weights(row, col);
        return weight == 0 ? 0 : adjOutput(row, col) / weight; //well outside if weight=0
    });
    grid_t adjWeights = grid_t::NullaryExpr(width, height, [&adjOutput, &output, &weights](Eigen::Index row, Eigen::Index col)->real
    {
        const real weight = weights(row, col);
        return weight == 0 ? 0 : -(adjOutput(row, col) * output(row, col)) / square(weight); //well outside if weight=0
    });

    //adjoint: blend into output grid
    for (int y=height-1; y>=0; --y)
    {
        for (int x=width-1; x>=0; --x)
        {
            real value = input(x, y);
			real extraWeight = 1;
			if (value <= -directForwardOuterSdfThreshold || value >= directForwardOuterSdfThreshold)
				extraWeight = std::max(directForwardOuterSdfWeight, -std::abs(value) + directForwardOuterSdfThreshold);
            vec2_t v(dispX(x, y), dispY(x, y));
            vec2_t p = vec2_t(x, y) - step * v;
            real adjValue = 0;
            vec2_t adjP(0, 0);
            for (int ix = std::min(width - 1, (int)std::ceil(p.x() + directForwardKernelRadiusOut)); ix >= std::max(0, (int)std::floor(p.x() - directForwardKernelRadiusOut)); --ix)
            {
                for (int iy = std::min(height - 1, (int)std::ceil(p.y() + directForwardKernelRadiusOut)); iy >= std::max(0, (int)std::floor(p.y() - directForwardKernelRadiusOut)); --iy)
                {
                    real d = (vec2_t(ix, iy) - p).squaredNorm();
                    if (d <= square(directForwardKernelRadiusIn))
                    {
                        real w = exp(-d * kernelDenom) * extraWeight;
                        real adjW = adjWeights(ix, iy);
                        adjValue += w * adjOut(ix, iy);
                        adjW += value * adjOut(ix, iy);
                        real adjD = -exp(-d * kernelDenom)*kernelDenom*extraWeight*adjW;
                        adjP += 2 * (p - vec2_t(ix, iy)) * adjD;
                    }
                	else if (d <= square(directForwardKernelRadiusOut))
                    {
						real w = exp(-d * kernelDenom) * extraWeight * directForwardOuterKernelWeight;
						real adjW = adjWeights(ix, iy);
						adjValue += w * adjOut(ix, iy);
						adjW += value * adjOut(ix, iy);
						real adjD = -exp(-d * kernelDenom) * kernelDenom * extraWeight * directForwardOuterKernelWeight * adjW;
						adjP += 2 * (p - vec2_t(ix, iy)) * adjD;
                    }
                }
            }
            vec2_t adjV = -step * adjP;
            adjDispX(x, y) += adjV.x();
            adjDispY(x, y) += adjV.y();
            adjInput(x, y) += adjValue;
        }
    }
}

ar::GridUtils2D::grid_t ar::GridUtils2D::advectGridDirectForwardAdjOpMult(const grid_t & adjInput,
	const grid_t & dispX, const grid_t & dispY, real step, const grid_t & weights)
{
	int width = (int)adjInput.rows();
	int height = (int)adjInput.cols();
	grid_t output(width, height);

	real kernelDenom = 1.0 / (2 * square(directForwardKernelRadiusIn / 3)); //98% of the Gaussian kernel is within kernelRadius
															 //blend into output grid
	for (int y = 0; y<height; ++y)
	{
		for (int x = 0; x<width; ++x)
		{
			//TODO: Do I need the correction by directForwardOuterSdfThreshold and directForwardOuterSdfWeight here?
			real out = 0;
			vec2_t v(dispX(x, y), dispY(x, y));
			vec2_t p = vec2_t(x, y) - step * v;
			for (int ix = std::max(0, (int)std::floor(p.x() - directForwardKernelRadiusOut)); ix <= std::min(width - 1, (int)std::ceil(p.x() + directForwardKernelRadiusOut)); ++ix)
			{
				for (int iy = std::max(0, (int)std::floor(p.y() - directForwardKernelRadiusOut)); iy <= std::min(height - 1, (int)std::ceil(p.y() + directForwardKernelRadiusOut)); ++iy)
				{
					real d = (vec2_t(ix, iy) - p).squaredNorm();
					if (d <= square(directForwardKernelRadiusIn))
					{
						real w = exp(-d * kernelDenom);
						out -= adjInput(ix, iy) * w / weights(ix, iy);
					}
					else if (d <= square(directForwardKernelRadiusOut))
					{
						real w = exp(-d * kernelDenom) * directForwardOuterKernelWeight;
						out -= adjInput(ix, iy) * w / weights(ix, iy);
					}
				}
			}
			output(x, y) = -out;
		}
	}

	return output;
}

ar::GridUtils2D::grid_t ar::GridUtils2D::recoverSDFViscosity(const grid_t& input, real viscosity, int iterations)
{
    int width = input.rows();
    int height = input.cols();
    grid_t output(width, height);
    grid_t inputOrigin = input;

    //use ar::GradientDescent
    auto gradFun = [&width, &height, &viscosity, &inputOrigin](const Eigen::Matrix<real, Eigen::Dynamic, 1>& a)
    {
        //allocate result
        Eigen::Matrix<real, Eigen::Dynamic, 1> grad(width * height);
        //compute new values
        //#pragma omp parallel for
        for (int y = 0; y < height; ++y)
        {
            for (int x = 0; x < width; ++x)
            {
                grad_t g = getGradientLinear(a.data(), x, y, width, height);
                double l = getLaplacianLinear(a.data(), x, y, width, height);
                double update = ar::utils::sgn(getLinear(inputOrigin.data(), x, y, width)) * (1 - g.norm()) + viscosity
                    * l;
                atLinear(grad.data(), x, y, width) = update;
            }
        }
        //return array
        return grad;
    };

    VectorX inputLinear = linearize(input);
    GradientDescent<Eigen::Matrix<real, Eigen::Dynamic, 1>> gd(inputLinear, gradFun);
    for (int i = 0; i < iterations; ++i)
    {
        bool end = gd.step();
        CI_LOG_I("Iteration " << i << ", step size: " << gd.getLastStepSize());
        if (end) break;
    }

    return delinearize(gd.getCurrentSolution(), width, height);
}

ar::GridUtils2D::grid_t ar::GridUtils2D::recoverSDFUpwind(const grid_t& input, int iterations)
{
    int width = input.rows();
    int height = input.cols();
    grid_t output(width, height);
    grid_t inputOrigin = input;

    //use ar::GradientDescent
    auto gradFun = [&width, &height, &inputOrigin](const Eigen::Matrix<real, Eigen::Dynamic, 1>& a)
    {
        //allocate result
        Eigen::Matrix<real, Eigen::Dynamic, 1> grad(width * height);
        //compute new values
        //#pragma omp parallel for
        for (int y = 0; y < height; ++y)
        {
            for (int x = 0; x < width; ++x)
            {
                real gradPos = getGradientPosLinear(a.data(), x, y, width, height);
                real gradNeg = getGradientNegLinear(a.data(), x, y, width, height);
                int sgn = ar::utils::sgn(getLinear(inputOrigin.data(), x, y, width));
                double update = std::max(0, sgn) * gradPos + std::min(0, sgn) * gradNeg - sgn;
                atLinear(grad.data(), x, y, width) = update;
            }
        }
        //return array
        return grad;
    };

    Eigen::Matrix<real, Eigen::Dynamic, 1> inputLinear = linearize(input);
    GradientDescent<Eigen::Matrix<real, Eigen::Dynamic, 1>> gd(inputLinear, gradFun);
    for (int i = 0; i < iterations; ++i)
    {
        bool end = gd.step();
        CI_LOG_I("Iteration " << i << ", step size: " << gd.getLastStepSize());
        if (end) break;
    }

    return delinearize(gd.getCurrentSolution(), width, height);
}

ar::GridUtils2D::grid_t ar::GridUtils2D::recoverSDFSussmann(
	const grid_t& input, real epsilon, int iterations, RecoverSDFSussmannAdjointStorage* adjointStorage)
{
	int width = input.rows();
	int height = input.cols();
	grid_t inputOrigin = input;

	bgrid_t canChange = bgrid_t::Constant(width, height, true);
	for (int x=1; x<width; ++x)
		for (int y=1; y<height; ++y)
		{
			int c = utils::insideEq(input(x-1, y-1))
				| (utils::insideEq(input(x-1, y)) << 1)
				| (utils::insideEq(input(x, y-1)) << 2)
				| (utils::insideEq(input(x, y)) << 3);
			if (!(c==0b0000 || c==0b111))
			{
				canChange(x - 1, y - 1) = false;
				canChange(x - 1, y) = false;
				canChange(x, y - 1) = false;
				canChange(x, y) = false;
			}
		}

    if (adjointStorage) {
        adjointStorage->epsilon = epsilon;
        adjointStorage->inputs.push_back(inputOrigin);
        adjointStorage->canChange = canChange;
    }

#if 0
    real finalCost = 0;
    const auto gradient = [width, height, &inputOrigin, &canChange, epsilon, &finalCost](const VectorX& x) -> VectorX
    {
        grid_t grad = recoverSDFSussmannGradient(delinearize(x, width, height), inputOrigin, canChange, finalCost, epsilon);
        return linearize(grad);
    };
    //start value
    VectorX value = linearize(input);
    //run optimization
    GradientDescent<VectorX> gd(value, gradient);
    gd.setEpsilon(1e-15);
    gd.setLinearStepsize(0.001);
    gd.setMaxStepsize(0.5);
    int oi;
    for (oi = 0; oi < iterations; ++oi) {
        bool done = gd.step();
        CI_LOG_I("Iteration " << oi << "/" << iterations << ", cost = " << finalCost << ", stepsize = " << gd.getLastStepSize());
        if (done) break;
    }
    value = gd.getCurrentSolution();
    return delinearize(value, width, height);
#elif 1
    //fixed step size
    real stepsize = 0.5;
    real cost = 0;
    grid_t value = input;
    for (int i=0; i<iterations; ++i)
    {
        grid_t grad = recoverSDFSussmannGradient(value, inputOrigin, canChange, cost, epsilon);
        value -= stepsize * grad;
        if (adjointStorage) adjointStorage->inputs.push_back(value);
    }
    if (adjointStorage) {
        adjointStorage->iterations = iterations;
        adjointStorage->stepsize = stepsize;
    }
    return value;
#else
	LBFGSpp::LBFGSParam<real> params;
	params.epsilon = 1e-10;
	params.max_iterations = iterations;
	LBFGSpp::LBFGSSolver<real> lbfgs(params);

	LBFGSpp::LBFGSSolver<real>::ObjectiveFunction_t gradFun = [width, height, &inputOrigin, &canChange, epsilon](const VectorX& va, VectorX& gradient) -> real
	{
		real cost = 0;
		grid_t grad = recoverSDFSussmannGradient(delinearize(va, width, height), inputOrigin, canChange, cost, epsilon);
		gradient = linearize(grad);
		return cost;
	};
	LBFGSpp::LBFGSSolver<real>::CallbackFunction_t callback([iterations](const VectorX& x, const real& v, int k) -> bool {
		CI_LOG_I("Iteration " << k << "/" << iterations << ", cost = " << v);
		return true;
	});

	real finalCost = 0;
	VectorX value = linearize(input);
	int finalIterations = lbfgs.minimize(gradFun, value, finalCost, callback);
    return delinearize(value, width, height);
#endif
}

void ar::GridUtils2D::recoverSDFSussmannAdjoint(const grid_t& input, const grid_t& output, const grid_t& adjOutput,
	grid_t& adjInput, const RecoverSDFSussmannAdjointStorage& adjointStorage)
{
	//Adjoint of recoverSDFSussmann

    const grid_t& input0 = adjointStorage.inputs[0];
    grid_t adjValue = adjOutput;
    for (int i=adjointStorage.iterations-1; i>=0; --i)
    {
        grid_t adjGrad = -adjointStorage.stepsize * adjValue;
        recoverSDFSussmannGradientAdjoint(adjointStorage.inputs[i], input0, adjointStorage.canChange, adjointStorage.epsilon, adjGrad, adjValue);
    }
    adjInput += adjValue;
}

ar::GridUtils2D::grid_t ar::GridUtils2D::recoverSDFSussmannGradient(
	const grid_t& input, const grid_t& input0, const bgrid_t& canChange,
	real& cost, real epsilon)
{
	int width = input.rows();
	int height = input.cols();

	VectorX inputLinear = linearize(input);
	const real* a = inputLinear.data();

	grid_t gradient(width, height);

	cost = 0;
	for (int y = 0; y < height; ++y)
	{
		for (int x = 0; x < width; ++x)
		{
			if (!canChange(x, y))
			{
				gradient(x, y) = 0;
				continue;
			}

			real phi0 = input0(x, y);
			real s = phi0 / sqrt(phi0*phi0 + epsilon);
			real s01 = utils::sgn(s);

			real phi = getLinear(a, x, y, width);
			real ta = x > 0
				? (phi - getLinear(a, x - 1, y, width))
				: -s01;// (getLinear(a, x + 1, y, width) - phi);
			real tb = x < width - 1
				? (getLinear(a, x + 1, y, width) - phi)
				: s01;// (phi - getLinear(a, x - 1, y, width));
			real tc = y > 0
				? (phi - getLinear(a, x, y - 1, width))
				: -s01;// (getLinear(a, x, y - 1, width) - phi);
			real td = y < height - 1
				? (getLinear(a, x, y + 1, width) - phi)
				: s01;// (phi - getLinear(a, x, y - 1, width));
			real aPos = std::max(real(0), ta), aNeg = std::min(real(0), ta);
			real bPos = std::max(real(0), tb), bNeg = std::min(real(0), tb);
			real cPos = std::max(real(0), tc), cNeg = std::min(real(0), tc);
			real dPos = std::max(real(0), td), dNeg = std::min(real(0), td);

			real g = 0;
			if (phi0 > 0)
				g = sqrt(std::max(aPos*aPos, bNeg*bNeg) + std::max(cPos*cPos, dNeg*dNeg)) - 1;
			else if (phi0 < 0)
				g = sqrt(std::max(aNeg*aNeg, bPos*bPos) + std::max(cNeg*cNeg, dPos*dPos)) - 1;

			cost += 0.5 * square(g);

			gradient(x, y) = s * g;
		}
	}

	return gradient;
}

void ar::GridUtils2D::recoverSDFSussmannGradientAdjoint(const grid_t& input, const grid_t& input0,
    const bgrid_t& canChange, real epsilon, const grid_t& adjOutput, grid_t& adjInput)
{
    int width = input.rows();
    int height = input.cols();
    for (int y = 0; y < height; ++y)
    {
        for (int x = 0; x < width; ++x)
        {
            if (!canChange(x, y))
                continue;

            real phi0 = input0(x, y);
            real s = phi0 / sqrt(phi0*phi0 + epsilon);
            real s01 = utils::sgn(s);

            real phi = input(x, y);
            real ta = x > 0
                ? (phi - input(x - 1, y))
                : -s01;// (getLinear(a, x + 1, y, width) - phi);
            real tb = x < width - 1
                ? (input(x + 1, y) - phi)
                : s01;// (phi - getLinear(a, x - 1, y, width));
            real tc = y > 0
                ? (phi - input(x, y - 1))
                : -s01;// (getLinear(a, x, y - 1, width) - phi);
            real td = y < height - 1
                ? (input(x, y + 1) - phi)
                : s01;// (phi - getLinear(a, x, y - 1, width));

            if (phi0 > 0)
            {
                real ap = std::max(ta, real(0)), bm = std::min(tb, real(0)), cp = std::max(tc, real(0)), dm = std::min(td, real(0));
                real ab = std::max(ap*ap, bm*bm), cd = std::max(cp*cp, dm*dm);
                real adjAB = (s / (2 * sqrt(ab + cd))) * adjOutput(x, y), adjCD = adjAB;
                real adjAp = ap * ap > bm*bm ? 0.5*ap*adjAB : 0;
                if (x > 0 && ta > 0) { adjInput(x, y) += adjAp; adjInput(x - 1, y) -= adjAp; }
                real adjBm = ap * ap < bm*bm ? 0.5*bm*adjAB : 0;
                if (x < width - 1 && tb < 0) { adjInput(x, y) -= adjBm; adjInput(x + 1, y) += adjBm; }
                real adjCp = cp * cp > dm*dm ? 0.5*cp*adjCD : 0;
                if (y > 0 && tc > 0) { adjInput(x, y) += adjCp; adjInput(x, y - 1) -= adjCp; }
                real adjDm = cp * cp < dm*dm ? 0.5*dm*adjCD : 0;
                if (y < height - 1 && td < 0) { adjInput(x, y) -= adjDm; adjInput(x, y + 1) += adjDm; }
            } else
            {
                real am = std::min(ta, real(0)), bp = std::max(tb, real(0)), cm = std::min(tc, real(0)), dp = std::max(td, real(0));
                real ab = std::max(am*am, bp*bp), cd = std::max(cm*cm, dp*dp);
                real adjAB = (s / (2 * sqrt(ab + cd))) * adjOutput(x, y), adjCD = adjAB;
                real adjAm = am * am > bp*bp ? 0.5*am*adjAB : 0;
                if (x > 0 && ta < 0) { adjInput(x, y) += adjAm; adjInput(x - 1, y) -= adjAm; }
                real adjBp = am * am < bp*bp ? 0.5*bp*adjAB : 0;
                if (x < width - 1 && tb > 0) { adjInput(x, y) -= adjBp; adjInput(x + 1, y) += adjBp; }
                real adjCm = cm * cm > dp*dp ? 0.5*cm*adjCD : 0;
                if (y > 0 && tc < 0) { adjInput(x, y) += adjCm; adjInput(x, y - 1) -= adjCm; }
                real adjDp = cm * cm < dp*dp ? 0.5*dp*adjCD : 0;
                if (y < height - 1 && td > 0) { adjInput(x, y) -= adjDp; adjInput(x, y + 1) += adjDp; }
            }
        }
    }
}

ar::GridUtils2D::grid_t ar::GridUtils2D::recoverSDFFastMarching(const grid_t& input)
{
    int width = input.rows();
    int height = input.cols();
    grid_t output = input;//(width, height);
    bgrid_t accepted = bgrid_t::Constant(width, height, false);

    //TODO: inside

    //outside loop
    typedef std::tuple<real, int, int, real> et;
    auto comp = [](const et& a, const et& b) {return std::get<0>(a) > std::get<0>(b); };
    std::priority_queue<et, std::vector<et>, decltype(comp)> close(comp);
    //initial poits
    for (int x=0; x<width; ++x) for (int y=0; y<height; ++y)
    {
        if (!utils::outside(input(x, y))) continue; //we are inside
        //find boundary case
        int c = (x > 0 && utils::insideEq(input(x - 1, y))) ? 1 : 0
            | ((x < width - 1 && utils::insideEq(input(x + 1, y))) ? 2 : 0)
            | ((y > 0 && utils::insideEq(input(x, y - 1))) ? 4 : 0)
            | ((y > height - 1 && utils::insideEq(input(x, y + 1))) ? 8 : 0);
        if (c == 0) continue; //completely outside, no boundary

        //process boundary case
        real vOld = input(x, y);
        real vNew = 0;
        switch (c) // +y -y +x -x
        {
        case 0b0001:
        case 0b0010:
        case 0b0100:
        case 0b1000:
        { //case a)
            real v = c == 0b0001 ? input(x - 1, y)
                : c == 0b0010 ? input(x + 1, y)
                : c == 0b0100 ? input(x, y - 1)
                : /*c == 0b1000 ?*/ input(x, y + 1);
            real s = v / (v - vOld); assert(s > 0);
            vNew = s;
        }break;
        case 0b0101:
        case 0b1010:
        case 0b0110:
        case 0b1001:
        { //case b)
            real v1 = c == 0b0101 ? input(x - 1, y)
                : c == 0b1010 ? input(x + 1, y)
                : c == 0b0110 ? input(x + 1, y)
                : /*c == 0b1001 ?*/ input(x - 1, y);
            real v2 = c == 0b0101 ? input(x, y - 1)
                : c == 0b1010 ? input(x, y + 1)
                : c == 0b0110 ? input(x, y - 1)
                : /*c == 0b1001 ?*/ input(x, y + 1);
            real s = v1 / (v1 - vOld); assert(s > 0);
            real t = v2 / (v2 - vOld); assert(t > 0);
            vNew = s * t / sqrt(s*s + t * t);
        } break;
        case 0b1110:
        case 0b1101:
        case 0b1011:
        case 0b0111:
        { //case c)
            real vs1 = c == 0b1110 ? input(x, y - 1)
                : c == 0b1101 ? input(x, y + 1)
                : c == 0b1011 ? input(x + 1, y)
                : /*c == 0b0111 ?*/ input(x - 1, y);
            real vs2 = c == 0b1110 ? input(x, y + 1)
                : c == 0b1101 ? input(x, y - 1)
                : c == 0b1011 ? input(x - 1, y)
                : /*c == 0b0111 ?*/ input(x + 1, y);
            real vt = c == 0b1110 ? input(x - 1, y)
                : c == 0b1101 ? input(x + 1, y)
                : c == 0b1011 ? input(x, y + 1)
                : /*c == 0b0111 ?*/ input(x, y - 1);
            real s1 = vs1 / (vs1 - vOld); assert(s1 > 0);
            real s2 = vs2 / (vs2 - vOld); assert(s2 > 0);
            real s = std::min(s1, s2);
            real t = vt / (vt - vOld); assert(t > 0);
            vNew = s * t / sqrt(s*s + t * t);
        } break;
        case 0b0011:
        case 0b1100:
        { //case d)
            real vs1 = c == 0b0011 ? input(x - 1, y)
                : /*c == 0b1100 ?*/ input(x, y - 1);
            real vs2 = c == 0b0011 ? input(x + 1, y)
                : /*c == 0b1100 ?*/ input(x, y + 1);
            real s1 = vs1 / (vs1 - vOld); assert(s1 > 0);
            real s2 = vs2 / (vs2 - vOld); assert(s2 > 0);
            vNew = std::min(s1, s2);
        } break;
        case 0b1111:
        { //case e);
            real vs1 = input(x - 1, y);
            real vs2 = input(x + 1, y);
            real vt1 = input(x, y - 1);
            real vt2 = input(x, y + 1);
            real s1 = vs1 / (vs1 - vOld); assert(s1 > 0);
            real s2 = vs2 / (vs2 - vOld); assert(s2 > 0);
            real t1 = vt1 / (vt1 - vOld); assert(t1 > 0);
            real t2 = vt2 / (vt2 - vOld); assert(t2 > 0);
            real s = std::min(s1, s2);
            real t = std::min(t1, t2);
            vNew = s * t / sqrt(s*s + t * t);
        } break;
        }
        assert(vNew > 0);

        //write output and mark as done
        output(x, y) = vNew;

        //add to the queue
        close.emplace(real(0), x, y, vNew);
    }
    //sweep
    while(!close.empty())
    {
        et e = close.top(); close.pop();
        real v = std::get<3>(e);
        int x = std::get<1>(e);
        int y = std::get<2>(e);

        if (accepted(x, y)) continue; //already processed

        //mark current as processed
        output(x, y) = v;
        accepted(x, y) = true;

        //add neighbors to the list
        int neighbors[4][2] = { {-1,0}, {+1,0}, {0,-1}, {0,+1} };
        const static real BIG = 1e10;
        for (int i=0; i<4; ++i)
        {
            int ix = x + neighbors[i][0];
            int iy = y + neighbors[i][1];
            if (ix < 0 || iy < 0 || ix >= width || iy >= height) continue;
            if (!utils::outside(input(ix, iy))) continue;
            if (accepted(ix, iy)) continue;
            real a = (ix + 1) >= width ? BIG
                : !accepted(ix + 1, iy) ? BIG
                : output(ix + 1, iy);
            real b = (ix - 1) < 0 ? BIG
                : !accepted(ix - 1, iy) ? BIG
                : output(ix - 1, iy);
            real c = (iy + 1) >= height ? BIG
                : !accepted(ix, iy + 1) ? BIG
                : output(ix, iy + 1);
            real d = (iy - 1) < 0 ? BIG
                : !accepted(ix, iy - 1) ? BIG
                : output(ix, iy - 1);
            real ab = std::min(a, b);
            real cd = std::min(c, d);
            real vNew = NAN;
            if (ab < BIG && cd < BIG)
                vNew = 0.5 * sqrt(-square(ab) + 2 * ab*cd - square(cd) + 2) + (ab + cd) / 2;
            else if (ab < BIG)
                vNew = ab + 1;
            else if (cd < BIG)
                vNew = cd + 1;
            close.emplace(vNew, ix, iy, vNew);
        }
    }

    return output;
}


#ifndef __ROBOT_SIMULATOR__KIN__
#define __ROBOT_SIMULATOR__KIN__
#include <ros/ros.h>
#include <robot_state_publisher/robot_state_publisher.h>
#include <urdf/model.h>
#include <sensor_msgs/JointState.h>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/jntarrayvel.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <boost/thread.hpp>
#include <Eigen/Dense>

/**
  Class that maintains a kinematic simulation of the robot loaded in the robot_description parameter
  in the ROS parameter server. It publishes the TF transforms through the robot state publisher.
**/
class RobotSimulator
{
public:
  RobotSimulator(double frequency);

  /**
    Initializes the kinematic chain connecting base_link to end_effector_link.
    The end_effector_link name will be used to index this chain.

    @param base_link The kinematic chain base link.
    @param end_effector_link The kinematic chain end-effector.
    @param desired_pose: Initialization pose for the end-effector
    @return True for a successful initialization; False in case of failure.
  **/
  bool initKinematicChain(const std::string &base_link, const std::string &end_effector_link, const std::vector<double> &desired_pose);

  /**
    Set the desired chain to a pose.
  **/
  bool setPose(const std::string &end_effector_link, const std::vector<double> &desired_pose);

  /**
    Get an initialized kinematic chain.

    @param end_effector_link The name of the chain's end-effector.
    @param chain The variable where to store the chain.
    @return False for failure in getting the chain.
  **/
  bool getKinematicChain(const std::string &end_effector_link, KDL::Chain &chain);

  /**
    Gets the pose of the end-effector.
  **/
  bool getPose(const std::string &end_effector_link, KDL::Frame &pose);

  /**
    Gets the current kinematic chain joint state
  **/
  bool getJointState(const std::string &end_effector_link, KDL::JntArray &q);

  /**
    Set the current joint velocities for the joint chain
  **/
  bool setJointVelocities(const std::string &end_effector_link, const Eigen::VectorXd &joint_velocities);
private:
  boost::shared_ptr<robot_state_publisher::RobotStatePublisher> robot_publisher_;
  KDL::Tree tree_;

  std::vector<std::string> end_effector_link_name_;
  std::vector<KDL::Chain> chain_;
  std::vector<KDL::JntArrayVel> joint_state_;
  std::vector<boost::shared_ptr<KDL::ChainFkSolverPos_recursive> > fk_solver_;
  std::vector<boost::shared_ptr<KDL::ChainIkSolverPos_LMA> > ik_solver_;
  std::map<std::string, double> joint_positions_;
  std::vector<Eigen::VectorXd> current_joint_velocities_;
  boost::mutex velocities_mutex_;
  boost::thread sim_thread_;
  ros::Publisher joint_state_pub_;
  sensor_msgs::JointState joint_msg_;
  double frequency_;

  /**
    Gets a KDL frame from a vector with a 7 dimensional pose description.

    @param pose_in Vector with a 3 dimensional position and a 4 dimensional quaternion representing orientation.
    @return The KDL frame that represents the same pose.
  **/
  KDL::Frame getKDLPose(const std::vector<double> &pose_in);

  /**
    Simulation thread.
  **/
  void simulation();

  /**
    Apply the given joint velocities to the kinematic chain indexed by the end-effector link.

    @param joint_velocities The joint velocities vector.
    @param end_effector_link The kinematic chain end-effector.
    @param dt Time step.
    @return False in case of failure.
  **/
  bool applyJointVelocities(const Eigen::VectorXd &joint_velocities, const std::string &end_effector_link, double dt);
};

#endif


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef ANISOTROPIC_HPP
#define ANISOTROPIC_HPP

#include <iosfwd>

#include "Config.hpp"

#include <Eigen/Core>

#include "utils/MathUtils.hpp"

/*! \file Anisotropic.hpp
 *  \class Anisotropic
 *  \brief describes a medium with anisotropy, i.e. liquid crystal
 *  \author Roberto Di Remigio
 *  \date 2014
 */

class Anisotropic __final
{
private:
    /// Diagonal of the permittivity tensor in the lab-fixed frame
    Eigen::Vector3d epsilonLab_;
    /// Euler angles (in degrees) relating molecule-fixed and lab-fixed frames
    Eigen::Vector3d eulerAngles_;
    /// Permittivity tensor in molecule-fixed frame
    Eigen::Matrix3d epsilon_;
    /// Inverse of the permittivity tensor in molecule-fixed frame
    Eigen::Matrix3d epsilonInv_;
    /// molecule-fixed to lab-fixed frames rotation matrix
    Eigen::Matrix3d R_;
    /// Determinant of the permittivity tensor
    double detEps_;
    /*! Initializes some internals: molecule-fixed to lab-fixed frame rotation matrix,
     * permittivity tensor in molecule-fixed frame and its inverse
     */
    void build() {
        // 1. construct rotation matrix from Euler angles
	    eulerRotation(R_, eulerAngles_);
	    // 2. Apply the rotation matrix: epsilon_ = R_^t * epsilonLab_ * R_
	    epsilon_ = R_.transpose() * epsilonLab_.asDiagonal() * R_;
	    // 3. Obtain epsilonInv_ = R_ * epsilonLab_^-1 * R_^t
	    Eigen::Vector3d scratch;
	    scratch << (1.0/epsilonLab_(0)), (1.0/epsilonLab_(1)), (1.0/epsilonLab_(2));
	    epsilonInv_ = R_ * scratch.asDiagonal() * R_.transpose();
	    // 4. As a __final step, calculate the determinant
	    detEps_ = epsilonLab_(0) * epsilonLab_(1) * epsilonLab_(2);
    }
public:
    Anisotropic() :
	    epsilonLab_(Eigen::Vector3d::Ones()), eulerAngles_(Eigen::Vector3d::Zero()) { this->build(); }
    /*!
     * \param[in] eigen_eps eigenvalues of the permittivity tensors
     * \param[in] euler_ang Euler angles in degrees
     */
    Anisotropic(const Eigen::Vector3d & eigen_eps, const Eigen::Vector3d & euler_ang) :
	    epsilonLab_(eigen_eps), eulerAngles_(euler_ang) { this->build(); }
    const Eigen::Matrix3d & epsilon() const { return epsilon_; }
    const Eigen::Matrix3d & epsilonInv() const { return epsilonInv_; }
    double detEps() const { return detEps_; }
    friend std::ostream & operator<<(std::ostream & os, Anisotropic & arg) {
        os << "Permittivity tensor diagonal (lab frame)   = " << arg.epsilonLab_.transpose() << std::endl;
        os << "Euler angles (molecule-to-lab frame)       = " << arg.eulerAngles_.transpose() << std::endl;
        os << "Permittivity tensor (molecule-fixed frame) =\n" << arg.epsilon_;
        return os;
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW /* See http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html */
};

#endif // ANISOTROPIC_HPP


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <ctime>

#include <kv/qr.hpp>

namespace ub = boost::numeric::ublas;

int main()
{
	int i, j;
	ub::matrix<double> a, q, r;

	a.resize(2, 2);

	a(0, 0) = 1.;
	a(0, 1) = 2.;
	a(1, 0) = 3.;
	a(1, 1) = 4.;

	kv::qr(a, q, r);

	std::cout << q << "\n";
	std::cout << r << "\n";
	std::cout << a - prod(q, r) << "\n";
	std::cout << prod(q, trans(q)) << "\n";

	for (i=0; i<10; i++) {
		kv::qr(a, q, r);
		a = prod(r, q);
		std::cout << a << "\n";
	}

	a.resize(5, 5);
	srand(time(NULL));
	for (i=0; i<5; i++) {
		for (j=0; j<5; j++) {
			a(i, j) = (double)rand();
		}
	}

	kv::qr(a, q, r);

	// std::cout << q << "\n";
	// std::cout << r << "\n";
	std::cout << a - prod(q, r) << "\n";
	std::cout << prod(q, trans(q)) << "\n";
}


/*
 * ForceDihedral.cpp
 *
 *  Created on: 27 feb 2018
 *      Author: lorenzo
 */

#include "ForceDihedral.h"

#include <Eigen/Dense>

namespace ashell {

ForceDihedral::ForceDihedral(std::string name) :
				ForceComputer(name),
				_kb(100.),
				_theta0(0.) {

}

ForceDihedral::~ForceDihedral() {

}

void ForceDihedral::set_kb(double n_kb) {
	_kb = n_kb;
}

void ForceDihedral::set_theta0(double n_theta0) {
	_theta0 = n_theta0;
}

void ForceDihedral::_compute_forces(ullint step) {
	const vector_vec3 &poss = _particles->positions();

	for(auto &dihedral : _sys_props->dihedrals()) {
		uint i = dihedral->members[0];
		uint j = dihedral->members[1];
		uint k = dihedral->members[2];
		uint l = dihedral->members[3];

		vec3 i_pos = poss[i];
		vec3 j_pos = poss[j];
		vec3 k_pos = poss[k];
		vec3 l_pos = poss[l];

		// see Sofia Biagi's thesis, pag. 114
		vec3 r_ij = _sys_props->box()->minimum_image(j_pos, i_pos);
		vec3 r_kj = _sys_props->box()->minimum_image(j_pos, k_pos);
		vec3 r_kl = _sys_props->box()->minimum_image(l_pos, k_pos);

		double r_kj_sqr = r_kj.dot(r_kj);
		double r_kj_mod = sqrt(r_kj_sqr);

		vec3 m = r_ij.cross(r_kj);
		vec3 n = r_kj.cross(r_kl);

		double m_sqr = m.dot(m);
		double n_sqr = n.dot(n);

		double costheta = m.dot(n) / (sqrt(m_sqr) * sqrt(n_sqr));
		if(costheta > 1.) costheta = 1.;
		if(costheta < -1.) costheta = -1.;

		double theta = acos(costheta);
		if(r_ij.dot(n) < 0.) theta = -theta;
		// we use the so-called polymer convention, for which theta(polymer) = theta(IUPAC) +/- pi
		theta -= M_PI;

		double dVdtheta = _kb * sin(theta - _theta0);
		vec3 F_i = (-dVdtheta * r_kj_mod / m_sqr) * m;
		vec3 F_l = (dVdtheta * r_kj_mod / n_sqr) * n;

		// We compute Fj and Fk from Fi and Fl by knowing that  F_i + Fj + Fk + Fl == 0
		// and that the dihedral potential energy function is rotationally invariant
		vec3 S = (r_ij.dot(r_kj) / r_kj_sqr) * F_i - (r_kl.dot(r_kj) / r_kj_sqr) * F_l;

		_forces[i] += F_i;
		_forces[j] += -F_i + S;
		_forces[k] += -F_l - S;
		_forces[l] += F_l;

		double energy = _kb * (1. - cos(theta - _theta0));

//		BOOST_LOG_TRIVIAL(info) << costheta << " " << (theta*180/M_PI) << " " << energy;

		_energy += energy;
		_energies[i] += energy / 4.;
		_energies[j] += energy / 4.;
		_energies[k] += energy / 4.;
		_energies[l] += energy / 4.;
	}
}

} /* namespace ashell */


//!
//! Contains the implementation of functions for computing the likelihood.
//!
//! \file likelihood/likelihood.hpp
//! \author Darren Shen
//! \date May 2014
//! \license General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <Eigen/Dense>

#include "datatype/datatypes.hpp"

namespace obsidian
{
  namespace lh
  {
    //! Calculate the Gaussian log likelihood.
    //!
    //! \param real Vector containing the real sensor data.
    //! \param candidate Vector containing the simulated sensor data.
    //! \param sensorSd The standard deviation of sensor noise.
    //!
    double gaussian(const Eigen::VectorXd &real, const Eigen::VectorXd &candidate, double sensorSd);

    //!
    //! Calculate the normal inverse Gamma marginal log likelihood
    //!
    //! \param real Vector containing the real sensor data
    //! \param candidate Vector containing the simulated sensor data
    //! \param A, B Alpha and beta parameters
    //!
    double normalInverseGamma(const Eigen::VectorXd &real, const Eigen::VectorXd &candidate, double A, double B);

    template<ForwardModel f>
    double likelihood(const typename Types<f>::Results& synthetic, const typename Types<f>::Results& real,
                      const typename Types<f>::Spec& spec);

    Eigen::VectorXd mtLikelihoodVector(const Eigen::MatrixX4cd& impedences);

    std::vector<double> likelihoodAll(const GlobalResults& synthetic, const GlobalResults& real, const GlobalSpec& spec,
                                      const std::set<ForwardModel>& enabled);
  }
}


/***************************************************************************
* Copyright (c) Johan Mabille, Sylvain Corlay and Wolf Vollprecht          *
* Copyright (c) QuantStack                                                 *
*                                                                          *
* Distributed under the terms of the BSD 3-Clause License.                 *
*                                                                          *
* The full license is in the file LICENSE, distributed with this software. *
****************************************************************************/

#include <benchmark/benchmark.h>

#ifdef HAS_XTENSOR
#include "xtensor/xnoalias.hpp"
#include "xtensor/xio.hpp"
#include "xtensor/xrandom.hpp"
#include "xtensor/xtensor.hpp"
#include "xtensor/xarray.hpp"
#endif

#ifdef HAS_EIGEN
#include <Eigen/Dense>
#include <Eigen/Core>
#endif

#ifdef HAS_BLITZ
#include <blitz/array.h>
#endif

#ifdef HAS_ARMADILLO
#include <armadillo>
#endif

#ifdef HAS_PYTHONIC
#include <pythonic/core.hpp>
#include <pythonic/python/core.hpp>
#include <pythonic/types/ndarray.hpp>
#include <pythonic/numpy/random/rand.hpp>
#endif

#define RANGE 3, 1000
#define MULTIPLIER 8

#ifdef HAS_XTENSOR
void Add2D_XTensor(benchmark::State& state)
{
    using namespace xt;

    xtensor<double, 2> a = random::rand<double>({state.range(0), state.range(0)});
    xtensor<double, 2> b = random::rand<double>({state.range(0), state.range(0)});

    for (auto _ : state)
    {
        xtensor<double, 2> res(a + b);
        benchmark::DoNotOptimize(res.data());
    }
}
BENCHMARK(Add2D_XTensor)->RangeMultiplier(MULTIPLIER)->Range(RANGE);
#endif

#ifdef HAS_EIGEN
void Add2D_Eigen(benchmark::State& state)
{
    using namespace Eigen;
    MatrixXd a = MatrixXd::Random(state.range(0), state.range(0));
    MatrixXd b = MatrixXd::Random(state.range(0), state.range(0));
    for (auto _ : state)
    {
        MatrixXd res(state.range(0), state.range(0));
        res.noalias() = a + b;
        benchmark::DoNotOptimize(res.data());
    }
}
BENCHMARK(Add2D_Eigen)->RangeMultiplier(MULTIPLIER)->Range(RANGE);
#endif

#ifdef HAS_BLITZ
void Add2D_Blitz(benchmark::State& state)
{
    using namespace blitz;
    Array<double, 2> a(state.range(0), state.range(0));
    Array<double, 2> b(state.range(0), state.range(0));
    for (auto _ : state)
    {
        Array<double, 2> res(a + b);
        benchmark::DoNotOptimize(res.data());
    }
}
BENCHMARK(Add2D_Blitz)->RangeMultiplier(MULTIPLIER)->Range(RANGE);
#endif

#ifdef HAS_ARMADILLO
void Add2D_Arma(benchmark::State& state)
{
    using namespace arma;
    mat a = randu<mat>(state.range(0), state.range(0));
    mat b = randu<mat>(state.range(0), state.range(0));
    for (auto _ : state)
    {
        mat res = a + b;
        benchmark::DoNotOptimize(res.memptr());
    }
}
BENCHMARK(Add2D_Arma)->RangeMultiplier(MULTIPLIER)->Range(RANGE);
#endif

#ifdef HAS_PYTHONIC
void Add2D_Pythonic(benchmark::State& state)
{
    auto x = pythonic::numpy::random::rand(state.range(0), state.range(0));
    auto y = pythonic::numpy::random::rand(state.range(0), state.range(0));

    for (auto _ : state)
    {
        pythonic::types::ndarray<double, 2> z = x + y;
        benchmark::DoNotOptimize(z.fbegin());
    }
}
BENCHMARK(Add2D_Pythonic)->RangeMultiplier(MULTIPLIER)->Range(RANGE);
#endif

#undef RANGE
#undef MULTIPLIER


#pragma once

#include <polyfem/Problem.hpp>
#include <polyfem/ExpressionValue.hpp>

#include <vector>
#include <Eigen/Dense>

namespace polyfem
{
	class GenericTensorProblem: public Problem
	{
	public:
		GenericTensorProblem(const std::string &name);

		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return fabs(rhs_.maxCoeff())<1e-10 && fabs(rhs_.minCoeff())<1e-10; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void neumann_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }

		void set_parameters(const json &params) override;

		bool is_dimention_dirichet(const int tag, const int dim) const override;
		bool all_dimentions_dirichelt() const override { return all_dimentions_dirichelt_; }

		// bool is_mixed() const override { return is_mixed_; }
	private:
		bool all_dimentions_dirichelt_ = true;
		// bool is_mixed_ = false;

		std::vector<Eigen::Matrix<ExpressionValue, 1, 3, Eigen::RowMajor>> forces_;
		std::vector<Eigen::Matrix<ExpressionValue, 1, 3, Eigen::RowMajor>> displacements_;

		std::vector<Eigen::Matrix<bool, 1, 3, Eigen::RowMajor>> dirichelt_dimentions_;

		Eigen::Matrix<double, 1, 3, Eigen::RowMajor> rhs_;
		bool is_all_;
	};

	class GenericScalarProblem: public Problem
	{
	public:
		GenericScalarProblem(const std::string &name);

		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return fabs(rhs_) < 1e-10; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void neumann_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return true; }

		void set_parameters(const json &params) override;

	private:
		std::vector<Eigen::Matrix<ExpressionValue, 1, 1, Eigen::RowMajor>> neumann_;
		std::vector<Eigen::Matrix<ExpressionValue, 1, 1, Eigen::RowMajor>> dirichlet_;

		double rhs_;
		bool is_all_;
	};
}


/*

Copyright (c) 2005-2018, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 University of Oxford 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 HOLDER 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.

*/

#ifndef AVERAGESOURCEPARABOLICPDE_HPP_
#define AVERAGESOURCEPARABOLICPDE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractCellPopulation.hpp"
#include "TetrahedralMesh.hpp"
#include "AbstractLinearParabolicPde.hpp"

/**
 * A parabolic PDE to be solved numerically using the finite element method, for
 * coupling to a cell-based simulation.
 *
 * The PDE takes the form
 *
 * c*du/dt = Grad.(D*Grad(u)) + k*u*rho(x),
 *
 * where the scalars c, D and k are specified by the members mDuDtCoefficient,
 * mDiffusionCoefficient and mSourceCoefficient, respectively. Their values must
 * be set in the constructor.
 *
 * The function rho(x) denotes the local density of non-apoptotic cells. This
 * quantity is computed for each element of a 'coarse' finite element mesh that is
 * passed to the method SetupSourceTerms() and stored in the member mCellDensityOnCoarseElements.
 * For a point x, rho(x) is defined to be the number of non-apoptotic cells whose
 * centres lie in each finite element containing that point, scaled by the area of
 * that element.
 */
template<unsigned DIM>
class AveragedSourceParabolicPde : public AbstractLinearParabolicPde<DIM,DIM>
{
    friend class TestCellBasedParabolicPdes;

private:

    /** Needed for serialization.*/
    friend class boost::serialization::access;
    /**
     * Serialize the PDE and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
       archive & boost::serialization::base_object<AbstractLinearParabolicPde<DIM, DIM> >(*this);
       archive & mDuDtCoefficient;
       archive & mDiffusionCoefficient;
       archive & mSourceCoefficient;
       archive & mCellDensityOnCoarseElements;
    }

protected:

    /** The cell population member. */
    AbstractCellPopulation<DIM, DIM>& mrCellPopulation;

    /** Coefficient of rate of change term.  */
    double mDuDtCoefficient;

    /** Diffusion coefficient. */
    double mDiffusionCoefficient;

    /** Coefficient of the rate of uptake of the dependent variable by non-apoptotic cells. */
    double mSourceCoefficient;

    /** Vector of averaged cell densities on elements of the coarse mesh. */
    std::vector<double> mCellDensityOnCoarseElements;

public:

    /**
     * Constructor.
     *
     * @param rCellPopulation reference to the cell population
     * @param duDtCoefficient rate of reaction (defaults to 1.0)
     * @param diffusionCoefficient rate of diffusion (defaults to 1.0)
     * @param sourceCoefficient the source term coefficient (defaults to 0.0)
     */
    AveragedSourceParabolicPde(AbstractCellPopulation<DIM, DIM>& rCellPopulation,
                               double duDtCoefficient=1.0,
                               double diffusionCoefficient=1.0,
                               double sourceCoefficient=0.0);

    /**
     * @return const reference to the cell population (used in archiving).
     */
    const AbstractCellPopulation<DIM>& rGetCellPopulation() const;

    /**
     * Set up the source terms.
     *
     * \todo this is identical to the one in AveragedSourceEllipticPde so refactor.
     *
     * @param rCoarseMesh reference to the coarse mesh
     * @param pCellPdeElementMap optional pointer to the map from cells to coarse elements
     */
    void virtual SetupSourceTerms(TetrahedralMesh<DIM,DIM>& rCoarseMesh, std::map<CellPtr, unsigned>* pCellPdeElementMap=nullptr);

    /**
     * Overridden ComputeDuDtCoefficientFunction() method.
     *
     * @return the function c(x) in "c(x) du/dt = Grad.(DiffusionTerm(x)*Grad(u))+LinearSourceTerm(x)+NonlinearSourceTerm(x, u)"
     *
     * @param rX the point in space at which the function c is computed
     */
    virtual double ComputeDuDtCoefficientFunction(const ChastePoint<DIM>& rX);

    /**
     * Overridden ComputeSourceTerm() method.
     *
     * @return computed source term.
     *
     * @param rX the point in space at which the nonlinear source term is computed
     * @param u the value of the dependent variable at the point
     * @param pElement the mesh element that x is contained in (optional; defaults to NULL).
     */
    virtual double ComputeSourceTerm(const ChastePoint<DIM>& rX,
                                     double u,
                                     Element<DIM,DIM>* pElement=NULL);

    /**
     * Overridden ComputeSourceTermAtNode() method. That is never called.
     *
     * @return computed source term at a node.
     *
     * @param rNode the node at which the nonlinear source term is computed
     * @param u the value of the dependent variable at the node
     */
    virtual double ComputeSourceTermAtNode(const Node<DIM>& rNode, double u);

    /**
     * Overridden ComputeDiffusionTerm() method.
     *
     * @param rX the point in space at which the diffusion term is computed
     * @param pElement the mesh element that x is contained in (optional; defaults to NULL).
     *
     * @return a matrix.
     */
    virtual c_matrix<double,DIM,DIM> ComputeDiffusionTerm(const ChastePoint<DIM>& rX, Element<DIM,DIM>* pElement=NULL);

    /**
     * @return the uptake rate.
     *
     * @param elementIndex the element we wish to return the uptake rate for
     */
    double GetUptakeRateForElement(unsigned elementIndex);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(AveragedSourceParabolicPde)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a AveragedSourceParabolicPde.
 */
template<class Archive, unsigned DIM>
inline void save_construct_data(
    Archive & ar, const AveragedSourceParabolicPde<DIM>* t, const unsigned int file_version)
{
    // Save data required to construct instance
    const AbstractCellPopulation<DIM, DIM>* p_cell_population = &(t->rGetCellPopulation());
    ar & p_cell_population;
}

/**
 * De-serialize constructor parameters and initialise a AveragedSourceParabolicPde.
 */
template<class Archive, unsigned DIM>
inline void load_construct_data(
    Archive & ar, AveragedSourceParabolicPde<DIM>* t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    AbstractCellPopulation<DIM, DIM>* p_cell_population;
    ar >> p_cell_population;

    // Invoke inplace constructor to initialise instance
    ::new(t)AveragedSourceParabolicPde<DIM>(*p_cell_population);
}
}
} // namespace ...

#endif /*AVERAGESOURCEPARABOLICPDE_HPP_*/


#include <CGAL/Simple_cartesian.h>
#include <CGAL/boost/graph/graph_traits_Triangulation_2.h>

#include <boost/graph/graph_test.hpp>
#include <boost/graph/random.hpp>
#include <boost/assign.hpp>

typedef CGAL::Simple_cartesian<double> Kernel;
typedef CGAL::Triangulation_2<Kernel> Triangulation;
typedef typename Triangulation::Point Point;
typedef typename Triangulation::Vertex Vertex;
typedef typename Triangulation::Vertex Vertex_handle;

int test_main(int, char*[])
{
  using namespace boost::assign;

  Triangulation t;
  {
    std::vector<Point> v;
    // taken from the triangulation test cases, should be randomly generated
    v += Point(5,6,1), Point(1,9,1), Point(6,14,1), Point(4,12,1), Point(3,29,1),  Point(6,7,1),
      Point(6,39,1), Point(8,9,1), Point(10,18,1), Point(75625,155625,10000),
      Point(10,50,2), Point(6,15,2), Point(6,16,2), Point(10,11,1),
      Point(10,40,1), Point(60,-10,1);

    t.insert(v.begin(), v.end());
  }

  typedef typename boost::graph_traits<Triangulation>::vertex_descriptor vertex_t;
  std::vector<vertex_t> vv;
  for(typename Triangulation::Finite_vertices_iterator it = t.finite_vertices_begin();
      it != t.finite_vertices_end(); ++it) {
    vv.push_back(it);
  }

  std::vector< std::pair<vertex_t, vertex_t> > e;
  for(Triangulation::All_edges_iterator it = t.all_edges_begin();
      it != t.all_edges_end(); ++it) {
    e.push_back(
      std::make_pair(
        it->first->vertex((it->second + 2) % 3)
        , it->first->vertex((it->second + 1) % 3)));
  }

  boost::graph_test<Triangulation> gt;
  gt.test_bidirectional_graph(vv, e, t);
  return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/strategies/strategy_transform.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

template <typename T, typename P>
inline T check_distance(P const& p)
{
    T x = bg::get<0>(p);
    T y = bg::get<1>(p);
    T z = bg::get<2>(p);
    return sqrt(x * x + y * y + z * z);
}

template <typename T>
void test_transformations_spherical()
{
    T const input_long = 15.0;
    T const input_lat = 5.0;

    T const expected_long = 0.26179938779914943653855361527329;
    T const expected_lat = 0.08726646259971647884618453842443;

    // Can be checked using http://www.calc3d.com/ejavascriptcoordcalc.html
    // (for phi use long, in radians, for theta use lat, in radians, they are listed there as "theta, phi")
    T const expected_polar_x = 0.084186;
    T const expected_polar_y = 0.0225576;
    T const expected_polar_z = 0.996195;

    // Can be checked with same URL using 90-theta for lat.
    // So for theta use 85 degrees, in radians: 0.08726646259971647884618453842443
    T const expected_equatorial_x = 0.962250;
    T const expected_equatorial_y = 0.257834;
    T const expected_equatorial_z = 0.0871557;

    // 1: Spherical-polar (lat=5, so it is near the pole - on a unit sphere)
    bg::model::point<T, 2, bg::cs::spherical<bg::degree> > sp(input_long, input_lat);

    // 1a: to radian
    bg::model::point<T, 2, bg::cs::spherical<bg::radian> > spr;
    bg::transform(sp, spr);
    BOOST_CHECK_CLOSE(bg::get<0>(spr), expected_long, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(spr), expected_lat, 0.001);

    // 1b: to cartesian-3d
    bg::model::point<T, 3, bg::cs::cartesian> pc3;
    bg::transform(sp, pc3);
    BOOST_CHECK_CLOSE(bg::get<0>(pc3), expected_polar_x, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(pc3), expected_polar_y, 0.001);
    BOOST_CHECK_CLOSE(bg::get<2>(pc3), expected_polar_z, 0.001);
    BOOST_CHECK_CLOSE(check_distance<T>(pc3), 1.0, 0.001);

    // 1c: back
    bg::transform(pc3, spr);
    BOOST_CHECK_CLOSE(bg::get<0>(spr), expected_long, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(spr), expected_lat, 0.001);

    // 2: Spherical-equatorial (lat=5, so it is near the equator)
    bg::model::point<T, 2, bg::cs::spherical_equatorial<bg::degree> > se(input_long, input_lat);

    // 2a: to radian
    bg::model::point<T, 2, bg::cs::spherical_equatorial<bg::radian> > ser;
    bg::transform(se, ser);
    BOOST_CHECK_CLOSE(bg::get<0>(ser), expected_long, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(ser), expected_lat, 0.001);

    bg::transform(se, pc3);
    BOOST_CHECK_CLOSE(bg::get<0>(pc3), expected_equatorial_x, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(pc3), expected_equatorial_y, 0.001);
    BOOST_CHECK_CLOSE(bg::get<2>(pc3), expected_equatorial_z, 0.001);
    BOOST_CHECK_CLOSE(check_distance<T>(pc3), 1.0, 0.001);

    // 2c: back
    bg::transform(pc3, ser);
    BOOST_CHECK_CLOSE(bg::get<0>(spr), expected_long, 0.001);  // expected_long
    BOOST_CHECK_CLOSE(bg::get<1>(spr), expected_lat, 0.001); // expected_lat

    // 3: Spherical-polar including radius
    bg::model::point<T, 3, bg::cs::spherical<bg::degree> > sp3(input_long, input_lat, 0.5);

    // 3a: to radian
    bg::model::point<T, 3, bg::cs::spherical<bg::radian> > spr3;
    bg::transform(sp3, spr3);
    BOOST_CHECK_CLOSE(bg::get<0>(spr3), expected_long, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(spr3), expected_lat, 0.001);
    BOOST_CHECK_CLOSE(bg::get<2>(spr3), 0.5, 0.001);

    // 3b: to cartesian-3d
    bg::transform(sp3, pc3);
    BOOST_CHECK_CLOSE(bg::get<0>(pc3), expected_polar_x / 2.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(pc3), expected_polar_y / 2.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<2>(pc3), expected_polar_z / 2.0, 0.001);
    BOOST_CHECK_CLOSE(check_distance<T>(pc3), 0.5, 0.001);

    // 3c: back
    bg::transform(pc3, spr3);
    BOOST_CHECK_CLOSE(bg::get<0>(spr3), expected_long, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(spr3), expected_lat, 0.001);
    BOOST_CHECK_CLOSE(bg::get<2>(spr3), 0.5, 0.001);

    // 4: Spherical-equatorial including radius
    bg::model::point<T, 3, bg::cs::spherical_equatorial<bg::degree> > se3(input_long, input_lat, 0.5);

    // 4a: to radian
    bg::model::point<T, 3, bg::cs::spherical_equatorial<bg::radian> > ser3;
    bg::transform(se3, ser3);
    BOOST_CHECK_CLOSE(bg::get<0>(ser3), expected_long, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(ser3), expected_lat, 0.001);
    BOOST_CHECK_CLOSE(bg::get<2>(ser3), 0.5, 0.001);

    // 4b: to cartesian-3d
    bg::transform(se3, pc3);
    BOOST_CHECK_CLOSE(bg::get<0>(pc3), expected_equatorial_x / 2.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(pc3), expected_equatorial_y / 2.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<2>(pc3), expected_equatorial_z / 2.0, 0.001);
    BOOST_CHECK_CLOSE(check_distance<T>(pc3), 0.5, 0.001);

    // 4c: back
    bg::transform(pc3, ser3);
    BOOST_CHECK_CLOSE(bg::get<0>(ser3), expected_long, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(ser3), expected_lat, 0.001);
    BOOST_CHECK_CLOSE(bg::get<2>(ser3), 0.5, 0.001);
}

int test_main(int, char* [])
{
    test_transformations_spherical<double>();

    return 0;
}


#include "spectral_clustering.h"

#include <set>
#include <array>
#include <algorithm>

#include <Eigen/Sparse>
#include <igl/eigs.h>

#include <algorithms/shortest_path.h>

using namespace shortest_path;

SpectralClustering::SpectralClustering(std::shared_ptr<Geometry> geometry, unsigned int k, double delta, double eta):
	_delta(delta),
	_eta(eta) {

	if (geometry == nullptr) {
		return;
	}

	// Generate affinity matrix from mesh faces
	Eigen::MatrixXd W = affinity_matrix(geometry);

	// Degree matrix
	Eigen::DiagonalMatrix<double, -1> Ds = W.rowwise().sum().cwiseInverse().cwiseSqrt().asDiagonal();

	// Graph Laplacian
	Eigen::MatrixXd L = Ds * (W * Ds);

	Eigen::MatrixXd M = Eigen::MatrixXd::Identity(L.rows(), L.cols());

	Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> es(L, M, Eigen::ComputeEigenvectors);
	Eigen::MatrixXd U = es.eigenvectors().real().rightCols(k);

	// kmeans clustering
	Eigen::MatrixXd V = U.colwise().normalized();
	Eigen::MatrixXd Q = V * V.transpose();

	// Group faces by clusters
	Eigen::VectorXi guess = initial_guess(Q, k);

	_segment_by_face = k_means_lloyds(guess, V, k);
}

SpectralClustering::~SpectralClustering() {
}

const Eigen::VectorXi& SpectralClustering::segment_by_face() const {
	return _segment_by_face;
}

Eigen::MatrixXd SpectralClustering::affinity_matrix(std::shared_ptr<Geometry> geometry) {
	Eigen::SparseMatrix<double> G(geometry->faces().rows(), geometry->faces().rows());
	Eigen::SparseMatrix<double> A(geometry->faces().rows(), geometry->faces().rows());

	std::set<Eigen::DenseIndex> eta_list;
	unsigned int num_adj = 0;
	const Eigen::SparseMatrix<int>& adj = geometry->adjacency_matrix();
	const trimesh::trimesh_t& halfedge_mesh = geometry->halfedge();

	unsigned int count = 0;

	std::vector<Eigen::Triplet<double>> g_triplets;
	std::vector<Eigen::Triplet<double>> a_triplets;
	Eigen::DenseIndex v_count = geometry->vertices().rows();
	for (Eigen::DenseIndex i = 0; i < v_count; ++i) {
		for (Eigen::DenseIndex j = i + 1; j < v_count; ++j) {
			trimesh::index_t he_index = halfedge_mesh.directed_edge2he_index(i, j);

			if (he_index < 0) {
				// Invalid vertex pair??
				continue;
			}

			const trimesh::trimesh_t::halfedge_t& he = halfedge_mesh.halfedge(he_index);
			const trimesh::trimesh_t::halfedge_t& op_he = halfedge_mesh.halfedge(he.opposite_he);

			if (he.face < 0 || op_he.face < 0) {
				// This edge is on a boundary!
				continue;
			}

			Eigen::DenseIndex r = he.face;
			Eigen::DenseIndex s = op_he.face;

			Eigen::VectorXi face1 = geometry->faces().row(r);
			Eigen::VectorXi face2 = geometry->faces().row(s);

			double gd = geodesic_distance({ he.to_vertex, op_he.to_vertex }, face1, face2, geometry->vertices());
			double ad = angular_distance(face1, face2, geometry->vertices());

			g_triplets.emplace_back(Eigen::Triplet<double>(r, s, gd));
			g_triplets.emplace_back(Eigen::Triplet<double>(s, r, gd));
			a_triplets.emplace_back(Eigen::Triplet<double>(r, s, ad));
			a_triplets.emplace_back(Eigen::Triplet<double>(s, r, ad));

			++count;
		}
	}

	double geodesic_avg = 0.0;
	double angular_avg = 0.0;

	G.setFromTriplets(g_triplets.begin(), g_triplets.end());
	A.setFromTriplets(a_triplets.begin(), a_triplets.end());

	if (count > 0) {
		geodesic_avg = G.sum() / static_cast<double>(2 * count);
		angular_avg = A.sum() / static_cast<double>(2 * count);
	}

	G = G * (_delta / geodesic_avg);
	A = A * ((1.0 - _delta) / angular_avg);

	Eigen::MatrixXd W = floyd_warshall(G + A);

	std::vector<std::pair<Eigen::DenseIndex, Eigen::DenseIndex>> inf_entries;
	for (Eigen::DenseIndex i = 0; i < W.rows(); ++i) {
		for (Eigen::DenseIndex j = 0; j < W.cols(); ++j) {
			if (W(i, j) > std::numeric_limits<double>::max()) {
				W(i, j) = 0.0;
				inf_entries.emplace_back(std::pair<Eigen::DenseIndex, Eigen::DenseIndex>(i, j));
			}
		}
	}

	double sigma = W.sum() / (std::pow(geometry->faces().rows(), 2.0));
	double den = 2 * std::pow(sigma, 2.0);
	W = -1.0 * W / den;

	for (Eigen::DenseIndex i = 0; i < W.rows(); ++i) {
		for (Eigen::DenseIndex j = 0; j < W.cols(); ++j) {
			W(i, j) = std::exp(W(i, j));
		}
	}

	for (auto it : inf_entries) {
		W(it.first, it.second) = 0.0;
	}

	for (Eigen::DenseIndex i = 0; i < W.rows(); ++i) {
		W(i, i) = 1.0;
	}

	return W;
}

double SpectralClustering::geodesic_distance(std::array<Eigen::DenseIndex,2> edge_verts, const Eigen::VectorXi& face1, const Eigen::VectorXi& face2, const Eigen::MatrixXd& V) const {
	if (face1.size() <= 0 || face2.size() <= 0) {
		return 0.0;
	}

	Eigen::VectorXd edge_center = (V.row(edge_verts[0]) + V.row(edge_verts[1])).transpose() / 2.0;

	Eigen::VectorXd face1_center = Eigen::VectorXd::Zero(edge_center.rows());
	for (Eigen::DenseIndex i = 0; i < face1.size(); ++i) {
		face1_center += V.row(face1(i)).transpose();
	}
	face1_center /= face1.size();

	Eigen::VectorXd face2_center = Eigen::VectorXd::Zero(edge_center.rows());
	for (Eigen::DenseIndex i = 0; i < face2.size(); ++i) {
		face2_center += V.row(face2(i)).transpose();
	}
	face2_center /= face2.size();

	return (edge_center - face1_center).norm() + (edge_center - face2_center).norm();
}

double SpectralClustering::angular_distance(const Eigen::VectorXi& face1, const Eigen::VectorXi& face2, const Eigen::MatrixXd& V) const {
	if (face1.size() != 3 || face2.size() != 3) {
		return 0.0;
	}

	Eigen::Vector3d e1 = (V.row(face1(0)) - V.row(face1(1))).block<1, 3>(0, 0).normalized().transpose();
	Eigen::Vector3d e2 = (V.row(face1(2)) - V.row(face1(1))).block<1, 3>(0, 0).normalized().transpose();

	Eigen::Vector3d face1_normal = e1.cross(e2).normalized();

	e1 = (V.row(face2(0)) - V.row(face2(1))).block<1, 3>(0, 0).normalized().transpose();
	e2 = (V.row(face2(2)) - V.row(face2(1))).block<1, 3>(0, 0).normalized().transpose();

	Eigen::Vector3d face2_normal = e1.cross(e2).normalized();

	Eigen::VectorXd face1_center = Eigen::VectorXd::Zero(V.row(face1(0)).cols());
	for (Eigen::DenseIndex i = 0; i < face1.size(); ++i) {
		face1_center += V.row(face1(i)).transpose();
	}
	face1_center /= face1.size();

	Eigen::VectorXd face2_center = Eigen::VectorXd::Zero(V.row(face2(0)).cols());
	for (Eigen::DenseIndex i = 0; i < face2.size(); ++i) {
		face2_center += V.row(face2(i)).transpose();
	}
	face2_center /= face2.size();

	bool use_eta = face1_normal.dot((face2_center - face1_center).block<3, 1>(0, 0).normalized()) < 0.0;

	return ((use_eta) ? _eta : 1.0) * (1.0 - face1_normal.dot(face2_normal));
}

Eigen::VectorXi SpectralClustering::k_means_lloyds(const Eigen::VectorXi& guess, const Eigen::MatrixXd& V, unsigned int k) {
	// guess
	Eigen::VectorXi means = guess; //initial_guess(V, k);
	Eigen::VectorXi prev_means = means;

	std::cout << means << std::endl;

	Eigen::VectorXi assignment = Eigen::VectorXi::Constant(V.rows(),-1);
	Eigen::VectorXi next_assignment = assignment;

	double stop_threshold = 0.001 * means.rowwise().norm().minCoeff();

	do {
		// assign
		for (Eigen::DenseIndex i = 0; i < V.rows(); ++i) {
			double min_dist = std::numeric_limits<double>::max();
			Eigen::DenseIndex min_index = -1;

			for (Eigen::DenseIndex j = 0; j < means.rows(); ++j) {
				double dist = (V.row(means(j)) - V.row(i)).norm();

				if (dist < min_dist) {
					min_dist = dist;
					min_index = j;
				}
			}

			if (min_index < 0) {
				// There was no nearest centroid?? Something is wrong..
				throw std::logic_error("Finding nearest centroid failed??");
			}

			next_assignment(i) = min_index;
		}

		if ((assignment - next_assignment).sum() == 0) {
			// converged
			std::cout << "Converged!" << std::endl;
			break;
		}

		// update
		assignment = next_assignment;

		Eigen::MatrixXd numeric_means = Eigen::MatrixXd::Zero(means.rows(), V.cols());
		Eigen::VectorXi c_count = Eigen::VectorXi::Zero(means.rows());
		means = Eigen::VectorXi::Zero(k);

		for (Eigen::DenseIndex i = 0; i < assignment.rows(); ++i) {
			numeric_means.row(assignment(i)) += V.row(i);
			c_count(assignment(i))++;
		}

		for (Eigen::DenseIndex i = 0; i < c_count.rows(); ++i) {
			if (c_count(i) > 0) {
				numeric_means.row(i) /= c_count(i);
			}
		}

		// Find nearest vector in V closest to each numeric mean
		for (Eigen::DenseIndex i = 0; i < numeric_means.rows(); ++i) {
			double min_dist = std::numeric_limits<double>::max();
			Eigen::DenseIndex min_index = -1;

			for (Eigen::DenseIndex j = 0; j < V.rows(); ++j) {
				double dist = (V.row(j) - numeric_means.row(i)).norm();

				if (dist < min_dist) {
					min_dist = dist;
					min_index = j;
				}
			}

			if (min_index < 0) {
				// There was no nearest vector?? Something is wrong..
				throw std::logic_error("Finding nearest point to centroid failed??");
			}

			means(i) = min_index;
		}

		prev_means = means;

	} while (true);

	std::cout << means << std::endl;

	return next_assignment;
}

Eigen::VectorXi SpectralClustering::initial_guess(const Eigen::MatrixXd& Q, unsigned int k) {
	//"""Computes an initial guess for the cluster-centers"""
	int n = Q.rows();
	double min_value = std::numeric_limits<double>::max();
	std::array<int, 2> min_indices = { -1, -1 };

	for (int i = 0; i < Q.rows(); ++i) {
		for (int j = 0; j < Q.cols(); ++j) {
			if (i != j && Q(i, j) < min_value) {
				min_value = Q(i, j);
				min_indices = { i, j };
			}
		}
	}

	std::set<int> chosen = { min_indices[0], min_indices[1] };

	while (chosen.size() < k) {
		double min_max = std::numeric_limits<double>::max();
		double cur_max = 0.0;
		int new_index = -1;

		for (Eigen::DenseIndex i = 0; i < n; ++i) {
			if (chosen.count(i) <= 0) {
				cur_max = std::numeric_limits<double>::min();
				for (auto c : chosen) {
					if (cur_max < Q(c, i)) {
						cur_max = Q(c, i);
					}
				}

				if (min_max - cur_max > 1e-6) {
					min_max = cur_max;
					new_index = i;
				}
			}
		}

		chosen.insert(new_index);
	}

	Eigen::VectorXi guess(chosen.size());
	Eigen::DenseIndex j = 0;
	for (auto c : chosen) {
		guess(j++) = c;
	}

	return guess;
}


// Naive Fibonacci calculation

#include <active/shared.hpp>
#include <active/promise.hpp>
#include <iostream>

#ifdef ACTIVE_USE_BOOST
	#include <boost/make_shared.hpp>
#endif

typedef active::basic ao_type;

struct fib : public active::shared<fib, ao_type>, public active::handle<fib,int>
{
	struct calculate
	{
		int value;
		active::sink<int>::sp result;
	};

	void active_method( calculate calculate )
	{
		if( calculate.value > 2 )
		{
			m_total=0;
			m_result = calculate.result;
			fib::calculate
				lhs = { calculate.value-1, shared_from_this() },
				rhs = { calculate.value-2, shared_from_this() };

			// Note: temporary AO destroyed only after its last message.
			(*active::platform::make_shared<fib>())(lhs);
			(*active::platform::make_shared<fib>())(rhs);
		}
		else
		{
			calculate.result->send(1);
		}
	}

	void active_method( int sub_result )
	{
		if( m_total ) m_result->send(m_total+sub_result);
		else m_total = sub_result;
	}

private:
	int m_total;
	sp m_result;
};

int main(int argc, char**argv)
{
	if( argc<2 ) { std::cout << "Usage: fib N\n"; return 1; }
	active::platform::shared_ptr<active::promise<int> > result = active::platform::make_shared<active::promise<int> >();
	fib::calculate calc = { atoi(argv[1]), result };
	(*active::platform::make_shared<fib>())(calc);
	active::run();
	std::cout << "Result = " << result->get() << std::endl;
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/exponential/include/functions/log.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <vector>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/cover.hpp>
#include <nt2/sdk/unit/args.hpp>

NT2_TEST_CASE_TPL(log,  NT2_SIMD_REAL_TYPES)
{
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  using nt2::unit::args;
  const std::size_t NR = args("samples", NT2_NB_RANDOM_TEST);
  const double ulpd = args("ulpd", 0.5);
  const T min = args("min", T(0));
  const T max = args("max", T(100));

  NT2_CREATE_BUF(in0, T, NR, min, max);

  std::vector<T> ref(NR);
  for(std::size_t i=0; i!=NR; ++i)
    ref[i] = std::log(in0[i]);

  NT2_COVER_ULP_EQUAL(nt2::tag::log_, ((vT,in0)), ref, ulpd);
}


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#include <stapl/array.hpp>
#include <stapl/algorithm.hpp>
#include <stapl/utility/do_once.hpp>
#include <boost/lexical_cast.hpp>

typedef unsigned long long ulong_type;

////////////////////////////////////////////////////////////////////////////////
/// @brief Returns true if a given number is NOT divisible by
///        3 or 5 and returns false otherwise.
////////////////////////////////////////////////////////////////////////////////
struct three_five_divisor
{
  template<typename T>
  bool operator()(T i)
  {
    return !((i % 3) == 0 || (i % 5) == 0);
  }
};

stapl::exit_code stapl_main(int argc, char** argv)
{
  ulong_type num = boost::lexical_cast<ulong_type> (argv[1]);

  // Creates array container of unsigned integers that will be used for storage.
  stapl::array<ulong_type> b(num);

  // Creates view over container.
  stapl::array_view<stapl::array<ulong_type>> vw(b);

  // Fills the container with values from 1 to n.
  stapl::iota(vw, 1);

  // For numbers in the container that return true to the three_five_divisor
  //   functor, they are set to 0.
  stapl::replace_if(vw, three_five_divisor(), 0);

  // Adds the total of all elements in container.
  ulong_type total = stapl::accumulate(vw, (ulong_type)0);

  // Prints the total sum.
  stapl::do_once ([&] {
    std::cout << "The total is: " << total << std::endl;
  });

  return EXIT_SUCCESS;
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [twopower]
#include <boost/simd/bitwise.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>

namespace bs = boost::simd;
using pack_it = bs::pack<std::int32_t, 4>;

int main() {
  pack_it pi = {1, 2, -1, 5};

  std::cout << "---- simd" << '\n'
            << "<- pi =               " << pi << '\n'
            << "-> bs::twopower(pi) = " << bs::twopower(pi) << '\n';

  std::int32_t xi = 4;

  std::cout << "---- scalar" << '\n'
            << "<- xi =               " << xi << '\n'
            << "-> bs::twopower(xi) = " << bs::twopower(xi) << '\n';
  return 0;
}
//! [twopower]