/*****************************************************************************
*  Copyright (C) 2015 by Anna Effeindzourou   anna.effeindzourou@gmail.com   *
*  Copyright (C) 2015 by Bruno Chareyre       bruno.chareyre@grenoble-inp.fr     *
*  Copyright (C) 2015 by Klaus Thoeni         klaus.thoeni@gmail.com         *
*  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 "PFacet.hpp"
#ifdef YADE_OPENGL
#include <lib/opengl/OpenGLWrapper.hpp>
#endif

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

using math::max;
using math::min; // using inside .cpp file is ok.


//!##################	IGeom Functors   #####################
// Function used in order to calculate the projection of the sphere on the PFacet element. The function returns if the the porjection is on the inside the triangle and the barycentric coordinates of the projection P on PFacet the element.
boost::tuple<Vector3r, bool, double, double, double> Ig2_Sphere_PFacet_ScGridCoGeom::projection(const shared_ptr<Shape>& cm2, const State& state1)
{
	const State* sphereSt = YADE_CAST<const State*>(&state1);
	PFacet*      Pfacet   = YADE_CAST<PFacet*>(cm2.get());

	vector<Vector3r> vertices;
	vertices.push_back(Pfacet->node1->state->pos);
	vertices.push_back(Pfacet->node2->state->pos);
	vertices.push_back(Pfacet->node3->state->pos);
	Vector3r center = vertices[0]
	        + ((vertices[2] - vertices[0]) * (vertices[1] - vertices[0]).norm() + (vertices[1] - vertices[0]) * (vertices[2] - vertices[0]).norm())
	                / ((vertices[1] - vertices[0]).norm() + (vertices[2] - vertices[1]).norm() + (vertices[0] - vertices[2]).norm());


	Vector3r e[3]   = { vertices[1] - vertices[0], vertices[2] - vertices[1], vertices[0] - vertices[2] };
	Vector3r normal = e[0].cross(e[1]) / ((e[0].cross(e[1])).norm());

	// 	Vector3r centerS=sphereSt->pos+shift2;//FIXME: periodicity?
	const Vector3r& centerS = sphereSt->pos;
	Vector3r        cl      = centerS - center;

	Real dist = normal.dot(cl);

	if (dist < 0) {
		normal = -normal;
		dist   = -dist;
	}

	Vector3r P = center + (cl - dist * normal);

	Vector3r v0 = vertices[1] - vertices[0];
	Vector3r v1 = vertices[2] - vertices[0];
	Vector3r v2 = P - vertices[0];

	// Compute dot products
	Real dot00 = v0.dot(v0);
	Real dot01 = v0.dot(v1);
	Real dot02 = v0.dot(v2);
	Real dot11 = v1.dot(v1);
	Real dot12 = v1.dot(v2);

	// Compute the barycentric coordinates of the projection P
	Real invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
	Real p2       = (dot11 * dot02 - dot01 * dot12) * invDenom;
	Real p3       = (dot00 * dot12 - dot01 * dot02) * invDenom;
	Real p1       = 1 - p2 - p3;

	// Check if P is in triangle
	bool isintriangle = (p1 > 0) && (p2 > 0) && (p1 + p2 < 1);
	return boost::make_tuple(P, isintriangle, p1, p2, p3);
}


bool Ig2_Sphere_PFacet_ScGridCoGeom::go(
        const shared_ptr<Shape>& cm1,
        const shared_ptr<Shape>& cm2,
        const State&             state1,
        const State& /*state2*/,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& c)
{
	TIMING_DELTAS_START();

	Sphere* sphere = YADE_CAST<Sphere*>(cm1.get());
	PFacet* Pfacet = YADE_CAST<PFacet*>(cm2.get());

	const State* sphereSt = YADE_CAST<const State*>(&state1);

	Real sphereRadius = sphere->radius;
	Real PFacetradius = Pfacet->radius;

	vector<Vector3r> vertices;
	vertices.push_back(Pfacet->node1->state->pos);
	vertices.push_back(Pfacet->node2->state->pos);
	vertices.push_back(Pfacet->node3->state->pos);
	Vector3r center = vertices[0]
	        + ((vertices[2] - vertices[0]) * (vertices[1] - vertices[0]).norm() + (vertices[1] - vertices[0]) * (vertices[2] - vertices[0]).norm())
	                / ((vertices[1] - vertices[0]).norm() + (vertices[2] - vertices[1]).norm() + (vertices[0] - vertices[2]).norm());


	Vector3r e[3]   = { vertices[1] - vertices[0], vertices[2] - vertices[1], vertices[0] - vertices[2] };
	Vector3r normal = e[0].cross(e[1]) / ((e[0].cross(e[1])).norm());

	// 	Vector3r centerS=sphereSt->pos+shift2;//FIXME: periodicity?
	const Vector3r& centerS = sphereSt->pos;
	Vector3r        cl      = centerS - center;
	Real            dist    = normal.dot(cl);

	shared_ptr<ScGridCoGeom> scm;
	bool                     isNew = !(c->geom);

	if (c->geom) scm = YADE_PTR_CAST<ScGridCoGeom>(c->geom);
	else
		scm = shared_ptr<ScGridCoGeom>(new ScGridCoGeom());

	if (scm->isDuplicate == 2 && scm->trueInt != c->id2) return true; //the contact will be deleted into the Law, no need to compute here.
	scm->isDuplicate = 0;
	scm->trueInt     = -1;


	if (math::abs(dist) > (PFacetradius + sphereRadius) && !c->isReal()
	    && !force) { // no contact, but only if there was no previous contact; ortherwise, the constitutive law is responsible for setting Interaction::isReal=false
		TIMING_DELTAS_CHECKPOINT("Ig2_Sphere_PFacet_ScGridCoGeom");
		return false;
	}
	if (dist < 0) {
		normal = -normal;
		dist   = -dist;
	}


	boost::tuple<Vector3r, bool, double, double, double> projectionres = projection(cm2, state1);
	Vector3r                                             P             = boost::get<0>(projectionres);
	bool                                                 isintriangle  = boost::get<1>(projectionres);
	Real                                                 p1            = boost::get<2>(projectionres);
	Real                                                 p2            = boost::get<3>(projectionres);
	Real                                                 p3            = boost::get<4>(projectionres);


	shared_ptr<Body> GridList[3] = { Pfacet->conn1, Pfacet->conn2, Pfacet->conn3 };

	// Check if the projection of the contact point is inside the triangle
	bool isconn1 = ((p1 > 0) && (p2 <= 0) && (p1 + p2 < 1)) || ((p1 > 0) && (p2 <= 0) && (p1 + p2 >= 1));
	bool isconn2 = ((p1 > 0) && (p2 > 0) && (p1 + p2 >= 1)) || ((p1 <= 0) && (p2 > 0) && (p1 + p2 >= 1));
	bool isconn3 = ((p1 <= 0) && (p2 > 0) && (p1 + p2 < 1)) || ((p1 <= 0) && (p2 <= 0) && (p1 + p2 < 1));

	Real penetrationDepth = 0;
	int  connnum          = -1;

	GridNode* GridNodeList[3] = { YADE_CAST<GridNode*>(Pfacet->node1->shape.get()),
		                      YADE_CAST<GridNode*>(Pfacet->node2->shape.get()),
		                      YADE_CAST<GridNode*>(Pfacet->node3->shape.get()) };
	// 	If the contact projection is in the triangle, a search for an old contact is performed to export the information regarding the contact (phys)
	if (isintriangle) {
		if (isNew) {
			for (int unsigned i = 0; i < 3; i++) {
				for (int unsigned j = 0; j < GridNodeList[i]->pfacetList.size(); j++) {
					if (GridNodeList[i]->pfacetList[j]->getId() != c->id2) {
						boost::tuple<Vector3r, bool, double, double, double> projectionPrev
						        = projection(GridNodeList[i]->pfacetList[j]->shape, state1);
						bool isintrianglePrev = boost::get<1>(projectionPrev);
						if (!isintrianglePrev) { //if(!isintrianglePrev)
							const shared_ptr<Interaction> intr
							        = scene->interactions->find(c->id1, GridNodeList[i]->pfacetList[j]->getId());
							if (intr
							    && intr->isReal()) { // if an interaction exist between the sphere and the previous pfacet, import parameters.
								// 								cout<<"Copying contact with pfacet geom and phys from "<<intr->id1<<"-"<<intr->id2<<" to here ("<<c->id1<<"-"<<c->id2<<")"<<endl;
								scm              = YADE_PTR_CAST<ScGridCoGeom>(intr->geom);
								c->geom          = scm;
								c->phys          = intr->phys;
								c->iterMadeReal  = intr->iterMadeReal; // iteration from the time when the contact became real
								scm->trueInt     = c->id2;
								scm->isDuplicate = 2; //command the old contact deletion.
								isNew            = 0;
								break;
							}
						}
					}
				}
			}
		}
	} else {
		// 		identification of the cylinder possibly in contact with the sphere
		if (isconn1) connnum = 0;
		if (isconn2) connnum = 1;
		if (isconn3) connnum = 2;
		// 		check if the identified cylinder  previously is in contact with the sphere
		if (connnum != -1) {
			//verify if there is a contact between the a neighbouring PFacet, avoid double contacts
			for (int unsigned i = 0; i < 3; i++) {
				for (int unsigned j = 0; j < GridNodeList[i]->pfacetList.size(); j++) {
					if (GridNodeList[i]->pfacetList[j]->getId() != c->id2) {
						boost::tuple<Vector3r, bool, double, double, double> projectionPrev
						        = projection(GridNodeList[i]->pfacetList[j]->shape, state1);
						bool isintrianglePrev = boost::get<1>(projectionPrev);
						if (isintrianglePrev) {
							const shared_ptr<Interaction> intr
							        = scene->interactions->find(c->id1, GridNodeList[i]->pfacetList[j]->getId());
							if (intr
							    && intr->isReal()) { // if an interaction exist between the sphere and the previous pfacet, import parameters.
								if (isNew) {
									return false;
								} else {
									scm->isDuplicate = 1;
									scm->trueInt     = -1;
									return true;
								}
							}
						}
					}
				}
			}
			//SPhere-cylinder contact
			const State*    sphereSt2 = YADE_CAST<const State*>(&state1);
			GridConnection* gridCo    = YADE_CAST<GridConnection*>(GridList[connnum]->shape.get());
			GridNode*       gridNo1   = YADE_CAST<GridNode*>(gridCo->node1->shape.get());
			GridNode*       gridNo2   = YADE_CAST<GridNode*>(gridCo->node2->shape.get());
			State*          gridNo1St = YADE_CAST<State*>(gridCo->node1->state.get());
			State*          gridNo2St = YADE_CAST<State*>(gridCo->node2->state.get());

			Vector3r segt = gridCo->getSegment();
			Real     len  = gridCo->getLength();

			Vector3r spherePos = sphereSt2->pos;
			Vector3r branch    = spherePos - gridNo1St->pos;
			Vector3r branchN   = spherePos - gridNo2St->pos;
			for (int i = 0; i < 3; i++) {
				if (math::abs(branch[i]) < 1e-14) branch[i] = 0.0;
				if (math::abs(branchN[i]) < 1e-14) branchN[i] = 0.0;
			}
			Real relPos = branch.dot(segt) / (len * len);
			bool SGr    = true;
			if (relPos <= 0) {                          // if the sphere projection is BEFORE the segment ...
				if (gridNo1->ConnList.size() > 1) { //	if the node is not an extremity of the Grid (only one connection)
					for (int unsigned i = 0; i < gridNo1->ConnList.size(); i++) { // ... loop on all the Connections of the same Node ...
						GridConnection* GC = (GridConnection*)gridNo1->ConnList[i]->shape.get();
						if (GC == gridCo) continue; //	self comparison.
						Vector3r segtCandidate1 = GC->node1->state->pos
						        - gridNo1St->pos; // (be sure of the direction of segtPrev to compare relPosPrev.)
						Vector3r segtCandidate2 = GC->node2->state->pos - gridNo1St->pos;
						Vector3r segtPrev       = segtCandidate1.norm() > segtCandidate2.norm() ? segtCandidate1 : segtCandidate2;
						for (int j = 0; j < 3; j++) {
							if (math::abs(segtPrev[j]) < 1e-14) segtPrev[j] = 0.0;
						}
						Real relPosPrev = (branch.dot(segtPrev)) / (segtPrev.norm() * segtPrev.norm());
						// ... and check whether the sphere projection is before the neighbours connections too.
						if (relPosPrev
						    <= 0) { //if the sphere projection is outside both the current Connection AND this neighbouring connection, then create the interaction if the neighbour did not already do it before.
							for (int unsigned j = 0; j < GC->pfacetList.size(); j++) {
								if (GC->pfacetList[j]->getId() != c->id2) {
									const shared_ptr<Interaction> intr
									        = scene->interactions->find(c->id1, GC->pfacetList[j]->getId());

									if (intr && intr->isReal()) {
										shared_ptr<ScGridCoGeom> intrGeom = YADE_PTR_CAST<ScGridCoGeom>(intr->geom);
										if (!(intrGeom->isDuplicate == 1)) { //skip contact.
											if (isNew) {
												SGr = false;
											} else {
												scm->isDuplicate = 1;
											} /*cout<<"Declare "<<c->id1<<"-"<<c->id2<<" as duplicated."<<endl;*/
										}
									}
								}
							}
						} else { //the sphere projection is outside the current Connection but inside the previous neighbour. The contact has to be handled by the Prev GridConnection, not here.
							if (isNew) {
								SGr = false;
							} else {
								//cout<<"The contact "<<c->id1<<"-"<<c->id2<<" HAVE to be copied and deleted NOW."<<endl ;
								scm->isDuplicate = 1;
								scm->trueInt     = -1; //trueInt id de l'objet avec lequel il y a un contact -1 = rien faire
								return true;
							}
						}
					}
				}
			}

			//Exactly the same but in the case the sphere projection is AFTER the segment.
			else if (relPos >= 1) {
				if (gridNo2->ConnList.size() > 1) {
					for (int unsigned i = 0; i < gridNo2->ConnList.size(); i++) {
						GridConnection* GC = (GridConnection*)gridNo2->ConnList[i]->shape.get();
						if (GC == gridCo) continue; //	self comparison.
						Vector3r segtCandidate1 = GC->node1->state->pos - gridNo2St->pos;
						Vector3r segtCandidate2 = GC->node2->state->pos - gridNo2St->pos;
						Vector3r segtNext       = segtCandidate1.norm() > segtCandidate2.norm() ? segtCandidate1 : segtCandidate2;
						for (int j = 0; j < 3; j++) {
							if (math::abs(segtNext[j]) < 1e-14) segtNext[j] = 0.0;
						}

						Real relPosNext = (branchN.dot(segtNext)) / (segtNext.norm() * segtNext.norm());
						if (relPosNext
						    <= 0) { //if the sphere projection is outside both the current Connection AND this neighbouring connection, then create the interaction if the neighbour did not already do it before.
							for (int unsigned j = 0; j < GC->pfacetList.size(); j++) {
								if (GC->pfacetList[j]->getId() != c->id2) {
									const shared_ptr<Interaction> intr
									        = scene->interactions->find(c->id1, GC->pfacetList[j]->getId());
									if (intr && intr->isReal()) {
										shared_ptr<ScGridCoGeom> intrGeom = YADE_PTR_CAST<ScGridCoGeom>(intr->geom);
										if (!(intrGeom->isDuplicate == 1)) {
											if (isNew) SGr = false;
											else
												scm->isDuplicate
												        = 1; /*cout<<"Declare "<<c->id1<<"-"<<c->id2<<" as duplicated."<<endl;*/
										}
									}
								}
							}
						} else { //the sphere projection is outside the current Connection but inside the previous neighbour. The contact has to be handled by the Prev GridConnection, not here.
							if (isNew) SGr = false;
							else { //cout<<"The contact "<<c->id1<<"-"<<c->id2<<" HAVE to be copied and deleted NOW."<<endl ;
								scm->isDuplicate = 1;
								scm->trueInt     = -1;
								return true;
							}
						}
					}
				}
			} else if (relPos <= 0.5) {
				if (gridNo1->ConnList.size() > 1) { //	if the node is not an extremity of the Grid (only one connection)
					for (int unsigned i = 0; i < gridNo1->ConnList.size(); i++) { // ... loop on all the Connections of the same Node ...
						GridConnection* GC = (GridConnection*)gridNo1->ConnList[i]->shape.get();
						if (GC == gridCo) continue; //	self comparison.

						Vector3r segtCandidate1 = GC->node1->state->pos
						        - gridNo1St->pos; // (be sure of the direction of segtPrev to compare relPosPrev.)
						Vector3r segtCandidate2 = GC->node2->state->pos - gridNo1St->pos;
						Vector3r segtPrev       = segtCandidate1.norm() > segtCandidate2.norm() ? segtCandidate1 : segtCandidate2;
						for (int j = 0; j < 3; j++) {
							if (math::abs(segtPrev[j]) < 1e-14) segtPrev[j] = 0.0;
						}
						Real relPosPrev = (branch.dot(segtPrev)) / (segtPrev.norm() * segtPrev.norm());
						if (relPosPrev
						    <= 0) { //the sphere projection is inside the current Connection and outide this neighbour connection.
							for (int unsigned j = 0; j < GC->pfacetList.size(); j++) {
								if (GC->pfacetList[j]->getId() != c->id2) {
									const shared_ptr<Interaction> intr
									        = scene->interactions->find(c->id1, GC->pfacetList[j]->getId());
									if (intr
									    && intr->isReal()) { // if an ineraction exist between the sphere and the previous connection, import parameters.
										// 									cout<<"1Copying contact geom and phys from "<<intr->id1<<"-"<<intr->id2<<" to here ("<<c->id1<<"-"<<c->id2<<")"<<endl;
										scm = YADE_PTR_CAST<ScGridCoGeom>(intr->geom);
										if (isNew) {
											c->geom         = scm;
											c->phys         = intr->phys;
											c->iterMadeReal = intr->iterMadeReal;
										}
										scm->trueInt     = c->id2;
										scm->isDuplicate = 2; //command the old contact deletion.
										isNew            = 0;
										break;
									}
								}
							}
						}
					}
				}

			}

			else if (relPos > 0.5) {
				if (gridNo2->ConnList.size() > 1) {
					for (int unsigned i = 0; i < gridNo2->ConnList.size(); i++) {
						GridConnection* GC = (GridConnection*)gridNo2->ConnList[i]->shape.get();
						if (GC == gridCo) continue; //	self comparison.
						Vector3r segtCandidate1 = GC->node1->state->pos - gridNo2St->pos;
						Vector3r segtCandidate2 = GC->node2->state->pos - gridNo2St->pos;
						Vector3r segtNext       = segtCandidate1.norm() > segtCandidate2.norm() ? segtCandidate1 : segtCandidate2;
						for (int j = 0; j < 3; j++) {
							if (math::abs(segtNext[j]) < 1e-14) segtNext[j] = 0.0;
						}
						Real relPosNext = (branchN.dot(segtNext)) / (segtNext.norm() * segtNext.norm());
						if (relPosNext
						    <= 0) { //the sphere projection is inside the current Connection and outside this neighbour connection.
							for (int unsigned j = 0; j < GC->pfacetList.size(); j++) {
								if (GC->pfacetList[j]->getId() != c->id2) {
									const shared_ptr<Interaction> intr
									        = scene->interactions->find(c->id1, GC->pfacetList[j]->getId());
									if (intr
									    && intr->isReal()) { // if an ineraction exist between the sphere and the previous connection, import parameters.
										scm = YADE_PTR_CAST<ScGridCoGeom>(intr->geom);
										if (isNew) {
											c->geom         = scm;
											c->phys         = intr->phys;
											c->iterMadeReal = intr->iterMadeReal;
										}
										scm->trueInt     = c->id2;
										scm->isDuplicate = 2; //command the old contact deletion.
										break;
									}
								}
							}
						}
					}
				}
			}
			if (SGr) {
				relPos               = relPos < 0 ? 0 : relPos; //min value of relPos : 0
				relPos               = relPos > 1 ? 1 : relPos; //max value of relPos : 1
				Vector3r fictiousPos = gridNo1St->pos + relPos * segt;
				Vector3r branchF     = fictiousPos - spherePos;

				Real dist2 = branchF.norm();
				bool SG    = !(isNew && (dist2 > (sphere->radius + gridCo->radius)));
				if (SG) {
					//	Create the geometry :
					if (isNew) c->geom = scm;
					scm->radius1 = sphere->radius;
					scm->radius2 = gridCo->radius;
					scm->id3     = gridCo->node1->getId();
					scm->id4     = gridCo->node2->getId();

					scm->relPos               = relPos;
					Vector3r normal2          = branchF / dist2;
					scm->penetrationDepth     = sphere->radius + gridCo->radius - dist2;
					scm->fictiousState.pos    = fictiousPos;
					scm->contactPoint         = spherePos + normal2 * (scm->radius1 - 0.5 * scm->penetrationDepth);
					scm->fictiousState.vel    = (1 - relPos) * gridNo1St->vel + relPos * gridNo2St->vel;
					scm->fictiousState.angVel = ((1 - relPos) * gridNo1St->angVel + relPos * gridNo2St->angVel).dot(segt / len) * segt
					                / len                                  //twist part : interpolated
					        + segt.cross(gridNo2St->vel - gridNo1St->vel); // non-twist part : defined from nodes velocities
					scm->precompute(
					        state1,
					        scm->fictiousState,
					        scene,
					        c,
					        normal2,
					        isNew,
					        shift2,
					        true); //use sphere-sphere precompute (with a virtual sphere)
					return true;
				}
			}
		} else { //taking into account the shadow zone
			for (int unsigned i = 0; i < 3; i++) {
				for (int unsigned j = 0; j < GridNodeList[i]->pfacetList.size(); j++) {
					if (GridNodeList[i]->pfacetList[j]->getId() != c->id2) {
						boost::tuple<Vector3r, bool, double, double, double> projectionPrev
						        = projection(GridNodeList[i]->pfacetList[j]->shape, state1);
						bool isintrianglePrev = boost::get<1>(projectionPrev);
						if (!isintrianglePrev) {
							//if the sphere projection is outside both the current PFacet AND this neighbouring PFacet, then create the interaction if the neighbour did not already do it before.
							const shared_ptr<Interaction> intr
							        = scene->interactions->find(c->id1, GridNodeList[i]->pfacetList[j]->getId());
							if (intr && intr->isReal()) {
								shared_ptr<ScGridCoGeom> intrGeom = YADE_PTR_CAST<ScGridCoGeom>(intr->geom);
								if (!(intrGeom->isDuplicate == 1)) { //skip contact.
									if (isNew) {
										return false;
									} else {
										scm->isDuplicate = 1;
									} /*cout<<"Declare "<<c->id1<<"-"<<c->id2<<" as duplicated."<<endl;*/
								}
							}
						} else { //the sphere projection is outside the current PFacet but inside the previous neighbour. The contact has to be handled by the Prev GridConnection, not here.
							if (isNew) {
								return false;
							} else {
								// 						cout<<"The contact "<<c->id1<<"-"<<c->id2<<" HAVE to be copied and deleted NOW."<<endl ;
								scm->isDuplicate = 1;  //scm->isDuplicate=1;
								scm->trueInt     = -1; //trueInt id de l'objet avec lequel il y a un contact -1 = rien faire
								return true;
							}
						}
					}
				}
			}
		}
	}
	if (isintriangle) {
		penetrationDepth = sphereRadius + PFacetradius - math::abs(dist);
		normal.normalize();
		if (penetrationDepth > 0 || c->isReal()) {
			if (isNew) c->geom = scm;
			scm->radius1              = sphereRadius;
			scm->radius2              = PFacetradius;
			scm->id3                  = Pfacet->node1->getId();
			scm->id4                  = Pfacet->node2->getId();
			scm->id5                  = Pfacet->node3->getId();
			scm->weight[0]            = p1;
			scm->weight[1]            = p2;
			scm->weight[2]            = p3;
			scm->penetrationDepth     = penetrationDepth;
			scm->fictiousState.pos    = P;
			scm->contactPoint         = centerS - (PFacetradius - 0.5 * penetrationDepth) * normal;
			scm->fictiousState.vel    = (p1 * Pfacet->node1->state->vel + p2 * Pfacet->node2->state->vel + p3 * Pfacet->node3->state->vel);
			scm->fictiousState.angVel = (p1 * Pfacet->node1->state->angVel + p2 * Pfacet->node2->state->angVel + p3 * Pfacet->node3->state->angVel);
			scm->precompute(
			        state1, scm->fictiousState, scene, c, -normal, isNew, shift2, true); //use sphere-sphere precompute (with a virtual sphere)
			TIMING_DELTAS_CHECKPOINT("Ig2_Sphere_PFacet_ScGridCoGeom");
			return true;
		}
	}
	TIMING_DELTAS_CHECKPOINT("Ig2_Sphere_PFacet_ScGridCoGeom");
	return false;
}
bool Ig2_Sphere_PFacet_ScGridCoGeom::goReverse(
        const shared_ptr<Shape>&       cm1,
        const shared_ptr<Shape>&       cm2,
        const State&                   state1,
        const State&                   state2,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& c)
{
	c->swapOrder();
	return go(cm2, cm1, state2, state1, -shift2, force, c);
}
YADE_PLUGIN((Ig2_Sphere_PFacet_ScGridCoGeom));

