// 2007,2008 © Václav Šmilauer <eudoxos@arcig.cz> 
#include"ConcretePM.hpp"
#include<core/Scene.hpp>
#include<pkg/dem/DemXDofGeom.hpp>
#include<pkg/dem/Shop.hpp>
#include<pkg/common/InteractionLoop.hpp>
#include<pkg/common/Facet.hpp>
#include<pkg/common/Wall.hpp>
#include<pkg/common/Box.hpp>

#ifdef YADE_OPENGL
	#include<lib/opengl/OpenGLWrapper.hpp>
	#include<lib/opengl/GLUtils.hpp>
#endif

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

YADE_PLUGIN((CpmState)(CpmMat)(Ip2_CpmMat_CpmMat_CpmPhys)(Ip2_FrictMat_CpmMat_FrictPhys)(CpmPhys)(Law2_ScGeom_CpmPhys_Cpm)
	#ifdef YADE_OPENGL
		(Gl1_CpmPhys)
	#endif	
		(CpmStateUpdater));


/********************** Ip2_CpmMat_CpmMat_CpmPhys ****************************/


CREATE_LOGGER(Ip2_FrictMat_CpmMat_FrictPhys);
void Ip2_FrictMat_CpmMat_FrictPhys::go(const shared_ptr<Material>& pp1, const shared_ptr<Material>& pp2, const shared_ptr<Interaction>& interaction){
	TIMING_DELTAS_START();
	const shared_ptr<FrictMat>& mat1 = YADE_PTR_CAST<FrictMat>(pp1);
	const shared_ptr<CpmMat>& mat2 = YADE_PTR_CAST<CpmMat>(pp2);
	Ip2_FrictMat_FrictMat_FrictPhys iPhysFunctor = Ip2_FrictMat_FrictMat_FrictPhys();
	iPhysFunctor.frictAngle = frictAngle;
	iPhysFunctor.go(mat1,mat2,interaction);
	TIMING_DELTAS_CHECKPOINT("end of Ip2_FritPhys");
}




CREATE_LOGGER(Ip2_CpmMat_CpmMat_CpmPhys);
void Ip2_CpmMat_CpmMat_CpmPhys::go(const shared_ptr<Material>& pp1, const shared_ptr<Material>& pp2, const shared_ptr<Interaction>& interaction){
	// no updates of an already existing contact necessary
	if (interaction->phys) return;
	TIMING_DELTAS_START();
	shared_ptr<CpmPhys> cpmPhys(new CpmPhys());
	interaction->phys = cpmPhys;
	CpmMat* mat1 = YADE_CAST<CpmMat*>(pp1.get());
	CpmMat* mat2 = YADE_CAST<CpmMat*>(pp2.get());

	// check unassigned values
	if (!mat1->neverDamage) {
		assert(!std::isnan(mat1->sigmaT));
		assert(!std::isnan(mat1->epsCrackOnset));
		assert(!std::isnan(mat1->relDuctility));
	}
	if (!mat2->neverDamage) {
		assert(!std::isnan(mat2->sigmaT));
		assert(!std::isnan(mat2->epsCrackOnset));
		assert(!std::isnan(mat2->relDuctility));
	}

	cpmPhys->damLaw = mat1->damLaw;
	// bodies sharing the same material; no averages necessary
	if (mat1->id>=0 && mat1->id == mat2->id) {
		cpmPhys->E = mat1->young;
		cpmPhys->G = mat1->young*mat1->poisson;
		cpmPhys->tanFrictionAngle = tan(mat1->frictionAngle);
		cpmPhys->undamagedCohesion = mat1->sigmaT;
		cpmPhys->isCohesive = (cohesiveThresholdIter < 0 || scene->iter < cohesiveThresholdIter);
		#define _CPATTR(a) cpmPhys->a=mat1->a
			_CPATTR(epsCrackOnset);
			_CPATTR(relDuctility);
			_CPATTR(equivStrainShearContrib);
			_CPATTR(neverDamage);
			_CPATTR(dmgTau);
			_CPATTR(dmgRateExp);
			_CPATTR(plTau);
			_CPATTR(plRateExp);
			_CPATTR(isoPrestress);
		#undef _CPATTR
	} else {
		// averaging over both materials
		#define _AVGATTR(a) cpmPhys->a=.5*(mat1->a+mat2->a)
			Real e = (!E) ? .5*(mat1->young + mat2->young) : (*E)(mat1->id,mat2->id,mat1->young,mat2->young);
			cpmPhys->E = e;
			cpmPhys->G = .5*(mat1->poisson + mat2->poisson)*cpmPhys->E;
			cpmPhys->tanFrictionAngle = tan(.5*(mat1->frictionAngle + mat2->frictionAngle));
			cpmPhys->undamagedCohesion = .5*(mat1->sigmaT + mat2->sigmaT);
			cpmPhys->isCohesive = (cohesiveThresholdIter < 0 || scene->iter < cohesiveThresholdIter);
			_AVGATTR(epsCrackOnset);
			_AVGATTR(relDuctility);
			_AVGATTR(equivStrainShearContrib);
			cpmPhys->neverDamage = (mat1->neverDamage || mat2->neverDamage);
			_AVGATTR(dmgTau);
			_AVGATTR(dmgRateExp);
			_AVGATTR(plTau);
			_AVGATTR(plRateExp);
			_AVGATTR(isoPrestress);
		#undef _AVGATTR
	}

	// NOTE: some params are not assigned until in Law2_ScGeom_CpmPhys_Cpm, since they need geometry as well; those are:
	// 	crossSection, kn, ks, refLength
	TIMING_DELTAS_CHECKPOINT("end of Ip2_CpmPhys");
}




