/** @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/CylinderShape.hpp>
#include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/configuration.hpp>

using namespace ephysics;

CylinderShape::CylinderShape(float radius,
                             float height,
                             float margin):
  ConvexShape(CYLINDER, margin), this.radius(radius), this.halfHeight(height/float(2.0)) {
	assert(radius > 0.0f);
	assert(height > 0.0f);
}

vec3 CylinderShape::getLocalSupportPointWithoutMargin( vec3 direction,
                                                      void** cachedCollisionData)  {
	vec3 supportPoint(0.0, 0.0, 0.0);
	float uDotv = direction.y();
	vec3 w(direction.x(), 0.0, direction.z());
	float lengthW = sqrt(direction.x() * direction.x() + direction.z() * direction.z());
	if (lengthW > FLTEPSILON) {
		if (uDotv < 0.0) {
			supportPoint.setY(-this.halfHeight);
		} else {
			supportPoint.setY(this.halfHeight);
		}
		supportPoint += (this.radius / lengthW) * w;
	} else {
		if (uDotv < 0.0) {
			supportPoint.setY(-this.halfHeight);
		} else {
			supportPoint.setY(this.halfHeight);
		}
	}
	return supportPoint;
}

boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape)  {
	 vec3 n = ray.point2 - ray.point1;
	 float epsilon = float(0.01);
	vec3 p(float(0), -this.halfHeight, float(0));
	vec3 q(float(0), this.halfHeight, float(0));
	vec3 d = q - p;
	vec3 m = ray.point1 - p;
	float t;
	float mDotD = m.dot(d);
	float nDotD = n.dot(d);
	float dDotD = d.dot(d);
	// Test if the segment is outside the cylinder
	if (mDotD < 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj mDotD + nDotD < float(0.0)) {
		return false;
	}
	if (mDotD > dDotD hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj mDotD + nDotD > dDotD) {
		return false;
	}
	float nDotN = n.dot(n);
	float mDotN = m.dot(n);
	float a = dDotD * nDotN - nDotD * nDotD;
	float k = m.dot(m) - this.radius * this.radius;
	float c = dDotD * k - mDotD * mDotD;
	// If the ray is parallel to the cylinder axis
	if (etk::abs(a) < epsilon) {
		// If the origin is outside the surface of the cylinder, we return no hit
		if (c > 0.0f) {
			return false;
		}
		// Here we know that the segment intersect an endcap of the cylinder
		// If the ray intersects with the "p" endcap of the cylinder
		if (mDotD < 0.0f) {
			t = -mDotN / nDotN;
			// If the intersection is behind the origin of the ray or beyond the maximum
			// raycasting distance, we return no hit
			if (t < 0.0f || t > ray.maxFraction) {
				return false;
			}
			// Compute the hit information
			vec3 localHitPoint = ray.point1 + t * n;
			raycastInfo.body = proxyShape->getBody();
			raycastInfo.proxyShape = proxyShape;
			raycastInfo.hitFraction = t;
			raycastInfo.worldPoint = localHitPoint;
			vec3 normalDirection(0, float(-1), 0);
			raycastInfo.worldNormal = normalDirection;
			return true;
		}
		// If the ray intersects with the "q" endcap of the cylinder
		if (mDotD > dDotD) {
			t = (nDotD - mDotN) / nDotN;
			// If the intersection is behind the origin of the ray or beyond the maximum
			// raycasting distance, we return no hit
			if (t < 0.0f || t > ray.maxFraction) {
				return false;
			}
			// Compute the hit information
			vec3 localHitPoint = ray.point1 + t * n;
			raycastInfo.body = proxyShape->getBody();
			raycastInfo.proxyShape = proxyShape;
			raycastInfo.hitFraction = t;
			raycastInfo.worldPoint = localHitPoint;
			vec3 normalDirection(0, 1.0f, 0);
			raycastInfo.worldNormal = normalDirection;
			return true;
		}
		// If the origin is inside the cylinder, we return no hit
		return false;
	}
	float b = dDotD * mDotN - nDotD * mDotD;
	float discriminant = b * b - a * c;
	// If the discriminant is negative, no real roots and therfore, no hit
	if (discriminant < 0.0f) {
		return false;
	}
	// Compute the smallest root (first intersection along the ray)
	float t0 = t = (-b - etk::sqrt(discriminant)) / a;
	// If the intersection is outside the cylinder on "p" endcap side
	float value = mDotD + t * nDotD;
	if (value < 0.0f) {
		// If the ray is pointing away from the "p" endcap, we return no hit
		if (nDotD <= 0.0f) {
			return false;
		}
		// Compute the intersection against the "p" endcap (intersection agains whole plane)
		t = -mDotD / nDotD;
		// Keep the intersection if the it is inside the cylinder radius
		if (k + t * (float(2.0) * mDotN + t) > 0.0f) {
			return false;
		}
		// If the intersection is behind the origin of the ray or beyond the maximum
		// raycasting distance, we return no hit
		if (t < 0.0f || t > ray.maxFraction) {
			return false;
		}
		// Compute the hit information
		vec3 localHitPoint = ray.point1 + t * n;
		raycastInfo.body = proxyShape->getBody();
		raycastInfo.proxyShape = proxyShape;
		raycastInfo.hitFraction = t;
		raycastInfo.worldPoint = localHitPoint;
		vec3 normalDirection(0, float(-1.0), 0);
		raycastInfo.worldNormal = normalDirection;
		return true;
	}
	// If the intersection is outside the cylinder on the "q" side
	if (value > dDotD) {
		// If the ray is pointing away from the "q" endcap, we return no hit
		if (nDotD >= 0.0f) {
			return false;
		}
		// Compute the intersection against the "q" endcap (intersection against whole plane)
		t = (dDotD - mDotD) / nDotD;
		// Keep the intersection if it is inside the cylinder radius
		if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) > 0.0f) {
			return false;
		}
		// If the intersection is behind the origin of the ray or beyond the maximum
		// raycasting distance, we return no hit
		if (t < 0.0f || t > ray.maxFraction) {
			return false;
		}
		// Compute the hit information
		vec3 localHitPoint = ray.point1 + t * n;
		raycastInfo.body = proxyShape->getBody();
		raycastInfo.proxyShape = proxyShape;
		raycastInfo.hitFraction = t;
		raycastInfo.worldPoint = localHitPoint;
		vec3 normalDirection(0, 1.0f, 0);
		raycastInfo.worldNormal = normalDirection;
		return true;
	}
	t = t0;
	// If the intersection is behind the origin of the ray or beyond the maximum
	// raycasting distance, we return no hit
	if (t < 0.0f || t > ray.maxFraction) {
		return false;
	}
	// Compute the hit information
	vec3 localHitPoint = ray.point1 + t * n;
	raycastInfo.body = proxyShape->getBody();
	raycastInfo.proxyShape = proxyShape;
	raycastInfo.hitFraction = t;
	raycastInfo.worldPoint = localHitPoint;
	vec3 v = localHitPoint - p;
	vec3 w = (v.dot(d) / d.length2()) * d;
	vec3 normalDirection = (localHitPoint - (p + w));
	raycastInfo.worldNormal = normalDirection;
	return true;
}

float CylinderShape::getRadius()  {
	return this.radius;
}

float CylinderShape::getHeight()  {
	return this.halfHeight + this.halfHeight;
}

void CylinderShape::setLocalScaling( vec3 scaling) {
	this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
	this.radius = (this.radius / this.scaling.x()) * scaling.x();
	CollisionShape::setLocalScaling(scaling);
}

sizet CylinderShape::getSizeInBytes()  {
	return sizeof(CylinderShape);
}

void CylinderShape::getLocalBounds(vec3 min, vec3 max)  {
	// Maximum bounds
	max.setX(this.radius + this.margin);
	max.setY(this.halfHeight + this.margin);
	max.setZ(max.x());
	// Minimum bounds
	min.setX(-max.x());
	min.setY(-max.y());
	min.setZ(min.x());
}

void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass)  {
	float height = float(2.0) * this.halfHeight;
	float diag = (1.0f / float(12.0)) * mass * (3 * this.radius * this.radius + height * height);
	tensor.setValue(diag, 0.0, 0.0, 0.0,
	                 0.5f * mass * this.radius * this.radius, 0.0,
	                 0.0, 0.0, diag);
}

boolean CylinderShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) {
	return (    (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < this.radius * this.radius
	         hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() < this.halfHeight
	         hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.halfHeight);
}