/*************************************************************************
*  Copyright (C) 2006 by Bruno Chareyre				 	 *
*  bruno.chareyre@grenoble-inp.fr					  	 *
*									 *
*  This program is free software; it is licensed under the terms of the  *
*  GNU General Public License v2 or later. See file LICENSE for details. *
*************************************************************************/

#include "TriaxialStressController.hpp"
#include <lib/high-precision/Constants.hpp>
#include <core/Clump.hpp>
#include <core/Scene.hpp>
#include <core/State.hpp>
#include <pkg/common/Box.hpp>
#include <pkg/common/Sphere.hpp>
#include <pkg/dem/FrictPhys.hpp>
#include <pkg/dem/ScGeom.hpp>
#include <assert.h>
#include <preprocessing/dem/Shop.hpp>

#ifdef FLOW_ENGINE
//#include<pkg/pfv/FlowEngine.hpp>
#include "FlowEngine_FlowEngineT.hpp"
#endif

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

CREATE_LOGGER(TriaxialStressController);
YADE_PLUGIN((TriaxialStressController));

TriaxialStressController::~TriaxialStressController() { }

Vector3r TriaxialStressController::getStress(int boundId) const
{
	assert(boundId >= 0 && boundId <= 5);
	return stress[boundId];
}

Vector3r TriaxialStressController::getStrainRate() const
{
	if (scene->bodies->exists(wall_right_id) and scene->bodies->exists(wall_left_id) and scene->bodies->exists(wall_top_id)
	    and scene->bodies->exists(wall_bottom_id) and scene->bodies->exists(wall_front_id) and scene->bodies->exists(wall_back_id)) {
		return Vector3r(
		        (Body::byId(wall_right_id, scene)->state->vel[0] - Body::byId(wall_left_id, scene)->state->vel[0]) / width,
		        (Body::byId(wall_top_id, scene)->state->vel[1] - Body::byId(wall_bottom_id, scene)->state->vel[1]) / height,
		        (Body::byId(wall_front_id, scene)->state->vel[2] - Body::byId(wall_back_id, scene)->state->vel[2]) / depth);
	} else {
		return Vector3r::Zero();
	}
}

void TriaxialStressController::updateStiffness()
{
	Real fluidStiffness = 0.;
#ifdef FLOW_ENGINE
	for (const auto& e : Omega::instance().getScene()->engines) {
		if (e->getClassName() == "FlowEngine") {
			TemplateFlowEngine_FlowEngineT<FlowCellInfo_FlowEngineT, FlowVertexInfo_FlowEngineT>* flow
			        = dynamic_cast<TemplateFlowEngine_FlowEngineT<FlowCellInfo_FlowEngineT, FlowVertexInfo_FlowEngineT>*>(e.get());
			if ((flow->fluidBulkModulus > 0) && (!(flow->dead))) fluidStiffness = flow->fluidBulkModulus / porosity;
		}
	}
#endif
	for (int i = 0; i < 6; ++i)
		stiffness[i] = 0;
	InteractionContainer::iterator ii    = scene->interactions->begin();
	InteractionContainer::iterator iiEnd = scene->interactions->end();
	for (; ii != iiEnd; ++ii)
		if ((*ii)->isReal()) {
			const shared_ptr<Interaction>& contact = *ii;
			Real                           fn      = (static_cast<FrictPhys*>(contact->phys.get()))->normalForce.norm();
			if (fn != 0) {
				int id1 = contact->getId1(), id2 = contact->getId2();
				for (int index = 0; index < 6; ++index)
					if (wall_id[index] == id1 || wall_id[index] == id2) {
						FrictPhys* currentContactPhysics = static_cast<FrictPhys*>(contact->phys.get());
						stiffness[index] += currentContactPhysics->kn;
					}
			}
		}
	if (fluidStiffness > 0) {
		stiffness[0] += fluidStiffness * width * depth / height;
		stiffness[1] += fluidStiffness * width * depth / height;
		stiffness[2] += fluidStiffness * height * depth / width;
		stiffness[3] += fluidStiffness * height * depth / width;
		stiffness[4] += fluidStiffness * width * height / depth;
		stiffness[5] += fluidStiffness * width * height / depth;
	}
}