/********************** CpmPhys ****************************/
CREATE_LOGGER(CpmPhys);

// !! at least one virtual function in the .cpp file
CpmPhys::~CpmPhys(){};

long CpmPhys::cummBetaIter=0, CpmPhys::cummBetaCount=0;

Real CpmPhys::solveBeta(const Real c, const Real N){
	#ifdef YADE_DEBUG
		cummBetaCount++;
	#endif
	const int maxIter = 20;
	const Real maxError = 1e-12;
	Real f, ret = 0.;
	for(int i = 0; i < maxIter; i++){
		#ifdef YADE_DEBUG
			cummBetaIter++;
		#endif
		Real aux = c*exp(N*ret)+exp(ret);
		f = log(aux);
		if (std::abs(f) < maxError) return ret;
		Real df = (c*N*exp(N*ret)+exp(ret))/aux;
		ret -= f/df;
	}
	LOG_FATAL("No convergence after "<<maxIter<<" iters; c="<<c<<", N="<<N<<", ret="<<ret<<", f="<<f);
	throw runtime_error("CpmPhys::solveBeta failed to converge.");
}

Real CpmPhys::computeDmgOverstress(Real dt){
	if (dmgStrain >= epsN*omega) { // unloading, no viscous stress
		dmgStrain = epsN*omega;
		LOG_TRACE("Elastic/unloading, no viscous overstress");
		return 0.;
	}
	Real c = epsCrackOnset*(1-omega)*pow(dmgTau/dt,dmgRateExp)*pow(epsN*omega-dmgStrain,dmgRateExp-1.);
	Real beta = solveBeta(c,dmgRateExp);
	Real deltaDmgStrain = (epsN*omega-dmgStrain)*exp(beta);
	dmgStrain += deltaDmgStrain;
	LOG_TRACE("deltaDmgStrain="<<deltaDmgStrain<<", viscous overstress "<<(epsN*omega-dmgStrain)*E);
	/* σN=Kn(εN-εd); dmgOverstress=σN-(1-ω)*Kn*εN=…=Kn(ω*εN-εd) */
	return (epsN*omega-dmgStrain)*E;
}

Real CpmPhys::computeViscoplScalingFactor(Real sigmaTNorm, Real sigmaTYield,Real dt){
	if (sigmaTNorm<sigmaTYield) return 1.;
	Real c = undamagedCohesion*pow(plTau/(G*dt),plRateExp)*pow(sigmaTNorm-sigmaTYield,plRateExp-1.);
	Real beta = solveBeta(c,plRateExp);
	//LOG_DEBUG("scaling factor "<<1.-exp(beta)*(1-sigmaTYield/sigmaTNorm));
	return 1.-exp(beta)*(1-sigmaTYield/sigmaTNorm);
}

