/** @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/shapes/TriangleShape.hpp>
#include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/engine/Profiler.hpp>
#include <ephysics/configuration.hpp>

// TODO: REMOVE this...
using namespace ephysics;

TriangleShape::TriangleShape( Vector3f point1,  Vector3f point2,  Vector3f point3, float margin)
			  : ConvexShape(TRIANGLE, margin) {
	this.points[0] = point1;
	this.points[1] = point2;
	this.points[2] = point3;
	this.raycastTestType = FRONT;
}

boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
	PROFILE("TriangleShape::raycast()");
	 Vector3f pq = ray.point2 - ray.point1;
	 Vector3f pa = this.points[0] - ray.point1;
	 Vector3f pb = this.points[1] - ray.point1;
	 Vector3f pc = this.points[2] - ray.point1;
	// Test if the line PQ is inside the eges BC, CA and AB. We use the triple
	// product for this test.
	 Vector3f m = pq.cross(pc);
	float u = pb.dot(m);
	if (this.raycastTestType == FRONT) {
		if (u < 0.0f) {
			return false;
		}
	} else if (this.raycastTestType == BACK) {
		if (u > 0.0f) {
			return false;
		}
	}
	float v = -pa.dot(m);
	if (this.raycastTestType == FRONT) {
		if (v < 0.0f) {
			return false;
		}
	} else if (this.raycastTestType == BACK) {
		if (v > 0.0f) {
			return false;
		}
	} else if (this.raycastTestType == FRONTANDBACK) {
		if (!sameSign(u, v)) {
			return false;
		}
	}
	float w = pa.dot(pq.cross(pb));
	if (this.raycastTestType == FRONT) {
		if (w < 0.0f) {
			return false;
		}
	} else if (this.raycastTestType == BACK) {
		if (w > 0.0f) {
			return false;
		}
	} else if (this.raycastTestType == FRONTANDBACK) {
		if (!sameSign(u, w)) {
			return false;
		}
	}
	// If the line PQ is in the triangle plane (case where u=v=w=0)
	if (    approxEqual(u, 0)
	     && approxEqual(v, 0)
	     && approxEqual(w, 0)) {
		return false;
	}
	// Compute the barycentric coordinates (u, v, w) to determine the
	// intersection point R, R = u * a + v * b + w * c
	float denom = 1.0f / (u + v + w);
	u *= denom;
	v *= denom;
	w *= denom;
	// Compute the local hit point using the barycentric coordinates
	 Vector3f localHitPoint = u * this.points[0] + v * this.points[1] + w * this.points[2];
	 float hitFraction = (localHitPoint - ray.point1).length() / pq.length();
	if (    hitFraction < 0.0f
	     || hitFraction > ray.maxFraction) {
		return false;
	}
	Vector3f localHitNormal = (this.points[1] - this.points[0]).cross(this.points[2] - this.points[0]);
	if (localHitNormal.dot(pq) > 0.0f) {
		localHitNormal = -localHitNormal;
	}
	raycastInfo.body = proxyShape.getBody();
	raycastInfo.proxyShape = proxyShape;
	raycastInfo.worldPoint = localHitPoint;
	raycastInfo.hitFraction = hitFraction;
	raycastInfo.worldNormal = localHitNormal;
	return true;
}

long TriangleShape::getSizeInBytes()  {
	return sizeof(TriangleShape);
}

Vector3f TriangleShape::getLocalSupportPointWithoutMargin( Vector3f direction,
                                                      void** cachedCollisionData)  {
	Vector3f dotProducts(direction.dot(this.points[0]), direction.dot(this.points[1]), direction.dot(this.points[2]));
	return this.points[dotProducts.getMaxAxis()];
}

void TriangleShape::getLocalBounds(Vector3f min, Vector3f max)  {
	 Vector3f xAxis(this.points[0].x(), this.points[1].x(), this.points[2].x());
	 Vector3f yAxis(this.points[0].y(), this.points[1].y(), this.points[2].y());
	 Vector3f zAxis(this.points[0].z(), this.points[1].z(), this.points[2].z());
	min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin());
	max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax());
	min -= Vector3f(this.margin, this.margin, this.margin);
	max += Vector3f(this.margin, this.margin, this.margin);
}

void TriangleShape::setLocalScaling( Vector3f scaling) {
	this.points[0] = (this.points[0] / this.scaling) * scaling;
	this.points[1] = (this.points[1] / this.scaling) * scaling;
	this.points[2] = (this.points[2] / this.scaling) * scaling;
	CollisionShape::setLocalScaling(scaling);
}

void TriangleShape::computeLocalInertiaTensor(Matrix3f tensor, float mass)  {
	tensor.setZero();
}

void TriangleShape::computeAABB(AABB aabb,  Transform3D transform)  {
	 Vector3f worldPoint1 = transform * this.points[0];
	 Vector3f worldPoint2 = transform * this.points[1];
	 Vector3f worldPoint3 = transform * this.points[2];
	 Vector3f xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x());
	 Vector3f yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y());
	 Vector3f zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z());
	aabb.setMin(Vector3f(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()));
	aabb.setMax(Vector3f(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()));
}

boolean TriangleShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  {
	return false;
}

TriangleRaycastSide TriangleShape::getRaycastTestType()  {
	return this.raycastTestType;
}


void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
	this.raycastTestType = testType;
}

Vector3f TriangleShape::getVertex(int index)  {
	assert(    index >= 0
	        && index < 3);
	return this.points[index];
}