bool Ig2_GridConnection_PFacet_ScGeom::go(
        const shared_ptr<Shape>& cm1,
        const shared_ptr<Shape>& cm2,
        const State& /*state1*/,
        const State& /*state2*/,
        const Vector3r& /*shift2*/,
        const bool& /*force*/,
        const shared_ptr<Interaction>& c)
{
	GridConnection* gridCo = YADE_CAST<GridConnection*>(cm1.get());
	PFacet*         Pfacet = YADE_CAST<PFacet*>(cm2.get());

	if (gridCo->node1 == Pfacet->node1 || gridCo->node1 == Pfacet->node2 || gridCo->node1 == Pfacet->node3 || gridCo->node2 == Pfacet->node1
	    || gridCo->node2 == Pfacet->node2 || gridCo->node2 == Pfacet->node3) {
		return false;
	}

	Body::id_t idNode1 = gridCo->node1->getId();
	Body::id_t idNode2 = gridCo->node2->getId();
	Body::id_t ids2[3] = { Pfacet->conn1->getId(), Pfacet->conn2->getId(), Pfacet->conn3->getId() };
	Body::id_t id1     = c->id1;
	Body::id_t id2     = c->id2;
	if (!scene->interactions->found(idNode1, id2)) {
		shared_ptr<Interaction> scm1(new Interaction(idNode1, id2));
		scene->interactions->insert(scm1);
	}

	if (!scene->interactions->found(idNode2, id2)) {
		shared_ptr<Interaction> scm2(new Interaction(idNode2, id2));
		scene->interactions->insert(scm2);
	}

	for (int i = 0; i < 3; i++) {
		int                entier = i;
		std::ostringstream oss;
		string             chaine = "scm";
		oss << chaine << entier;
		string chaine1 = oss.str();
		if (!scene->interactions->found(id1, ids2[i])) {
			shared_ptr<Interaction> chain1(new Interaction(id1, ids2[i]));
			scene->interactions->insert(chain1);
		}
	}

	return (false);
}