Real CpmPhys::funcG(const Real& kappaD, const Real& epsCrackOnset, const Real& epsFracture, const bool& neverDamage, const int& damLaw) {
	if (kappaD<epsCrackOnset || neverDamage) return 0;
	switch (damLaw) {
		case 0: // linear
			return (1.-epsCrackOnset/kappaD)/(1.-epsCrackOnset/epsFracture);
		case 1: // exponential
			return 1.-(epsCrackOnset/kappaD)*exp(-(kappaD-epsCrackOnset)/epsFracture);
	}
	throw runtime_error("CpmPhys::funcG: wrong damLaw\n");
}

Real CpmPhys::funcGDKappa(const Real& kappaD, const Real& epsCrackOnset, const Real& epsFracture, const bool& /*neverDamage*/, const int& damLaw) {
	switch (damLaw) {
		case 0: // linear
			return epsCrackOnset / ((1.-epsCrackOnset/epsFracture)*kappaD*kappaD);
		case 1: // exponential
			return epsCrackOnset/kappaD * (1./kappaD + 1./epsFracture) * exp(-(kappaD-epsCrackOnset)/epsFracture);
	}
	throw runtime_error("CpmPhys::funcGDKappa: wrong damLaw\n");
}

Real CpmPhys::funcGInv(const Real& omega, const Real& epsCrackOnset, const Real& epsFracture, const bool& neverDamage, const int& damLaw) {
	if (omega==0. || neverDamage) return 0;
	switch (damLaw) {
		case 0: // linear
			return epsCrackOnset / (1. - omega*(1. - epsCrackOnset/epsFracture));
		case 1: // exponential
			// Newton's iterations
			Real fg,dfg,decr,ret=epsCrackOnset,tol=1e-3;
			int maxIter = 100;
			for (int i=0; i<maxIter; i++) {
				fg = - omega + 1. - epsCrackOnset/ret * exp(-(ret-epsCrackOnset)/epsFracture);
				//dfg = (epsCrackOnset/ret/ret - epsCrackOnset*(ret-epsCrackOnset)/ret/epsFracture/epsFracture) * exp(-(ret-epsCrackOnset)/epsFracture);
				dfg = CpmPhys::funcGDKappa(ret,epsCrackOnset,epsFracture,neverDamage,damLaw);
				decr = fg/dfg;
				ret -= decr;
				if (std::abs(decr/epsCrackOnset) < tol) {
					return ret;
				}
			}
			throw runtime_error("CpmPhys::funcGInv: no convergence\n");
	}
	throw runtime_error("CpmPhys::funcGInv: wrong damLaw\n");
}

void CpmPhys::setDamage(Real dmg) {
	if (neverDamage) { return; }
	omega = dmg;
	kappaD = CpmPhys::funcGInv(dmg,epsCrackOnset,epsFracture,neverDamage,damLaw);
}

void CpmPhys::setRelResidualStrength(Real r) {
	if (neverDamage) { return; }
	if (r == 1.) {
		relResidualStrength = r;
		kappaD = omega = 0.;
		return;
	}
	Real k = epsFracture;
	Real g,dg,f,df,tol=1e-3,e0i=1./epsCrackOnset,decr;
	int maxIter = 100;
	int i;
	for (i=0; i<maxIter; i++) {
		g = CpmPhys::funcG(k,epsCrackOnset,epsFracture,neverDamage,damLaw);
		dg = CpmPhys::funcGDKappa(k,epsCrackOnset,epsFracture,neverDamage,damLaw);
		f = -r + (1-g)*k*e0i;
		df = e0i*(1-g-k*dg);
		decr = f/df;
		k -= decr;
		if (std::abs(decr) < tol) {
			kappaD = k;
			omega = CpmPhys::funcG(k,epsCrackOnset,epsFracture,neverDamage,damLaw);
			relResidualStrength = r;
			return;
		}
	}
	throw runtime_error("CpmPhys::setRelResidualStrength: no convergence\n");
}








/********************** Law2_ScGeom_CpmPhys_Cpm ****************************/
CREATE_LOGGER(Law2_ScGeom_CpmPhys_Cpm);



#ifdef YADE_CPM_FULL_MODEL_AVAILABLE
	#include"../../../brefcom-mm.hh"
#endif

// #undef CPM_MATERIAL_MODEL (force trunk version of the model)

