/*************************************************************************
*  Copyright (C) 2004 by Olivier Galizzi                                 *
*  olivier.galizzi@imag.fr                                               *
*  Copyright (C) 2004 by Janek Kozicki                                   *
*  cosurgi@berlios.de                                                    *
*  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 "Ig2_Box_Sphere_ScGeom.hpp"
#include <lib/base/Math.hpp>
#include <core/Scene.hpp>
#include <pkg/common/Box.hpp>
#include <pkg/common/Sphere.hpp>
#include <pkg/dem/ScGeom.hpp>

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

bool Ig2_Box_Sphere_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)
{
	const Se3r& se31 = state1.se3;
	const Se3r& se32 = state2.se3;

	bool     inside = true;
	Vector3r pt1, pt2, normal;
	Real     depth;

	Box*    obb = static_cast<Box*>(cm1.get());
	Sphere* s   = static_cast<Sphere*>(cm2.get());

	Vector3r extents = obb->extents;

	// FIXME: do we need rotation matrix? Can't quaternion do just fine?
	Matrix3r boxAxisT = se31.orientation.toRotationMatrix();
	Matrix3r boxAxis  = boxAxisT.transpose();

	Vector3r relPos21 = se32.position + shift2 - se31.position; // relative position of centroids

	// cOnBox_boxLocal is the sphere centroid (in box-local coordinates), but projected onto box if it is outside.
	// _boxLocal means that ROTATION is local and origin is in box's origin
	Vector3r cOnBox_boxLocal = boxAxis * relPos21;

	if (cOnBox_boxLocal[0] < -extents[0]) {
		cOnBox_boxLocal[0] = -extents[0];
		inside             = false;
	}
	if (cOnBox_boxLocal[0] > extents[0]) {
		cOnBox_boxLocal[0] = extents[0];
		inside             = false;
	}
	if (cOnBox_boxLocal[1] < -extents[1]) {
		cOnBox_boxLocal[1] = -extents[1];
		inside             = false;
	}
	if (cOnBox_boxLocal[1] > extents[1]) {
		cOnBox_boxLocal[1] = extents[1];
		inside             = false;
	}
	if (cOnBox_boxLocal[2] < -extents[2]) {
		cOnBox_boxLocal[2] = -extents[2];
		inside             = false;
	}
	if (cOnBox_boxLocal[2] > extents[2]) {
		cOnBox_boxLocal[2] = extents[2];
		inside             = false;
	}

	const Real sphereRadiusForBox = (hertzian) ? hertzFac * s->radius : s->radius;

	shared_ptr<ScGeom> scm;
	if (inside) {
		// sphere center inside box. find largest `cOnBox_boxLocal' value:
		// minCBoxDist_index is the coordinate index that minimizes extents[minCBoxDist_index]-math::abs(cOnBox_boxLocal[minCBoxDist_index] (sphere center closest to box boundary)
		// where cOnBox_boxLocal is minimal (i.e. where sphere center is closest perpendicularly to the box)
		Real minCBoxDist       = extents[0] - math::abs(cOnBox_boxLocal[0]);
		int  minCBoxDist_index = 0;
		for (int i = 1; i < 3; i++) {
			Real tt = extents[i] - math::abs(cOnBox_boxLocal[i]);
			if (tt < minCBoxDist) {
				minCBoxDist       = tt;
				minCBoxDist_index = i;
			}
		}

		// contact normal aligned with box edge along largest `cOnBox_boxLocal' value
		Vector3r normal_boxLocal           = Vector3r(0, 0, 0);
		normal_boxLocal[minCBoxDist_index] = (cOnBox_boxLocal[minCBoxDist_index] > 0) ? 1.0 : -1.0;

		normal = boxAxisT * normal_boxLocal;
		normal.normalize();

		// se32 is sphere's se3
		/*
		 *
		 *           _--(pt1)-_         BOX
		 *  +------~-----x-----~----------------+
		 *  |    /       ^      \               |
		 *  |   /        | (normal)*minCBoxDist |
		 *  |   |        x        |             |
		 *  |   \        | c ≡ se32->position   |
		 *  |    \       |       /              |
		 *  |      ~     |     /  SPHERE        |
		 *  |        ^~~ x ~~^                  |
		 *  |              (pt2)                |
		 *  +-----------------------------------+
		 *
		 *
		 */
		pt1              = se32.position + normal * minCBoxDist;
		pt2              = se32.position - normal * s->radius;
		Vector3r normal2 = pt1 - pt2;
		normal2.normalize();
		bool isNew = !c->geom;
		if (isNew) scm = shared_ptr<ScGeom>(new ScGeom());
		else
			scm = YADE_PTR_CAST<ScGeom>(c->geom);

		// contact point is in the middle of overlapping volumes
		//(in the direction of penetration, which is normal2 to the box surface closest to sphere center) of overlapping volumes
		scm->contactPoint = 0.5 * (pt1 + pt2);
		// 		scm->normal2 = normal2;
		scm->penetrationDepth = (pt1 - pt2).norm();
		scm->radius1          = sphereRadiusForBox;
		scm->radius2          = s->radius;
		c->geom               = scm;
		scm->precompute(state1, state2, scene, c, normal2, isNew, shift2, true);
	} else { // outside
		Vector3r cOnBox_box = boxAxisT
		        * cOnBox_boxLocal; // projection of sphere's center on closest box surface - relative to box's origin, but GLOBAL orientation!
		Vector3r cOnBox_sphere = cOnBox_box - relPos21; // same, but origin in sphere's center
		depth                  = s->radius - cOnBox_sphere.norm();
		if (-depth > s->radius * (interactionDetectionFactor - 1.) && !c->isReal() && !force) return false;

		/*
		 *  +-----------------------------------+
		 *  |                                   |
		 *  |          se31->position           |
		 *  |         pt2   ×                   |
		 *  |            × / cOnBox_box         |
		 *  |         pt1 /                     |
		 *  +------~-----×-----~----------------+
		 *       /       ^       \
		 *      /        | cOnBox_sphere
		 *      |        ×        |
		 *      \           c ≡ se32->position
		 *       \               /
		 *         ~           /
		 *           ^~~ ~ ~~^
		 *
		 */

		pt1 = cOnBox_box + se31.position;

		cOnBox_sphere.normalize(); // we want only direction in the following

		pt2 = se32.position + shift2 + cOnBox_sphere * s->radius;

		bool isNew = !c->geom;
		if (isNew) scm = shared_ptr<ScGeom>(new ScGeom());
		else
			scm = YADE_PTR_CAST<ScGeom>(c->geom);
		scm->contactPoint     = 0.5 * (pt1 + pt2);
		scm->penetrationDepth = depth;
		scm->radius1          = sphereRadiusForBox;
		scm->radius2          = s->radius;
		c->geom               = scm;
		//FIXME : NOT TESTED : do we precompute correctly if boxes are moving?
		scm->precompute(state1, state2, scene, c, -cOnBox_sphere, isNew, shift2, false);
	}
	return true;
}


bool Ig2_Box_Sphere_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)
{
	c->swapOrder();
	return go(cm2, cm1, state2, state1, -shift2, force, c);
}

YADE_PLUGIN((Ig2_Box_Sphere_ScGeom));


bool Ig2_Box_Sphere_ScGeom6D::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)
{
	bool isNew = !c->geom;
	if (Ig2_Box_Sphere_ScGeom::go(cm1, cm2, state1, state2, shift2, force, c)) {
		if (isNew) { //generate a 6DOF interaction from the 3DOF one generated by Ig2_Box_Sphere_ScGeom
			shared_ptr<ScGeom6D> sc(new ScGeom6D());
			*(YADE_PTR_CAST<ScGeom>(sc)) = *(YADE_PTR_CAST<ScGeom>(c->geom));
			c->geom                      = sc;
		}
		YADE_PTR_CAST<ScGeom6D>(c->geom)->precomputeRotations(state1, state2, isNew, false);
		return true;
	} else
		return false;
}


bool Ig2_Box_Sphere_ScGeom6D::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_Box_Sphere_ScGeom6D));

} // namespace yade
