#ifdef YADE_SPH
#include "SPHEngine.hpp"
#include <core/Scene.hpp>
#include <pkg/common/Sphere.hpp>
#include <pkg/dem/ViscoelasticPM.hpp>

#include <core/Omega.hpp>
#include <core/State.hpp>

namespace yade { // Cannot have #include directive inside.

void SPHEngine::action()
{
	{
		YADE_PARALLEL_FOREACH_BODY_BEGIN(const shared_ptr<Body>& b, scene->bodies)
		{
			if (mask > 0 && (b->groupMask & mask) == 0) continue;
			this->calculateSPHRho(b);
			b->state->press = math::max(0.0, k * (b->state->rho - b->state->rho0));
		}
		YADE_PARALLEL_FOREACH_BODY_END();
	}
}

void SPHEngine::calculateSPHRho(const shared_ptr<Body>& b)
{
	if (b->state->rho0 < 0) { b->state->rho0 = rho0; }
	if (not b->isClump()) {
		Real rho = 0;

		// Pointer to kernel function
		KernelFunction kernelFunctionCurDensity = returnKernelFunction(KernFunctionDensity, Norm);

		// Calculate rho for every particle
		for (Body::MapId2IntrT::iterator it = b->intrs.begin(), end = b->intrs.end(); it != end; ++it) {
			const shared_ptr<Body> b2 = Body::byId((*it).first, scene);
			Sphere*                s  = dynamic_cast<Sphere*>(b->shape.get());
			if (!s) continue;

			if (((*it).second)->geom and ((*it).second)->phys) {
				const ScGeom     geom = *(YADE_PTR_CAST<ScGeom>(((*it).second)->geom));
				const ViscElPhys phys = *(YADE_PTR_CAST<ViscElPhys>(((*it).second)->phys));

				Real Mass = b2->state->mass;
				if (Mass == 0) Mass = b->state->mass;

				const Real SmoothDist = (b2->state->pos - b->state->pos).norm();

				// [Monaghan1992], (2.7) (3.8)
				rho += b2->state->mass * kernelFunctionCurDensity(SmoothDist, h);
			}
		}
		// Self mass contribution
		rho += b->state->mass * kernelFunctionCurDensity(0.0, h);
		b->state->rho = rho;
	}
}

Real smoothkernelLucy(const double& r, const double& h)
{
	if (r <= h && h > 0) {
		// Lucy Kernel function, [Lucy1977] (27)
		const Real r_h = r / h;
		return 105. / (16. * M_PI * h * h * h) * (1. + 3. * r_h) * math::pow((1. - r_h), 3);
	} else {
		return 0;
	}
}

Real smoothkernelLucyGrad(const double& r, const double& h)
{
	if (r <= h && h > 0) {
		// 1st derivative of Lucy Kernel function, [Lucy1977] (27)
		return 105. / (16. * M_PI * h * h * h) * (-12. * r) * math::pow((h - r), 2) / (h * h * h * h);
	} else {
		return 0;
	}
}

Real smoothkernelLucyLapl(const double& r, const double& h)
{
	if (r <= h && h > 0) {
		// 2nd derivative of Lucy Kernel function, [Lucy1977] (27)
		return 105. / (16. * M_PI * h * h * h) * (-12.) / (h * h * h * h) * (h * h - 2. * r * h + 3. * r * r);
	} else {
		return 0;
	}
}
//=========================================================================

Real smoothkernelBSpline1(const double& r, const double& h)
{
	// BSpline Kernel function, [Monaghan1985] (21)
	if (r <= 2.0 * h && h > 0) {
		const Real coefA = 3. / (2. * M_PI * h * h * h);
		const Real r_h   = r / h;
		if (r <= h) {
			return coefA * (2. / 3. - r_h * r_h + 1. / 2. * r_h * r_h * r_h);
		} else {
			return coefA / 6. * math::pow((2. - r_h), 3);
		}
	} else {
		return 0;
	}
}

Real smoothkernelBSpline1Grad(const double& r, const double& h)
{
	// 1st derivative of BSpline Kernel function, [Monaghan1985] (21)
	if (r <= 2. * h && h > 0) {
		const Real coefA = 3. / (2. * M_PI * h * h * h);
		const Real r_h   = r / h;
		if (r <= h) {
			return coefA * (-r_h) * (2. - 3. / 2. * r_h);
		} else {
			return coefA * (-1. / 2.) * math::pow((2. - r_h), 2);
		}
	} else {
		return 0;
	}
}

Real smoothkernelBSpline1Lapl(const double& r, const double& h)
{
	// 2nd derivative of BSpline Kernel function, [Monaghan1985] (21)
	if (r <= 2.0 * h && h > 0) {
		const Real coefA = 3. / (2. * M_PI * h * h * h);
		const Real r_h   = r / h;
		if (r <= h) {
			return coefA * (-2. + 3. * r_h);
		} else {
			return coefA * (2. - r_h);
		}
	} else {
		return 0;
	}
}

//=========================================================================

Real smoothkernelBSpline2(const double& r, const double& h)
{
	// BSpline Kernel function, [Monaghan1985] (22)
	if (r <= 2.0 * h && h > 0) {
		const Real coefA = 3. / (4. * M_PI * h * h * h);
		const Real r_h   = r / h;
		if (r <= h) {
			return coefA * (10. / 3. - 7. * r_h * r_h + 4 * r_h * r_h * r_h);
		} else {
			return coefA * math::pow((2. - r_h), 2) * ((5. - 4. * r_h) / 3.);
		}
	} else {
		return 0;
	}
}

Real smoothkernelBSpline2Grad(const double& r, const double& h)
{
	// 1st derivative of BSpline Kernel function, [Monaghan1985] (22)
	if (r <= 2.0 * h && h > 0) {
		const Real coefA = 3. / (4. * M_PI * h * h * h);
		const Real r_h   = r / h;
		if (r <= h) {
			return coefA * (-2.) / (h * h) * (7. * r - 6. * r * r_h);
		} else {
			return coefA * 2. / h * (-6. + 7. * r_h - 2. * math::pow(r_h, 2));
		}
	} else {
		return 0;
	}
}

Real smoothkernelBSpline2Lapl(const double& r, const double& h)
{
	// 2nd derivative of BSpline Kernel function, [Monaghan1985] (22)
	if (r <= 2.0 * h && h > 0) {
		const Real coefA = 3. / (4. * M_PI * h * h * h);
		const Real r_h   = r / h;
		if (r <= h) {
			return coefA * (-2.) / (h * h) * (7. - 12. * r_h);
		} else {
			return coefA * 2. / (h * h) * (7. - 4. * r_h);
		}
	} else {
		return 0;
	}
}

//=========================================================================
KernelFunction returnKernelFunction(const int a, const typeKernFunctions typeF) { return returnKernelFunction(a, a, typeF); }

KernelFunction returnKernelFunction(const int a, const int b, const typeKernFunctions typeF)
{
	if (a != b) { throw runtime_error("Kernel types should be equal!"); }
	if (a == Lucy) {
		if (typeF == Norm) {
			return smoothkernelLucy;
		} else if (typeF == Grad) {
			return smoothkernelLucyGrad;
		} else if (typeF == Lapl) {
			return smoothkernelLucyLapl;
		} else {
			KERNELFUNCDESCR
		}
	} else if (a == BSpline1) {
		if (typeF == Norm) {
			return smoothkernelBSpline1;
		} else if (typeF == Grad) {
			return smoothkernelBSpline1Grad;
		} else if (typeF == Lapl) {
			return smoothkernelBSpline1Lapl;
		} else {
			KERNELFUNCDESCR
		}
	} else if (a == BSpline2) {
		if (typeF == Norm) {
			return smoothkernelBSpline2;
		} else if (typeF == Grad) {
			return smoothkernelBSpline2Grad;
		} else if (typeF == Lapl) {
			return smoothkernelBSpline2Lapl;
		} else {
			KERNELFUNCDESCR
		}
	} else {
		KERNELFUNCDESCR
	}
}

bool computeForceSPH(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I, Vector3r& force)
{
	const ScGeom& geom  = *static_cast<ScGeom*>(_geom.get());
	Scene*        scene = Omega::instance().getScene().get();
	ViscElPhys&   phys  = *static_cast<ViscElPhys*>(_phys.get());

	const int id1 = I->getId1();
	const int id2 = I->getId2();


	const BodyContainer& bodies = *scene->bodies;

	if (bodies[id1]->isClumpMember() and bodies[id2]->isClumpMember() and bodies[id1]->clumpId == bodies[id2]->clumpId) {
		//If 2 bodies belong to the same clump, do not calculate forces
		force = Vector3r::Zero();
		return true;
	}

	//////////////////////////////////////////////////////////////////
	// Copy-paste
	// Handle periodicity.
	const Vector3r shift2   = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist) : Vector3r::Zero();
	const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist) : Vector3r::Zero();

	const State& de1 = *static_cast<State*>(bodies[id1]->state.get());
	const State& de2 = *static_cast<State*>(bodies[id2]->state.get());

	const Vector3r c1x = (geom.contactPoint - de1.pos);
	const Vector3r c2x = (geom.contactPoint - de2.pos - shift2);

	const Vector3r relativeVelocity = (de1.vel + de1.angVel.cross(c1x)) - (de2.vel + de2.angVel.cross(c2x)) + shiftVel;
	const Real     normalVelocity   = geom.normal.dot(relativeVelocity);

	// Copy-paste
	//////////////////////////////////////////////////////////////////


	const Real Mass1 = bodies[id1]->state->mass;
	const Real Mass2 = bodies[id2]->state->mass;

	const Real Rho1 = bodies[id1]->state->rho;
	const Real Rho2 = bodies[id2]->state->rho;

	const Vector3r xixj = de2.pos - de1.pos;

	if (phys.kernelFunctionCurrentPressure(xixj.norm(), phys.h)) {
		Real fpressure = 0.0;
		if (Rho1 != 0.0 and Rho2 != 0.0) {
			// from [Monaghan1992], (3.3), multiply by Mass2, because we need a force, not du/dt
			fpressure = -Mass1 * Mass2 * (bodies[id1]->state->press / (Rho1 * Rho1) + bodies[id2]->state->press / (Rho2 * Rho2))
			        * phys.kernelFunctionCurrentPressure(xixj.norm(), phys.h);
		}

		Vector3r fvisc = Vector3r::Zero();
		if (Rho1 != 0.0 and Rho2 != 0.0) {
			// from [Morris1997], (22), multiply by Mass2, because we need a force, not du/dt
			fvisc = phys.mu * Mass1 * Mass2 * (-normalVelocity * geom.normal) / (Rho1 * Rho2) * 1 / (xixj.norm())
			        * phys.kernelFunctionCurrentPressure(xixj.norm(), phys.h);
			//phys.kernelFunctionCurrentVisco(xixj.norm(), phys.h);
		}
		force = fpressure * geom.normal + fvisc;
		return true;
	} else {
		return false;
	}
}
YADE_PLUGIN((SPHEngine));

} // namespace yade

#endif