Real Law2_ScGeom_CpmPhys_Cpm::elasticEnergy() {
	#ifdef YADE_CPM_FULL_MODEL_AVAILABLE
		CPM_MATERIAL_MODEL_ELE
	#else
	Real ret = 0.;
	FOREACH(const shared_ptr<Interaction>& I, *scene->interactions){
		if(!I->isReal()) continue;
		CpmPhys* phys = dynamic_cast<CpmPhys*>(I->phys.get());
		if(phys) {
			ret += 0.5*phys->normalForce.squaredNorm()/((1-(phys->epsN>0?phys->omega:0))*phys->kn);
			ret += 0.5*phys->shearForce.squaredNorm()/phys->ks;
		}
	}
	return ret;	
	#endif
}








#ifdef YADE_DEBUG
	#define CPM_YADE_DEBUG_A \
		if(std::isnan(epsN)){\
			/*LOG_FATAL("refLength="<<geom->refLength<<"; pos1="<<geom->se31.position<<"; pos2="<<geom->se32.position<<"; displacementN="<<geom->displacementN());*/ \
			throw runtime_error("!! epsN==NaN !!");\
		}
#else
	#define CPM_YADE_DEBUG_A
#endif


#define YADE_VERIFY(condition) if(!(condition)){LOG_FATAL("Verification `"<<#condition<<"' failed!"); LOG_FATAL("in interaction #"<<I->getId1()<<"+#"<<I->getId2()); Omega::instance().saveSimulation("/tmp/verificationFailed.xml"); throw;}
#define NNAN(a) YADE_VERIFY(!std::isnan(a));
#define NNANV(v) YADE_VERIFY(!std::isnan(v[0])); assert(!std::isnan(v[1])); assert(!std::isnan(v[2]));

