/** @file
 * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
 * @author Daniel CHAPPUIS
 * @author Edouard DUPIN
 * @copyright 2010-2016, Daniel Chappuis
 * @copyright 2017, Edouard DUPIN
 * @license MPL v2.0 (see license file)
 */
#include <ephysics/collision/ContactManifoldSet.hpp>

using namespace ephysics;

ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1,
                                       ProxyShape* shape2,
                                       int nbMaxManifolds):
  this.nbMaxManifolds(nbMaxManifolds),
  this.nbManifolds(0),
  this.shape1(shape1),
  this.shape2(shape2) {
	assert(nbMaxManifolds >= 1);
}

ContactManifoldSet::~ContactManifoldSet() {
	clear();
}

void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
	// Compute an Id corresponding to the normal direction (using a cubemap)
	int16t normalDirectionId = computeCubemapNormalId(contact.getNormal());
	// If there is no contact manifold yet
	if (this.nbManifolds == 0) {
		createManifold(normalDirectionId);
		this.manifolds[0].addContactPoint(contact);
		assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0);
		for (int i=0; i<this.nbManifolds; i++) {
			assert(this.manifolds[i].getNbContactPoints() > 0);
		}
		return;
	}
	// Select the manifold with the most similar normal (if exists)
	int similarManifoldIndex = 0;
	if (this.nbMaxManifolds > 1) {
		similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
	}
	// If a similar manifold has been found
	if (similarManifoldIndex != -1) {
		// Add the contact point to that similar manifold
		this.manifolds[similarManifoldIndex].addContactPoint(contact);
		assert(this.manifolds[similarManifoldIndex].getNbContactPoints() > 0);
		return;
	}
	// If the maximum number of manifold has not been reached yet
	if (this.nbManifolds < this.nbMaxManifolds) {
		// Create a new manifold for the contact point
		createManifold(normalDirectionId);
		this.manifolds[this.nbManifolds-1].addContactPoint(contact);
		for (int i=0; i<this.nbManifolds; i++) {
			assert(this.manifolds[i].getNbContactPoints() > 0);
		}
		return;
	}
	// The contact point will be in a new contact manifold, we now have too much
	// manifolds condidates. We need to remove one. We choose to keep the manifolds
	// with the largest contact depth among their points
	int smallestDepthIndex = -1;
	float minDepth = contact.getPenetrationDepth();
	assert(this.nbManifolds == this.nbMaxManifolds);
	for (int i=0; i<this.nbManifolds; i++) {
		float depth = this.manifolds[i].getLargestContactDepth();
		if (depth < minDepth) {
			minDepth = depth;
			smallestDepthIndex = i;
		}
	}
	// If we do not want to keep to new manifold (not created yet) with the
	// new contact point
	if (smallestDepthIndex == -1) {
		// Delete the new contact
		ETKDELETE(ContactPoint, contact);
		contact = null;
		return;
	}
	assert(smallestDepthIndex >= 0 && smallestDepthIndex < this.nbManifolds);
	// Here we need to replace an existing manifold with a new one (that contains
	// the new contact point)
	removeManifold(smallestDepthIndex);
	createManifold(normalDirectionId);
	this.manifolds[this.nbManifolds-1].addContactPoint(contact);
	assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0);
	for (int i=0; i<this.nbManifolds; i++) {
		assert(this.manifolds[i].getNbContactPoints() > 0);
	}
	return;
}

int ContactManifoldSet::selectManifoldWithSimilarNormal(int16t normalDirectionId)  {
	// Return the Id of the manifold with the same normal direction id (if exists)
	for (int i=0; i<this.nbManifolds; i++) {
		if (normalDirectionId == this.manifolds[i].getNormalDirectionId()) {
			return i;
		}
	}
	return -1;
}

int16t ContactManifoldSet::computeCubemapNormalId( Vector3f normal)  {
	assert(normal.length2() > FLTEPSILON);
	int faceNo;
	float u, v;
	float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z()));
	Vector3f normalScaled = normal / max;
	if (normalScaled.x() >= normalScaled.y() && normalScaled.x() >= normalScaled.z()) {
		faceNo = normalScaled.x() > 0 ? 0 : 1;
		u = normalScaled.y();
		v = normalScaled.z();
	} else if (normalScaled.y() >= normalScaled.x() && normalScaled.y() >= normalScaled.z()) {
		faceNo = normalScaled.y() > 0 ? 2 : 3;
		u = normalScaled.x();
		v = normalScaled.z();
	} else {
		faceNo = normalScaled.z() > 0 ? 4 : 5;
		u = normalScaled.x();
		v = normalScaled.y();
	}
	int indexU = floor(((u + 1)/2) * CONTACTCUBEMAPFACENBSUBDIVISIONS);
	int indexV = floor(((v + 1)/2) * CONTACTCUBEMAPFACENBSUBDIVISIONS);
	if (indexU == CONTACTCUBEMAPFACENBSUBDIVISIONS) {
		indexU--;
	}
	if (indexV == CONTACTCUBEMAPFACENBSUBDIVISIONS) {
		indexV--;
	}
	 int nbSubDivInFace = CONTACTCUBEMAPFACENBSUBDIVISIONS * CONTACTCUBEMAPFACENB_SUBDIVISIONS;
	return faceNo * 200 + indexU * nbSubDivInFace + indexV;
}

void ContactManifoldSet::update() {
	for (int i=this.nbManifolds-1; i>=0; i--) {
		// Update the contact manifold
		this.manifolds[i].update(this.shape1.getBody().getTransform() * this.shape1.getLocalToBodyTransform(),
							  this.shape2.getBody().getTransform() * this.shape2.getLocalToBodyTransform());
		// Remove the contact manifold if has no contact points anymore
		if (this.manifolds[i].getNbContactPoints() == 0) {
			removeManifold(i);
		}
	}
}

void ContactManifoldSet::clear() {
	for (int i=this.nbManifolds-1; i>=0; i--) {
		removeManifold(i);
	}
	assert(this.nbManifolds == 0);
}

void ContactManifoldSet::createManifold(int16t normalDirectionId) {
	assert(this.nbManifolds < this.nbMaxManifolds);
	this.manifolds[this.nbManifolds] = ETKNEW(ContactManifold, this.shape1, this.shape2, normalDirectionId);
	this.nbManifolds++;
}

void ContactManifoldSet::removeManifold(int index) {
	assert(this.nbManifolds > 0);
	assert(index >= 0 && index < this.nbManifolds);
	// Delete the new contact
	ETKDELETE(ContactManifold, this.manifolds[index]);
	this.manifolds[index] = null;
	for (int i=index; (i+1) < this.nbManifolds; i++) {
		this.manifolds[i] = this.manifolds[i+1];
	}
	this.nbManifolds--;
}

ProxyShape* ContactManifoldSet::getShape1()  {
	return this.shape1;
}

ProxyShape* ContactManifoldSet::getShape2()  {
	return this.shape2;
}

int ContactManifoldSet::getNbContactManifolds()  {
	return this.nbManifolds;
}

ContactManifold* ContactManifoldSet::getContactManifold(int index)  {
	assert(index >= 0 && index < this.nbManifolds);
	return this.manifolds[index];
}

int ContactManifoldSet::getTotalNbContactPoints()  {
	int nbPoints = 0;
	for (int i=0; i<this.nbManifolds; i++) {
		nbPoints += this.manifolds[i].getNbContactPoints();
	}
	return nbPoints;
}