/*************************************************************************
*  Copyright (C) 2013 by Burak ER    burak.er@btu.edu.tr              	 *
*									 *
*                                                                        *
*  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. *
*************************************************************************/

#ifdef YADE_FEM
#include <pkg/fem/DeformableElement.hpp>
#include <pkg/fem/If2_Lin4NodeTetra_LinIsoRayleighDampElast.hpp>

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

YADE_PLUGIN((If2_Lin4NodeTetra_LinIsoRayleighDampElast));
If2_Lin4NodeTetra_LinIsoRayleighDampElast::~If2_Lin4NodeTetra_LinIsoRayleighDampElast() {};


/********************************************************************
                      InternalForceDispatcher
*********************************************************************/

typedef DeformableElement::NodeMap NodeMap;
#define VECTOR12_TEMPLATE(Scalar) Eigen::Matrix<Scalar, 12, 1>
typedef VECTOR12_TEMPLATE(Real) Vector12r;

CREATE_LOGGER(If2_Lin4NodeTetra_LinIsoRayleighDampElast);

void If2_Lin4NodeTetra_LinIsoRayleighDampElast::go(const shared_ptr<Shape>& element, const shared_ptr<Material>& material, const shared_ptr<Body>& /*bdy*/)
{
	shared_ptr<Lin4NodeTetra>              tetel = YADE_PTR_CAST<Lin4NodeTetra>(element);
	shared_ptr<LinIsoRayleighDampElastMat> mat   = YADE_PTR_CAST<LinIsoRayleighDampElastMat>(material);

	DeformableElement::NodeMap::iterator i0(tetel->localmap.begin());
	DeformableElement::NodeMap::iterator i1(i0);
	DeformableElement::NodeMap::iterator i2(i0);
	DeformableElement::NodeMap::iterator i3(i0);

	std::advance(i1, 1);
	std::advance(i2, 2);
	std::advance(i3, 3);

	if (tetel->massMatrixInvProductstiffnessMatrix == 0) {
		Vector3r node0relpos = Vector3r(0, 0, 0);
		Vector3r node1relpos = i1->second.position - i0->second.position;
		Vector3r node2relpos = i2->second.position - i0->second.position;
		Vector3r node3relpos = i3->second.position - i0->second.position;
		// I dont know wheter this is optimum or not
		tetel->massMatrixInvProductstiffnessMatrix = shared_ptr<MatrixXr>(new MatrixXr(
		        tetel->calculateMassMatrix(mat->density, mat->poissonratio).inverse()
		        * tetel->calculateStiffness(mat->youngmodulus, mat->poissonratio, node0relpos, node1relpos, node2relpos, node3relpos)));
	}

	//apply internal forces to the tetrahedron
	//Calculate displacements
	Vector12r displacements;
	Vector12r displacementvelocity;

	displacements << i0->first->state->pos - i0->second.position, i1->first->state->pos - i1->second.position, i2->first->state->pos - i2->second.position,
	        i3->first->state->pos - i3->second.position;
	displacementvelocity << i0->first->state->vel, i1->first->state->vel, i2->first->state->vel, i3->first->state->vel;


	//Now calculate the forces
	Vector12r forces = -(*tetel->massMatrixInvProductstiffnessMatrix) * displacements
	        - (mat->alpha * MatrixXr::Identity(12, 12) + (*tetel->massMatrixInvProductstiffnessMatrix) * (mat->beta)) * displacementvelocity;

	scene->forces.addForce(i0->first->getId(), forces.segment(0, 3));
	scene->forces.addForce(i1->first->getId(), forces.segment(3, 3));
	scene->forces.addForce(i2->first->getId(), forces.segment(6, 3));
	scene->forces.addForce(i3->first->getId(), forces.segment(9, 3));

	return;
}

} // namespace yade

#endif //YADE_FEM