bool Law2_ScGeom_CpmPhys_Cpm::go(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I){
	TIMING_DELTAS_START();
	ScGeom* geom=static_cast<ScGeom*>(_geom.get());
	CpmPhys* phys=static_cast<CpmPhys*>(_phys.get());

	/* just the first time */
	if (I->isFresh(scene)) {
		const shared_ptr<Body> b1 = Body::byId(I->id1,scene);
		const shared_ptr<Body> b2 = Body::byId(I->id2,scene);
		const int sphereIndex = Sphere::getClassIndexStatic();
		const int facetIndex = Facet::getClassIndexStatic();
		const int wallIndex = Wall::getClassIndexStatic();
		const int boxIndex = Box::getClassIndexStatic();
		const int b1index = b1->shape->getClassIndex();
		const int b2index = b2->shape->getClassIndex();
		if (b1index == sphereIndex && b2index == sphereIndex) { // both bodies are spheres
			const Vector3r& pos1 = Body::byId(I->id1,scene)->state->pos;
			const Vector3r& pos2 = Body::byId(I->id2,scene)->state->pos;
			Real minRad = (geom->refR1 <= 0? geom->refR2 : (geom->refR2 <=0? geom->refR1 : std::min(geom->refR1,geom->refR2)));
			Vector3r shift2 = scene->isPeriodic? Vector3r(scene->cell->hSize*I->cellDist.cast<Real>()) : Vector3r::Zero();
			phys->refLength = (pos2 - pos1 + shift2).norm();
			phys->crossSection = Mathr::PI*pow(minRad,2);
			phys->refPD = geom->refR1 + geom->refR2 - phys->refLength;
		} else if (b1index == facetIndex || b2index == facetIndex || b1index == wallIndex || b2index == wallIndex || b1index == boxIndex || b2index == boxIndex) { // one body is facet or wall or box
			shared_ptr<Body> sphere, plane;
			if (b1index == facetIndex || b1index == wallIndex || b1index == boxIndex) { plane = b1; sphere = b2; }
			else { plane = b2; sphere = b1; }
			Real rad = ( (Sphere*) sphere->shape.get() )->radius;
			phys->refLength = rad;
			phys->crossSection = Mathr::PI*pow(rad,2);
			phys->refPD = 0.;
		}
		phys->kn = phys->crossSection*phys->E/phys->refLength;
		phys->ks = phys->crossSection*phys->G/phys->refLength;
		phys->epsFracture = phys->epsCrackOnset*phys->relDuctility;
	}
	
	/* shorthands */
	Real& epsN(phys->epsN);
	Vector3r& epsT(phys->epsT);
	Vector3r& epsTPl(phys->epsTPl);
	Real& kappaD(phys->kappaD);
	/* Real& epsPlSum(phys->epsPlSum); */
	const Real& E(phys->E); \
	const Real& undamagedCohesion(phys->undamagedCohesion);
	const Real& tanFrictionAngle(phys->tanFrictionAngle);
	const Real& G(phys->G);
	const Real& crossSection(phys->crossSection);
	const Real& omegaThreshold(this->omegaThreshold);
	const Real& epsCrackOnset(phys->epsCrackOnset);
	Real& relResidualStrength(phys->relResidualStrength);
	/*const Real& relDuctility(phys->relDuctility); */
	const Real& epsFracture(phys->epsFracture);
	const int& damLaw(phys->damLaw);
	const bool& neverDamage(phys->neverDamage);
	Real& omega(phys->omega);
	Real& sigmaN(phys->sigmaN);
	Vector3r& sigmaT(phys->sigmaT);
	Real& Fn(phys->Fn);
	Vector3r& Fs(phys->Fs); /* for python access */
	const bool& isCohesive(phys->isCohesive);


	#ifdef CPM_MATERIAL_MODEL
		const Real& dt = scene->dt;
		const Real& dmgTau(phys->dmgTau);
		const Real& plTau(phys->plTau);
		const Real& yieldLogSpeed(this->yieldLogSpeed);
		const int& yieldSurfType(this->yieldSurfType);
		const Real& yieldEllipseShift(this->yieldEllipseShift);
	#endif
	Real& epsNPl(phys->epsNPl);
	const Real& epsSoft(this->epsSoft);
	const Real& relKnSoft(this->relKnSoft);

	TIMING_DELTAS_CHECKPOINT("GO A");
	
	epsN = - (-phys->refPD + geom->penetrationDepth) / phys->refLength;
	//epsT = geom->rotate(epsT);
	geom->rotate(epsT);
	//epsT += geom->shearIncrement() / (phys->refLength + phys->refPD) ; 
	epsT += geom->shearIncrement() / phys->refLength;

	/* debugging */
	CPM_YADE_DEBUG_A

	NNAN(epsN); NNANV(epsT);

	/* constitutive law */
	#ifdef CPM_MATERIAL_MODEL
		CPM_MATERIAL_MODEL
	#else
		/* simplified public model */
		epsN += phys->isoPrestress/E;
		/* very simplified version of the constitutive law */
		Real xi2 = std::pow(phys->equivStrainShearContrib,2);
		Real epsNorm = std::sqrt(std::pow(std::max(epsN-epsNPl,0.),2)+xi2*epsT.squaredNorm());
		kappaD = std::max(epsNorm,kappaD); /* internal variable, max positive strain (non-decreasing) */
		omega = isCohesive? phys->funcG(kappaD,epsCrackOnset,epsFracture,neverDamage,damLaw) : 1.; /* damage variable (non-decreasing, as funcG is also non-decreasing) */
		sigmaN = (1-(epsN-epsNPl>0?omega:0))*E*(epsN-epsNPl); /* damage taken in account in tension only */
		if((epsSoft<0) && (epsN-epsNPl<epsSoft)){ /* plastic slip in compression */
			Real sigmaNSoft=E*(epsSoft+relKnSoft*(epsN-epsNPl-epsSoft));
			if(sigmaNSoft>sigmaN){ /*assert(sigmaNSoft>sigmaN);*/ epsNPl+=(sigmaN-sigmaNSoft)/E; sigmaN=sigmaNSoft; }
		}
		sigmaT = G*(epsT-epsTPl); /* trial stress */
		Real yieldSigmaT = std::max((Real)0.,undamagedCohesion*(1-omega)-sigmaN*tanFrictionAngle); /* Mohr-Coulomb law with damage */
		if (sigmaT.squaredNorm() > yieldSigmaT*yieldSigmaT) {
			Real scale = yieldSigmaT/sigmaT.norm();
			sigmaT *= scale; /* stress return */
			epsTPl += sigmaT*(1-scale)/G;
		}
		relResidualStrength = isCohesive? (kappaD<epsCrackOnset? 1. : (1-omega)*(kappaD)/epsCrackOnset) : 0;
	#endif

	sigmaN -= phys->isoPrestress;
   
   NNAN(sigmaN);
   NNANV(sigmaT);
   NNAN(crossSection);
   if (!neverDamage) {
      NNAN(kappaD);
      NNAN(epsFracture);
      NNAN(omega);
   }

	/* handle broken contacts */
	if (epsN>0. && ((isCohesive && omega>omegaThreshold) || !isCohesive)) {
		/* if (isCohesive) { */
			 const shared_ptr<Body>& body1 = Body::byId(I->getId1(),scene), body2 = Body::byId(I->getId2(),scene); assert(body1); assert(body2);
			 const shared_ptr<CpmState>& st1 = YADE_PTR_CAST<CpmState>(body1->state), st2 = YADE_PTR_CAST<CpmState>(body2->state);
			/* nice article about openMP::critical vs. scoped locks: http://www.thinkingparallel.com/2006/08/21/scoped-locking-vs-critical-in-openmp-a-personal-shootout/ */
			{ boost::mutex::scoped_lock lock(st1->updateMutex); st1->numBrokenCohesive += 1; /* st1->epsPlBroken += epsPlSum; */ }
			{ boost::mutex::scoped_lock lock(st2->updateMutex); st2->numBrokenCohesive += 1; /* st2->epsPlBroken += epsPlSum; */ }
		/* } */
		return false;
	}

	Fn = sigmaN*crossSection; phys->normalForce = -Fn*geom->normal;
	Fs = sigmaT*crossSection; phys->shearForce = -Fs;

	TIMING_DELTAS_CHECKPOINT("GO B");

	Body::id_t id1 = I->getId1();
 	Body::id_t id2 = I->getId2();

	State* b1 = Body::byId(id1,scene)->state.get();
	State* b2 = Body::byId(id2,scene)->state.get();	

	Vector3r f = -phys->normalForce - phys->shearForce;
	if (!scene->isPeriodic) {
		applyForceAtContactPoint(f, geom->contactPoint , id1, b1->se3.position, id2, b2->se3.position);
	} else {
		scene->forces.addForce(id1,f);
		scene->forces.addForce(id2,-f);
		scene->forces.addTorque(id1,(geom->radius1+.5*(phys->refPD-geom->penetrationDepth))*geom->normal.cross(f));
		scene->forces.addTorque(id2,(geom->radius2+.5*(phys->refPD-geom->penetrationDepth))*geom->normal.cross(f));
	}
	TIMING_DELTAS_CHECKPOINT("rest");
	return true;
}








