/** @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)
 */
// Libraries
#include <ephysics/collision/shapes/BoxShape.hpp>
#include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/configuration.hpp>
#include <etk/Vector.hpp>

using namespace ephysics;

BoxShape::BoxShape( Vector3f extent, float margin):
  ConvexShape(BOX, margin),
  this.extent(extent - Vector3f(margin, margin, margin)) {
	assert(extent.x() > 0.0f && extent.x() > margin);
	assert(extent.y() > 0.0f && extent.y() > margin);
	assert(extent.z() > 0.0f && extent.z() > margin);
}

void BoxShape::computeLocalInertiaTensor(Matrix3f tensor, float mass)  {
	float factor = (1.0f / float(3.0)) * mass;
	Vector3f realExtent = this.extent + Vector3f(this.margin, this.margin, this.margin);
	float xSquare = realExtent.x() * realExtent.x();
	float ySquare = realExtent.y() * realExtent.y();
	float zSquare = realExtent.z() * realExtent.z();
	tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0,
	                 0.0, factor * (xSquare + zSquare), 0.0,
	                 0.0, 0.0, factor * (xSquare + ySquare));
}

boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
	Vector3f rayDirection = ray.point2 - ray.point1;
	float tMin = FLTMIN;
	float tMax = FLTMAX;
	Vector3f normalDirection(0,0,0);
	Vector3f currentNormal(0,0,0);
	// For each of the three slabs
	for (int iii=0; iii<3; ++iii) {
		// If ray is parallel to the slab
		if (abs(rayDirection[iii]) < FLTEPSILON) {
			// If the ray's origin is not inside the slab, there is no hit
			if (ray.point1[iii] > this.extent[iii] || ray.point1[iii] < -this.extent[iii]) {
				return false;
			}
		} else {
			// Compute the intersection of the ray with the near and far plane of the slab
			float oneOverD = 1.0f / rayDirection[iii];
			float t1 = (-this.extent[iii] - ray.point1[iii]) * oneOverD;
			float t2 = (this.extent[iii] - ray.point1[iii]) * oneOverD;
			currentNormal[0] = (iii == 0) ? -this.extent[iii] : 0.0f;
			currentNormal[1] = (iii == 1) ? -this.extent[iii] : 0.0f;
			currentNormal[2] = (iii == 2) ? -this.extent[iii] : 0.0f;
			// Swap t1 and t2 if need so that t1 is intersection with near plane and
			// t2 with far plane
			if (t1 > t2) {
				swap(t1, t2);
				currentNormal = -currentNormal;
			}
			// Compute the intersection of the of slab intersection interval with previous slabs
			if (t1 > tMin) {
				tMin = t1;
				normalDirection = currentNormal;
			}
			tMax = min(tMax, t2);
			// If tMin is larger than the maximum raycasting fraction, we return no hit
			if (tMin > ray.maxFraction) {
				return false;
			}
			// If the slabs intersection is empty, there is no hit
			if (tMin > tMax) {
				return false;
			}
		}
	}
	// If tMin is negative, we return no hit
	if (    tMin < 0.0f
	     || tMin > ray.maxFraction) {
		return false;
	}
	if (normalDirection == Vector3f(0,0,0)) {
		return false;
	}
	// The ray intersects the three slabs, we compute the hit point
	Vector3f localHitPoint = ray.point1 + tMin * rayDirection;
	raycastInfo.body = proxyShape.getBody();
	raycastInfo.proxyShape = proxyShape;
	raycastInfo.hitFraction = tMin;
	raycastInfo.worldPoint = localHitPoint;
	raycastInfo.worldNormal = normalDirection;
	return true;
}

Vector3f BoxShape::getExtent()  {
	return this.extent + Vector3f(this.margin, this.margin, this.margin);
}

void BoxShape::setLocalScaling( Vector3f scaling) {
	this.extent = (this.extent / this.scaling) * scaling;
	CollisionShape::setLocalScaling(scaling);
}

void BoxShape::getLocalBounds(Vector3f min, Vector3f max)  {
	// Maximum bounds
	max = this.extent + Vector3f(this.margin, this.margin, this.margin);
	// Minimum bounds
	min = -max;
}

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

Vector3f BoxShape::getLocalSupportPointWithoutMargin( Vector3f direction,
                                                 void** cachedCollisionData)  {
	return Vector3f(direction.x() < 0.0 ? -this.extent.x() : this.extent.x(),
	            direction.y() < 0.0 ? -this.extent.y() : this.extent.y(),
	            direction.z() < 0.0 ? -this.extent.z() : this.extent.z());
}

boolean BoxShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape)  {
	return (    localPoint.x() < this.extent[0]
	         && localPoint.x() > -this.extent[0]
	         && localPoint.y() < this.extent[1]
	         && localPoint.y() > -this.extent[1]
	         && localPoint.z() < this.extent[2]
	         && localPoint.z() > -this.extent[2]);
}