void TriaxialStressController::controlExternalStress(
        int wall, Vector3r resultantForce, State* p, Real wall_max_vel) // controls walls such that Sum Forces from Sample on Wall = resultantForce
{
	scene->forces.sync();
	Real       translation = normal[wall].dot(getForce(scene, wall_id[wall]) - resultantForce);
	const bool log         = false;
	if (log)
		LOG_DEBUG(
		        "wall=" << wall << " actualForce=" << getForce(scene, wall_id[wall]) << ", resultantForce=" << resultantForce
		                << ", translation=" << translation);
	if (translation != 0) {
		if (stiffness[wall] != 0) {
			translation /= stiffness[wall];
			if (log) TRVAR2(translation, wall_max_vel * scene->dt)
			translation = math::min(math::abs(translation), wall_max_vel * scene->dt) * math::sign(translation);
		} else
			translation = wall_max_vel * math::sign(translation) * scene->dt;
	}
	previousTranslation[wall]
	        = (1 - stressDamping) * translation * normal[wall] + 0.8 * previousTranslation[wall]; // formula for "steady-flow" evolution with fluctuations
	//Don't update position since Newton is doing that starting from bzr2612
	// 	p->se3.position += previousTranslation[wall];
	externalWork += previousTranslation[wall].dot(getForce(scene, wall_id[wall]));
	// this is important is using VelocityBins. Otherwise the motion is never detected. Related to (old site, fixed bug) https://bugs.launchpad.net/yade/+bug/398089
	p->vel = previousTranslation[wall] / scene->dt;
	//if(log)TRVAR2(previousTranslation,p->se3.position);
}