bool Ig2_GridConnection_PFacet_ScGeom::goReverse(
        const shared_ptr<Shape>&       cm1,
        const shared_ptr<Shape>&       cm2,
        const State&                   state1,
        const State&                   state2,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& c)
{
	return go(cm1, cm2, state2, state1, -shift2, force, c);
}
YADE_PLUGIN((Ig2_GridConnection_PFacet_ScGeom));


bool Ig2_PFacet_PFacet_ScGeom::go(
        const shared_ptr<Shape>& cm1,
        const shared_ptr<Shape>& cm2,
        const State& /*state1*/,
        const State& /*state2*/,
        const Vector3r& /*shift2*/,
        const bool& /*force*/,
        const shared_ptr<Interaction>& c)
{
	PFacet*    Pfacet1 = YADE_CAST<PFacet*>(cm1.get());
	PFacet*    Pfacet2 = YADE_CAST<PFacet*>(cm2.get());
	Body::id_t id1     = c->id1;
	Body::id_t id2     = c->id2;

	if (Pfacet1->node1 == Pfacet2->node1 || Pfacet1->node1 == Pfacet2->node2 || Pfacet1->node1 == Pfacet2->node3 || Pfacet1->node2 == Pfacet2->node1
	    || Pfacet1->node2 == Pfacet2->node2 || Pfacet1->node2 == Pfacet2->node3 || Pfacet1->node3 == Pfacet2->node1 || Pfacet1->node3 == Pfacet2->node2
	    || Pfacet1->node3 == Pfacet2->node3) {
		return false;
	}


	boost::tuple<Vector3r, bool, double, double, double> p1 = Ig2_Sphere_PFacet_ScGridCoGeom::projection(cm1, *Pfacet2->node1->state);
	boost::tuple<Vector3r, bool, double, double, double> p2 = Ig2_Sphere_PFacet_ScGridCoGeom::projection(cm1, *Pfacet2->node2->state);
	boost::tuple<Vector3r, bool, double, double, double> p3 = Ig2_Sphere_PFacet_ScGridCoGeom::projection(cm1, *Pfacet2->node3->state);

	boost::tuple<Vector3r, bool, double, double, double> p4 = Ig2_Sphere_PFacet_ScGridCoGeom::projection(cm2, *Pfacet1->node1->state);
	boost::tuple<Vector3r, bool, double, double, double> p5 = Ig2_Sphere_PFacet_ScGridCoGeom::projection(cm2, *Pfacet1->node2->state);
	boost::tuple<Vector3r, bool, double, double, double> p6 = Ig2_Sphere_PFacet_ScGridCoGeom::projection(cm2, *Pfacet1->node3->state);

	bool isintriangle1 = boost::get<1>(p1);
	bool isintriangle2 = boost::get<1>(p2);
	bool isintriangle3 = boost::get<1>(p3);


	bool isintriangle4 = boost::get<1>(p4);
	bool isintriangle5 = boost::get<1>(p5);
	bool isintriangle6 = boost::get<1>(p6);


	bool istriangleNodes2P1 = (isintriangle1 == true) && (isintriangle2 == true) && (isintriangle3 == true);
	bool istriangleNodes1P2 = (isintriangle4 == true) && (isintriangle5 == true) && (isintriangle6 == true);

	if (istriangleNodes1P2) {
		Body::id_t idNode11 = Pfacet1->node1->getId();
		Body::id_t idNode12 = Pfacet1->node2->getId();
		Body::id_t idNode13 = Pfacet1->node3->getId();

		// FIXME: if projection of some nodes is in triangle but not in contact, do we need to create interaxctions here?
		if (!scene->interactions->found(idNode11, id2)) {
			shared_ptr<Interaction> scm1(new Interaction(idNode11, id2));
			scene->interactions->insert(scm1);
		}

		if (!scene->interactions->found(idNode12, id2)) {
			shared_ptr<Interaction> scm2(new Interaction(idNode12, id2));
			scene->interactions->insert(scm2);
		}

		if (!scene->interactions->found(idNode13, id2)) {
			shared_ptr<Interaction> scm3(new Interaction(idNode13, id2));
			scene->interactions->insert(scm3);
		}
	} else if (istriangleNodes2P1) {
		Body::id_t idNode21 = Pfacet2->node1->getId();
		Body::id_t idNode22 = Pfacet2->node2->getId();
		Body::id_t idNode23 = Pfacet2->node3->getId();


		if (!scene->interactions->found(idNode21, id1)) {
			shared_ptr<Interaction> scm1(new Interaction(idNode21, id1));
			scene->interactions->insert(scm1);
		}

		if (!scene->interactions->found(idNode22, id1)) {
			shared_ptr<Interaction> scm2(new Interaction(idNode22, id1));
			scene->interactions->insert(scm2);
		}

		if (!scene->interactions->found(idNode23, id1)) {
			shared_ptr<Interaction> scm3(new Interaction(idNode23, id1));
			scene->interactions->insert(scm3);
		}
	} else {
		Body::id_t ids1[3] = { Pfacet1->conn1->getId(), Pfacet1->conn2->getId(), Pfacet1->conn3->getId() };
		Body::id_t ids2[3] = { Pfacet2->conn1->getId(), Pfacet2->conn2->getId(), Pfacet2->conn3->getId() };

		if ((isintriangle1 == false) && (isintriangle2 == false) && (isintriangle3 == false) && (isintriangle4 == false) && (isintriangle5 == false)
		    && (isintriangle6 == false)) {
			for (int i = 0; i < 3; i++) {
				for (int j = 0; j < 3; j++) {
					if (!scene->interactions->found(ids1[i], ids2[j])) {
						if ((Body::byId(ids1[i])->getGroupMask() != 0) and (Body::byId(ids2[j])->getGroupMask() != 0)) {
							shared_ptr<Interaction> chain1(new Interaction(ids1[i], ids2[j]));
							scene->interactions->insert(chain1);
						}
					}
				}
			}
		} else {
			Body::id_t idNode11 = Pfacet1->node1->getId();
			Body::id_t idNode12 = Pfacet1->node2->getId();
			Body::id_t idNode13 = Pfacet1->node3->getId();


			if (!scene->interactions->found(idNode11, id2)) {
				shared_ptr<Interaction> scm1(new Interaction(idNode11, id2));
				scene->interactions->insert(scm1);
			}

			if (!scene->interactions->found(idNode12, id2)) {
				shared_ptr<Interaction> scm2(new Interaction(idNode12, id2));
				scene->interactions->insert(scm2);
			}

			if (!scene->interactions->found(idNode13, id2)) {
				shared_ptr<Interaction> scm3(new Interaction(idNode13, id2));
				scene->interactions->insert(scm3);
			}

			Body::id_t idNode21 = Pfacet2->node1->getId();
			Body::id_t idNode22 = Pfacet2->node2->getId();
			Body::id_t idNode23 = Pfacet2->node3->getId();


			if (!scene->interactions->found(idNode21, id1)) {
				shared_ptr<Interaction> scm1(new Interaction(idNode21, id1));
				scene->interactions->insert(scm1);
			}

			if (!scene->interactions->found(idNode22, id1)) {
				shared_ptr<Interaction> scm2(new Interaction(idNode22, id1));
				scene->interactions->insert(scm2);
			}

			if (!scene->interactions->found(idNode23, id1)) {
				shared_ptr<Interaction> scm3(new Interaction(idNode23, id1));
				scene->interactions->insert(scm3);
			}

			Body::id_t ids1a[3] = { Pfacet1->conn1->getId(), Pfacet1->conn2->getId(), Pfacet1->conn3->getId() };
			Body::id_t ids2a[3] = { Pfacet2->conn1->getId(), Pfacet2->conn2->getId(), Pfacet2->conn3->getId() };

			for (int i = 0; i < 3; i++) {
				for (int j = 0; j < 3; j++) {
					int                entier = j + i * 3;
					std::ostringstream oss;
					string             chaine = "scm";
					oss << chaine << entier;
					string chaine1 = oss.str();
					if (!scene->interactions->found(ids1a[i], ids2a[j])) {
						if ((Body::byId(ids1a[i])->getGroupMask() != 0) and (Body::byId(ids2a[j])->getGroupMask() != 0)) {
							shared_ptr<Interaction> chain1(new Interaction(ids1a[i], ids2a[j]));
							scene->interactions->insert(chain1);
						}
					}
				}
			}
		}
	}
	return (false);
}