#ifdef YADE_OPENGL
	/********************** Gl1_CpmPhys ****************************/

	CREATE_LOGGER(Gl1_CpmPhys);

	bool Gl1_CpmPhys::contactLine=true;
	bool Gl1_CpmPhys::dmgLabel=true;
	bool Gl1_CpmPhys::dmgPlane=false;
	bool Gl1_CpmPhys::epsNLabel=true;
	bool Gl1_CpmPhys::epsT=false;
	bool Gl1_CpmPhys::epsTAxes=false;
	bool Gl1_CpmPhys::normal=false;
	Real Gl1_CpmPhys::colorStrainRatio=-1;


	void Gl1_CpmPhys::go(const shared_ptr<IPhys>& ip, const shared_ptr<Interaction>& i, const shared_ptr<Body>& /*b1*/, const shared_ptr<Body>& /*b2*/, bool /*wireFrame*/){
		const shared_ptr<CpmPhys>& phys = boost::static_pointer_cast<CpmPhys>(ip);
		const shared_ptr<GenericSpheresContact>& geom = YADE_PTR_CAST<GenericSpheresContact>(i->geom);
		// FIXME: get the scene for periodicity; ugly!
		Scene* scene=Omega::instance().getScene().get();

		//Vector3r lineColor(phys->omega,1-phys->omega,0.0); /* damaged links red, undamaged green */
		Vector3r lineColor = Shop::scalarOnColorScale(1.-phys->relResidualStrength);

		if(colorStrainRatio>0) lineColor = Shop::scalarOnColorScale(phys->epsN/(phys->epsCrackOnset*colorStrainRatio));

		// FIXME: should be computed by the renderer; for now, use the real values
		//Vector3r pos1=geom->se31.position, pos2=geom->se32.position;
		Vector3r pos1 = scene->bodies->operator[](i->id1)->state->pos, pos2 = scene->bodies->operator[](i->id2)->state->pos;
		if (scene->isPeriodic) {
			Vector3r dPos = pos2 - pos1;
			pos1=scene->cell->wrapShearedPt(pos1);
			Vector3r shift2 = scene->isPeriodic? Vector3r(scene->cell->hSize*i->cellDist.cast<Real>()) : Vector3r::Zero();\
			pos2 = pos1 + dPos + shift2;
			//phys->refLength = (pos2 - pos1 + shift2).norm();
			//pos2=pos1+(geom->se32.position-geom->se31.position);
		}
		/*
		if (scene->isPeriodic) {
			Vector3r temp = pos2 - pos1;
			pos1 = scene->cell->wrapShearedPt(pos1);
			pos2 = pos1 + temp;
		}
		*/

		if (contactLine) GLUtils::GLDrawLine(pos1,pos2,lineColor);
		if (dmgLabel) { GLUtils::GLDrawNum(phys->omega,0.5*(pos1+pos2),lineColor); }
		else if (epsNLabel) { GLUtils::GLDrawNum(phys->epsN,0.5*(pos1+pos2),lineColor); }
		if (phys->omega>0 && dmgPlane) {
			Real halfSize = sqrt(1-phys->relResidualStrength)*.5*.705*sqrt(phys->crossSection);
			Vector3r midPt = .5*Vector3r(pos1+pos2);
			glDisable(GL_CULL_FACE);
			glPushMatrix();
				glTranslatev(midPt);
				Quaternionr q; q.setFromTwoVectors(Vector3r::UnitZ(),geom->normal);
				AngleAxisr aa(q);
				glRotate(aa.angle()*Mathr::RAD_TO_DEG,aa.axis()[0],aa.axis()[1],aa.axis()[2]);
				glBegin(GL_POLYGON);
					glColor3v(lineColor); 
					glVertex3d(halfSize,0.,0.);
					glVertex3d(.5*halfSize,.866*halfSize,0.);
					glVertex3d(-.5*halfSize,.866*halfSize,0.);
					glVertex3d(-halfSize,0.,0.);
					glVertex3d(-.5*halfSize,-.866*halfSize,0.);
					glVertex3d(.5*halfSize,-.866*halfSize,0.);
				glEnd();
			glPopMatrix();
		}

		Vector3r cp = boost::static_pointer_cast<GenericSpheresContact>(i->geom)->contactPoint;
		if (scene->isPeriodic) {cp = scene->cell->wrapShearedPt(cp);}
		if (epsT) {
			Real maxShear = (phys->undamagedCohesion-phys->sigmaN*phys->tanFrictionAngle)/phys->G;
			Real relShear = phys->epsT.norm()/maxShear;
			Real scale = phys->refLength;
			Vector3r dirShear = phys->epsT; dirShear.normalize();
			if(epsTAxes){
				GLUtils::GLDrawLine(cp-Vector3r(scale,0,0),cp+Vector3r(scale,0,0));
				GLUtils::GLDrawLine(cp-Vector3r(0,scale,0),cp+Vector3r(0,scale,0));
				GLUtils::GLDrawLine(cp-Vector3r(0,0,scale),cp+Vector3r(0,0,scale));
			}
			GLUtils::GLDrawArrow(cp,cp+dirShear*relShear*scale,Vector3r(1.,0.,0.));
			GLUtils::GLDrawLine(cp+dirShear*relShear*scale,cp+dirShear*scale,Vector3r(.3,.3,.3));

			/* normal strain */ GLUtils::GLDrawArrow(cp,cp+geom->normal*(phys->epsN/maxShear),Vector3r(0.,1.,0.));
		}
		//if(normal) GLUtils::GLDrawArrow(cp,cp+geom->normal*.5*phys->equilibriumDist,Vector3r(0.,1.,0.));
	}
