// 2016 © Jan Stránský <jan.stransky@fsv.cvut.cz>
#include "MortarMat.hpp"

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


YADE_PLUGIN((MortarMat)(Ip2_MortarMat_MortarMat_MortarPhys)(MortarPhys)(Law2_ScGeom_MortarPhys_Lourenco))


CREATE_LOGGER(Ip2_MortarMat_MortarMat_MortarPhys);
void Ip2_MortarMat_MortarMat_MortarPhys::go(
        const shared_ptr<Material>& material1, const shared_ptr<Material>& material2, const shared_ptr<Interaction>& interaction)
{
	if (interaction->phys) return;
	if (scene->iter >= cohesiveThresholdIter) { LOG_ERROR("MortarMat not implemented for non-cohesive contacts"); }
	shared_ptr<MortarPhys> phys(new MortarPhys());
	interaction->phys           = phys;
	MortarMat*             mat1 = YADE_CAST<MortarMat*>(material1.get());
	MortarMat*             mat2 = YADE_CAST<MortarMat*>(material2.get());
	GenericSpheresContact* geom = YADE_CAST<GenericSpheresContact*>(interaction->geom.get());

	if (mat1->id >= 0 && mat1->id == mat2->id) {
#define _CPATTR(a) phys->a = mat1->a
		_CPATTR(tensileStrength);
		_CPATTR(compressiveStrength);
		_CPATTR(cohesion);
		_CPATTR(ellAspect);
		_CPATTR(neverDamage);
#undef _CPATTR
		phys->tangensOfFrictionAngle = math::tan(mat1->frictionAngle);
	} else {
// averaging over both materials
#define _MINATTR(a) phys->a = math::min(mat1->a, mat2->a)
#define _AVGATTR(a) phys->a = .5 * (mat1->a + mat2->a)
		_MINATTR(tensileStrength);
		_MINATTR(compressiveStrength);
		_MINATTR(cohesion);
		_AVGATTR(ellAspect);
#undef _AVGATTR
#undef _MINATTR
		phys->neverDamage            = mat1->neverDamage || mat2->neverDamage;
		phys->tangensOfFrictionAngle = math::tan(.5 * (mat1->frictionAngle + mat2->frictionAngle));
	}
	//
	const Real& r1     = geom->refR1;
	const Real& r2     = geom->refR2;
	Real        minRad = r1 <= 0 ? r2 : r2 <= 0 ? r1 : math::min(r1, r2);
	phys->crossSection = math::pow(minRad, 2);
	const Real& E1     = mat1->young;
	const Real& E2     = mat2->young;
	const Real& n1     = mat1->poisson;
	const Real& n2     = mat2->poisson;
	phys->kn           = 2 * E1 * r1 * E2 * r2 / (E1 * r1 + E2 * r2);
	phys->ks           = 2 * E1 * r1 * n1 * E2 * r2 * n2 / (E1 * r1 * n1 + E2 * r2 * n2);
}


CREATE_LOGGER(MortarPhys);

MortarPhys::~MortarPhys() {};

bool MortarPhys::failureCondition(Real sigmaN2, Real sigmaT2)
// declaration of ‘sigmaT’ shadows a member of ‘yade::MortarPhys’ [-Werror=shadow]
// declaration of ‘sigmaN’ shadows a member of ‘yade::MortarPhys’ [-Werror=shadow]
{
	bool cond1 = sigmaN2 - tensileStrength > 0;
	bool cond2 = sigmaT2 + sigmaN2 * tangensOfFrictionAngle - cohesion > 0;
	bool cond3 = math::pow(sigmaN2, 2) + math::pow(ellAspect * sigmaT2, 2) - math::pow(compressiveStrength, 2) > 0;
	return cond1 || cond2 || cond3;
}


/********************** Law2_ScGeom_MortarPhys_Lourenco ****************************/
CREATE_LOGGER(Law2_ScGeom_MortarPhys_Lourenco);
bool Law2_ScGeom_MortarPhys_Lourenco::go(shared_ptr<IGeom>& iGeom, shared_ptr<IPhys>& iPhys, Interaction* interaction)
{
	ScGeom*     geom = static_cast<ScGeom*>(iGeom.get());
	MortarPhys* phys = static_cast<MortarPhys*>(iPhys.get());

	Body::id_t             id1 = interaction->getId1();
	Body::id_t             id2 = interaction->getId2();
	const shared_ptr<Body> b1  = Body::byId(id1, scene);
	const shared_ptr<Body> b2  = Body::byId(id2, scene);

	/* shorthands */
	const Real& crossSection(phys->crossSection);
	Real&       sigmaN(phys->sigmaN);
	Vector3r&   sigmaT(phys->sigmaT);
	Real&       kn(phys->kn);
	Real&       ks(phys->ks);
	Vector3r&   normalForce(phys->normalForce);
	Vector3r&   shearForce(phys->shearForce);


	/* constitutive law */
	normalForce = kn * geom->penetrationDepth * geom->normal;
	geom->rotate(shearForce);
	shearForce -= ks * geom->shearIncrement();
	sigmaN = -normalForce.dot(geom->normal) / crossSection;
	sigmaT = -shearForce / crossSection;

	if (!phys->neverDamage && phys->failureCondition(sigmaN, sigmaT.norm())) { return false; }

	State* s1 = b1->state.get();
	State* s2 = b2->state.get();

	Vector3r f = -normalForce - shearForce;
	if (!scene->isPeriodic) {
		applyForceAtContactPoint(f, geom->contactPoint, id1, s1->se3.position, id2, s2->se3.position);
	} else {
		scene->forces.addForce(id1, f);
		scene->forces.addForce(id2, -f);
		scene->forces.addTorque(id1, (geom->radius1 - .5 * (geom->penetrationDepth)) * geom->normal.cross(f));
		scene->forces.addTorque(id2, (geom->radius2 - .5 * (geom->penetrationDepth)) * geom->normal.cross(f));
	}
	return true;
}

} // namespace yade