bool Ig2_PFacet_PFacet_ScGeom::goReverse(
        const shared_ptr<Shape>&       cm1,
        const shared_ptr<Shape>&       cm2,
        const State&                   state1,
        const State&                   state2,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& c)
{
	return go(cm1, cm2, state2, state1, -shift2, force, c);
}
YADE_PLUGIN((Ig2_PFacet_PFacet_ScGeom));


/********* Wall + Sphere **********/

bool Ig2_Wall_PFacet_ScGeom::go(
        const shared_ptr<Shape>& /*cm1*/,
        const shared_ptr<Shape>& cm2,
        const State& /*state1*/,
        const State& /*state2*/,
        const Vector3r& /*shift2*/,
        const bool& /*force*/,
        const shared_ptr<Interaction>& c)
{
	PFacet* Pfacet = YADE_CAST<PFacet*>(cm2.get());

	Body::id_t idNode1 = Pfacet->node1->getId();
	Body::id_t idNode2 = Pfacet->node2->getId();
	Body::id_t idNode3 = Pfacet->node3->getId();

	Body::id_t id1 = c->id1;

	if (!scene->interactions->found(id1, idNode1)) {
		shared_ptr<Interaction> scm1(new Interaction(id1, idNode1));
		scene->interactions->insert(scm1);
	}

	if (!scene->interactions->found(id1, idNode2)) {
		shared_ptr<Interaction> scm2(new Interaction(id1, idNode2));
		scene->interactions->insert(scm2);
	}

	if (!scene->interactions->found(id1, idNode3)) {
		shared_ptr<Interaction> scm3(new Interaction(id1, idNode3));
		scene->interactions->insert(scm3);
	}

	return (false);
}