void TriaxialStressController::action()
{
	// sync thread storage of ForceContainer
	scene->forces.sync();
	if (first) { // sync boundaries ids in the table
		wall_id[wall_bottom] = wall_bottom_id;
		wall_id[wall_top]    = wall_top_id;
		wall_id[wall_left]   = wall_left_id;
		wall_id[wall_right]  = wall_right_id;
		wall_id[wall_front]  = wall_front_id;
		wall_id[wall_back]   = wall_back_id;
	}

	if (thickness < 0) thickness = 2.0 * YADE_PTR_CAST<Box>(Body::byId(wall_bottom_id, scene)->shape)->extents.y();
	State* p_bottom = Body::byId(wall_bottom_id, scene)->state.get();
	State* p_top    = Body::byId(wall_top_id, scene)->state.get();
	State* p_left   = Body::byId(wall_left_id, scene)->state.get();
	State* p_right  = Body::byId(wall_right_id, scene)->state.get();
	State* p_front  = Body::byId(wall_front_id, scene)->state.get();
	State* p_back   = Body::byId(wall_back_id, scene)->state.get();
	height          = p_top->se3.position.y() - p_bottom->se3.position.y() - thickness;
	width           = p_right->se3.position.x() - p_left->se3.position.x() - thickness;
	depth           = p_front->se3.position.z() - p_back->se3.position.z() - thickness;

	boxVolume = height * width * depth;
	if ((first) || (updatePorosity)) {
		particlesVolume = 0;
		for (const auto& b : *scene->bodies) {
			if (b->isClump()) {
				const shared_ptr<Clump>& clump  = YADE_PTR_CAST<Clump>(b->shape);
				const shared_ptr<Body>&  member = Body::byId(clump->members.begin()->first, scene);
				particlesVolume += b->state->mass / member->material->density;
			} else if (b->isDynamic() && !b->isClumpMember()) {
				const shared_ptr<Sphere>& sphere = YADE_PTR_CAST<Sphere>(b->shape);
				particlesVolume += 1.3333333 * Mathr::PI * pow(sphere->radius, 3);
			}
		}
		first          = false;
		updatePorosity = false;
	}
	max_vel1 = 3 * width / (height + width + depth) * max_vel;
	max_vel2 = 3 * height / (height + width + depth) * max_vel;
	max_vel3 = 3 * depth / (height + width + depth) * max_vel;

	porosity        = (boxVolume - particlesVolume) / boxVolume;
	position_top    = p_top->se3.position.y();
	position_bottom = p_bottom->se3.position.y();
	position_right  = p_right->se3.position.x();
	position_left   = p_left->se3.position.x();
	position_front  = p_front->se3.position.z();
	position_back   = p_back->se3.position.z();

	// must be done _after_ height, width, depth have been calculated
	//Update stiffness only if it has been computed by StiffnessCounter (see "stiffnessUpdateInterval")
	if (scene->iter % stiffnessUpdateInterval == 0 || scene->iter < 100) updateStiffness();
	bool isARadiusControlIteration = (scene->iter % radiusControlInterval == 0);

	if (scene->iter % computeStressStrainInterval == 0 || (internalCompaction && isARadiusControlIteration)) computeStressStrain();

	if (!internalCompaction) {
		Vector3r wallForce(0, goal2 * width * depth, 0);
		if (wall_bottom_activated) {
			if (stressMask & 2) controlExternalStress(wall_bottom, wallForce, p_bottom, max_vel2);
			else {
				p_bottom->vel[1] += (-normal[wall_bottom][1] * 0.5 * goal2 * height - p_bottom->vel[1]) * (1 - strainDamping);
				externalWork += p_bottom->vel.dot(getForce(scene, wall_bottom_id)) * scene->dt;
			}
		} else
			p_bottom->vel = Vector3r::Zero();
		if (wall_top_activated) {
			if (stressMask & 2) controlExternalStress(wall_top, -wallForce, p_top, max_vel2);
			else {
				p_top->vel[1] += (-normal[wall_top][1] * 0.5 * goal2 * height - p_top->vel[1]) * (1 - strainDamping);
				externalWork += p_top->vel.dot(getForce(scene, wall_top_id)) * scene->dt;
			}
		} else
			p_top->vel = Vector3r::Zero();

		wallForce = Vector3r(goal1 * height * depth, 0, 0);
		if (wall_left_activated) {
			if (stressMask & 1) controlExternalStress(wall_left, wallForce, p_left, max_vel1);
			else {
				p_left->vel[0] += (-normal[wall_left][0] * 0.5 * goal1 * width - p_left->vel[0]) * (1 - strainDamping);
				externalWork += p_left->vel.dot(getForce(scene, wall_left_id)) * scene->dt;
			}
		} else
			p_left->vel = Vector3r::Zero();
		if (wall_right_activated) {
			if (stressMask & 1) controlExternalStress(wall_right, -wallForce, p_right, max_vel1);
			else {
				p_right->vel[0] += (-normal[wall_right][0] * 0.5 * goal1 * width - p_right->vel[0]) * (1 - strainDamping);
				externalWork += p_right->vel.dot(getForce(scene, wall_right_id)) * scene->dt;
			}
		} else
			p_right->vel = Vector3r::Zero();

		wallForce = Vector3r(0, 0, goal3 * height * width);
		if (wall_back_activated) {
			if (stressMask & 4) controlExternalStress(wall_back, wallForce, p_back, max_vel3);
			else {
				p_back->vel[2] += (-normal[wall_back][2] * 0.5 * goal3 * depth - p_back->vel[2]) * (1 - strainDamping);
				externalWork += p_back->vel.dot(getForce(scene, wall_back_id)) * scene->dt;
			}
		} else
			p_back->vel = Vector3r::Zero();
		if (wall_front_activated) {
			if (stressMask & 4) controlExternalStress(wall_front, -wallForce, p_front, max_vel3);
			else {
				p_front->vel[2] += (-normal[wall_front][2] * 0.5 * goal3 * depth - p_front->vel[2]) * (1 - strainDamping);
				externalWork += p_front->vel.dot(getForce(scene, wall_front_id)) * scene->dt;
			}
		} else
			p_front->vel = Vector3r::Zero();
	} else //if internal compaction
	{
		p_bottom->vel = Vector3r::Zero();
		p_top->vel    = Vector3r::Zero();
		p_left->vel   = Vector3r::Zero();
		p_right->vel  = Vector3r::Zero();
		p_back->vel   = Vector3r::Zero();
		p_front->vel  = Vector3r::Zero();
		if (isARadiusControlIteration) {
			Real sigma_iso_ = bool(stressMask & 1) * goal1 + bool(stressMask & 2) * goal2 + bool(stressMask & 4) * goal3;
			sigma_iso_ /= bool(stressMask & 1) + bool(stressMask & 2) + bool(stressMask & 4);
			if (math::abs(sigma_iso_) <= math::abs(meanStress)) maxMultiplier = finalMaxMultiplier;
			if (meanStress == 0) previousMultiplier = maxMultiplier;
			else {
				//     		previousMultiplier = 1+0.7*(sigma_iso-s)*(previousMultiplier-1.f)/(s-previousStress); // = (Dsigma/apparentModulus)*0.7
				//     		previousMultiplier = math::max(2-maxMultiplier, math::min(previousMultiplier, maxMultiplier));
				if (sigma_iso_
				    < 0) // compressive case: we have to increase radii if meanStress > sigma_iso_, considering that sigma_iso_ < 0. We end with the same expression as before sign change
					previousMultiplier
					        = 1. + (sigma_iso_ - meanStress) / sigma_iso_ * (maxMultiplier - 1.); // = (Dsigma/apparentModulus)*0.7
				else // tensile case: we have to increase radii if meanStress > sigma_iso_ too. But here sigma_iso_ > 0 => another expression
					previousMultiplier
					        = 1. + (meanStress - sigma_iso_) / sigma_iso_ * (maxMultiplier - 1.); // = (Dsigma/apparentModulus)*0.7
			}
			previousStress = meanStress;
			//Real apparentModulus = (s-previousStress)/(previousMultiplier-1.f);
			controlInternalStress(previousMultiplier);
		}
	}
}