#endif










/********************** CpmStateUpdater ****************************/
CREATE_LOGGER(CpmStateUpdater);
//Real CpmStateUpdater::maxOmega=0.;
//Real CpmStateUpdater::avgRelResidual=0.;

void CpmStateUpdater::update(Scene* _scene){
	Scene *scene = _scene? _scene : Omega::instance().getScene().get();
	vector<BodyStats> bodyStats; bodyStats.resize(scene->bodies->size());
	assert(bodyStats[0].nCohLinks == 0); // should be initialized by dfault ctor
	avgRelResidual = 0; Real nAvgRelResidual = 0;
	Matrix3r identity = Matrix3r::Identity();
	Real dmg;
	Matrix3r incr;
	FOREACH(const shared_ptr<Interaction>& I, *scene->interactions){
		if (!I) continue;
		if (!I->isReal()) continue;
		shared_ptr<CpmPhys> phys = YADE_PTR_DYN_CAST<CpmPhys>(I->phys);
		if (!phys) continue;
		const Body::id_t id1 = I->getId1(), id2 = I->getId2();
		GenericSpheresContact* geom = YADE_CAST<GenericSpheresContact*>(I->geom.get());

		const Vector3r& n = geom->normal;
		const Real& Fn = phys->Fn;
		const Vector3r& Fs = phys->Fs;
		//stress[i,j] += geom->refLength*(Fn*n[i]*n[j]+0.5*(Fs[i]*n[j]+Fs[j]*n[i]));
		//stress += geom->refLength*(Fn*outer(n,n)+.5*(outer(Fs,n)+outer(n,Fs)));
		Matrix3r stress = phys->refLength*(Fn*n*n.transpose()+.5*(Fs*n.transpose()+n*Fs.transpose()));
		
		bodyStats[id1].stress += stress;
		bodyStats[id2].stress += stress;
		bodyStats[id1].nLinks++; bodyStats[id2].nLinks++;
		
		if (!phys->isCohesive) continue;
		bodyStats[id1].nCohLinks++; bodyStats[id1].dmgSum += (1-phys->relResidualStrength); // bodyStats[id1].epsPlSum += phys->epsPlSum;
		bodyStats[id2].nCohLinks++; bodyStats[id2].dmgSum += (1-phys->relResidualStrength); // bodyStats[id2].epsPlSum += phys->epsPlSum;
		maxOmega = std::max(maxOmega,phys->omega);
		avgRelResidual += phys->relResidualStrength;
		nAvgRelResidual += 1;
		for (int i=0; i<3; i++) {
			for (int j=0; j<3; j++) {
				dmg = 1-phys->relResidualStrength;
				incr = -identity*dmg*1.5 + n*n.transpose()*dmg*7.5;
				bodyStats[id1].damageTensor += incr;
				bodyStats[id2].damageTensor += incr;
			}
		}
	}
// 	Real tr;
	FOREACH(shared_ptr<Body> B, *scene->bodies){
		if (!B) continue;
		const Body::id_t& id = B->getId();
		// add damaged contacts that have already been deleted
		CpmState* state = dynamic_cast<CpmState*>(B->state.get());
		if (!state) continue;
		state->stress = bodyStats[id].stress;
		int cohLinksWhenever = bodyStats[id].nCohLinks+state->numBrokenCohesive;
		if (cohLinksWhenever>0) {
			state->normDmg = (bodyStats[id].dmgSum+state->numBrokenCohesive)/cohLinksWhenever;
			// state->normEpsPl = (bodyStats[id].epsPlSum+state->epsPlBroken)/cohLinksWhenever;
			if (state->normDmg>1) {
				LOG_WARN("#"<<id<<" normDmg="<<state->normDmg<<" nCohLinks="<<bodyStats[id].nCohLinks<<", numBrokenCohesive="<<state->numBrokenCohesive<<", dmgSum="<<bodyStats[id].dmgSum<<", numAllCohLinks"<<cohLinksWhenever);
			}
			state->damageTensor = bodyStats[id].damageTensor / cohLinksWhenever;
		}
		else { state->normDmg = 0; /*state->normEpsPl=0;*/ state->damageTensor = Matrix3r::Zero(); }
		B->shape->color = Vector3r(state->normDmg,1-state->normDmg,B->state->blockedDOFs==State::DOF_ALL?0:1);
		nAvgRelResidual += 0.5*state->numBrokenCohesive; // add half or broken interactions, other body has the other half
		Sphere* sphere = dynamic_cast<Sphere*>(B->shape.get());
		if(!sphere) continue;
		Real& r = sphere->radius;
		state->stress = bodyStats[id].stress/(4/3.*Mathr::PI*r*r*r/.62)*.5;
	}
	avgRelResidual /= nAvgRelResidual;
}





#undef YADE_VERIFY
#undef NNAN
#undef NNANV

} // namespace yade