YADE_PLUGIN((Ig2_Wall_PFacet_ScGeom));
//!##################	Bounds   #####################

void Bo1_PFacet_Aabb::go(const shared_ptr<Shape>& cm, shared_ptr<Bound>& bv, const Se3r& /*se3*/, const Body* /*b*/)
{
	PFacet* Pfacet = YADE_CAST<PFacet*>(cm.get());
	if (!bv) { bv = shared_ptr<Bound>(new Aabb); }
	Aabb*    aabb = static_cast<Aabb*>(bv.get());
	Vector3r O    = Pfacet->node1->state->pos;
	Vector3r O2   = Pfacet->node2->state->pos;
	Vector3r O3   = Pfacet->node3->state->pos;

	if (!scene->isPeriodic) {
		for (int k = 0; k < 3; k++) {
			aabb->min[k] = min(min(O[k], O2[k]), O3[k]) - Pfacet->radius;
			aabb->max[k] = max(max(O[k], O2[k]), O3[k]) + Pfacet->radius;
		}
		return;
	} else {
		O  = scene->cell->unshearPt(O);
		O2 = scene->cell->unshearPt(O2);
		O3 = scene->cell->unshearPt(O3);

		Vector3r T = scene->cell->hSize * Pfacet->cellDist.cast<Real>();
		O          = O + T;
		O2         = O2 + T;
		O3         = O3 + T;
		for (int k = 0; k < 3; k++) {
			aabb->min[k] = min(min(O[k], O2[k]), O3[k]) - Pfacet->radius;
			aabb->max[k] = max(max(O[k], O2[k]), O3[k]) + Pfacet->radius;
		}
	}
}

YADE_PLUGIN((Bo1_PFacet_Aabb));

} // namespace yade