void TriaxialStressController::computeStressStrain()
{
	scene->forces.sync();
	State* p_bottom = Body::byId(wall_bottom_id, scene)->state.get();
	State* p_top    = Body::byId(wall_top_id, scene)->state.get();
	State* p_left   = Body::byId(wall_left_id, scene)->state.get();
	State* p_right  = Body::byId(wall_right_id, scene)->state.get();
	State* p_front  = Body::byId(wall_front_id, scene)->state.get();
	State* p_back   = Body::byId(wall_back_id, scene)->state.get();

	height = p_top->se3.position.y() - p_bottom->se3.position.y() - thickness;
	width  = p_right->se3.position.x() - p_left->se3.position.x() - thickness;
	depth  = p_front->se3.position.z() - p_back->se3.position.z() - thickness;

	meanStress = 0;
	if (height0 == 0) height0 = height;
	if (width0 == 0) width0 = width;
	if (depth0 == 0) depth0 = depth;
	strain[0]        = log(width / width0); // all strain values are positiv for extension
	strain[1]        = log(height / height0);
	strain[2]        = log(depth / depth0);
	volumetricStrain = strain[0] + strain[1] + strain[2];

	Real invXSurface = 1.f / (height * depth);
	Real invYSurface = 1.f / (width * depth);
	Real invZSurface = 1.f / (width * height);

	force[wall_bottom]  = getForce(scene, wall_id[wall_bottom]);
	stress[wall_bottom] = force[wall_bottom] * invYSurface; // all stress values are positiv for tension
	force[wall_top]     = getForce(scene, wall_id[wall_top]);
	stress[wall_top]    = -force[wall_top] * invYSurface;
	force[wall_left]    = getForce(scene, wall_id[wall_left]);
	stress[wall_left]   = force[wall_left] * invXSurface;
	force[wall_right]   = getForce(scene, wall_id[wall_right]);
	stress[wall_right]  = -force[wall_right] * invXSurface;
	force[wall_front]   = getForce(scene, wall_id[wall_front]);
	stress[wall_front]  = -force[wall_front] * invZSurface;
	force[wall_back]    = getForce(scene, wall_id[wall_back]);
	stress[wall_back]   = force[wall_back] * invZSurface;

	for (int i = 0; i < 6; i++)
		meanStress += stress[i].dot(pow(-1.0, i) * normal[i]); // normal[i] is always inwards
	meanStress /= 6.;                                              // ( sXX(xLeft) + sXX(xRight) + sYY(yBottom) + sYY(yTop) + sZZ(zBack) + sZZ(zFront) ) / 6
}

void TriaxialStressController::controlInternalStress(Real multiplier)
{
	particlesVolume *= pow(multiplier, 3);
	Shop::growParticles(multiplier, true, true);
}

/*!
    \fn TriaxialStressController::ComputeUnbalancedForce( bool maxUnbalanced)
 */
Real TriaxialStressController::ComputeUnbalancedForce(bool maxUnbalanced) { return Shop::unbalancedForce(maxUnbalanced, scene); }

} // namespace yade