#include "btSphericalTerrainShape.h"

#include "LinearMath/btTransformUtil.h"

btSphericalTerrainShape::btSphericalTerrainShape(const btVector3& center, btScalar radius, btVector3 (*calcVertex)(const btVector3& position, const btVector3& center))
: btConcaveShape(), m_center(center),
m_radius(radius),
m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.))
{
	m_shapeType = CUSTOM_CONCAVE_SHAPE_TYPE;
	calculateTerrainVertex = calcVertex;
}

btSphericalTerrainShape::~btSphericalTerrainShape()
{
}

void btSphericalTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
	(void)t;

	aabbMin.setValue(m_center.x()-m_radius, m_center.y()-m_radius, m_center.z()-m_radius);
	aabbMax.setValue(m_center.x()+m_radius, m_center.y()+m_radius, m_center.z()+m_radius);
}

void btSphericalTerrainShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
{
    // calculate 8 corners of the AABB
    btVector3 bottomFrontLeft = aabbMin;
    btVector3 bottomFrontRight(aabbMax.x(), aabbMin.y(), aabbMin.z());
    btVector3 bottomBackLeft(aabbMin.x(), aabbMin.y(), aabbMax.z());
    btVector3 bottomBackRight(aabbMax.x(), aabbMin.y(), aabbMax.z());
    btVector3 topBackRight = aabbMax;
    btVector3 topFrontRight(aabbMax.x(), aabbMax.y(), aabbMin.z());
    btVector3 topBackLeft(aabbMin.x(), aabbMax.y(), aabbMax.z());
    btVector3 topFrontLeft(aabbMin.x(), aabbMax.y(), aabbMin.z());

   // calculate the midpoint of each of the 6 sides of the AABB
   btVector3 halfSize = (aabbMax - aabbMin) * btScalar(0.5);
   btVector3 frontCenter(aabbMin.x() + halfSize.x(), aabbMin.y() + halfSize.y(), aabbMin.z());
   btVector3 bottomCenter(aabbMin.x() + halfSize.x(), aabbMin.y(), aabbMin.z() + halfSize.z());
   btVector3 leftCenter(aabbMin.x(), aabbMin.y() + halfSize.y(), aabbMin.z() + halfSize.z());
   btVector3 backCenter(aabbMin.x() + halfSize.x(), aabbMin.y() + halfSize.y(), aabbMax.z());
   btVector3 topCenter(aabbMin.x() + halfSize.x(), aabbMax.y(), aabbMin.z() + halfSize.z());
   btVector3 rightCenter(aabbMax.x(), aabbMin.y() + halfSize.y(), aabbMin.z() + halfSize.z());

   // calculate the position of the vertex on the terrain by "projecting" the corners of the AABB
   btVector3 bottomFrontLeftVertex = calculateTerrainVertex(bottomFrontLeft, m_center);
   btVector3 bottomFrontRightVertex = calculateTerrainVertex(bottomFrontRight, m_center);
   btVector3 bottomBackLeftVertex = calculateTerrainVertex(bottomBackLeft, m_center);
   btVector3 bottomBackRightVertex = calculateTerrainVertex(bottomBackRight, m_center);
   btVector3 topBackRightVertex = calculateTerrainVertex(topBackRight, m_center);
   btVector3 topFrontRightVertex = calculateTerrainVertex(topFrontRight, m_center);
   btVector3 topBackLeftVertex = calculateTerrainVertex(topBackLeft, m_center);
   btVector3 topFrontLeftVertex = calculateTerrainVertex(topFrontLeft, m_center);

   // determine which of the corners of the AABB are colliding with the terrain
   bool bottomFrontLeftCollides = bottomFrontLeft.distance(m_center) <= bottomFrontLeftVertex.distance(m_center);
   bool bottomFrontRightCollides = bottomFrontRight.distance(m_center) <= bottomFrontRightVertex.distance(m_center);
   bool bottomBackLeftCollides = bottomBackLeft.distance(m_center) <= bottomBackLeftVertex.distance(m_center);
   bool bottomBackRightCollides = bottomBackRight.distance(m_center) <= bottomBackRightVertex.distance(m_center);
   bool topBackRightCollides = topBackRight.distance(m_center) <= topBackRightVertex.distance(m_center);
   bool topFrontRightCollides = topFrontRight.distance(m_center) <= topFrontRightVertex.distance(m_center);
   bool topBackLeftCollides = topBackLeft.distance(m_center) <= topBackLeftVertex.distance(m_center);
   bool topFrontLeftCollides = topFrontLeft.distance(m_center) <= topFrontLeftVertex.distance(m_center);

   // find which 3 sides are closest in order to prevent extraneous triangle processing
   bool frontCloser = frontCenter.distance(m_center) < backCenter.distance(m_center);
   bool bottomCloser = bottomCenter.distance(m_center) < topCenter.distance(m_center);
   bool leftCloser = leftCenter.distance(m_center) < rightCenter.distance(m_center);

   if (frontCloser)
   {
      //create triangles for front side
      if (bottomFrontLeftCollides || bottomFrontRightCollides || topFrontRightCollides)
	  {
		  btVector3 vertices[3] = {bottomFrontLeftVertex, bottomFrontRightVertex, topFrontRightVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
      if (topFrontRightCollides || topFrontLeftCollides || bottomFrontLeftCollides)
	  {
		  btVector3 vertices[3] = {topFrontRightVertex, topFrontLeftVertex, bottomFrontLeftVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
   }
   else
   {
      //create triangles for back side
      if (bottomBackLeftCollides || bottomBackRightCollides || topBackRightCollides)
	  {
		  btVector3 vertices[3] = {bottomBackLeftVertex, bottomBackRightVertex, topBackRightVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
      if (topBackRightCollides || topBackLeftCollides || bottomBackLeftCollides)
	  {
		  btVector3 vertices[3] = {topBackRightVertex, topBackLeftVertex, bottomBackLeftVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
   }

   if (bottomCloser)
   {
      //create triangles for bottom side
      if (bottomFrontLeftCollides || bottomFrontRightCollides || bottomBackRightCollides)
	  {
		  btVector3 vertices[3] = {bottomFrontLeftVertex, bottomFrontRightVertex, bottomBackRightVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
      if (bottomBackRightCollides || bottomBackLeftCollides || bottomFrontLeftCollides)
	  {
		  btVector3 vertices[3] = {bottomBackRightVertex, bottomBackLeftVertex, bottomFrontLeftVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
   }
   else
   {
      //create triangles for top side
      if (topFrontLeftCollides || topFrontRightCollides || topBackRightCollides)
	  {
		  btVector3 vertices[3] = {topFrontLeftVertex, topFrontRightVertex, topBackRightVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
      if (topBackRightCollides || topBackLeftCollides || topFrontLeftCollides)
	  {
		  btVector3 vertices[3] = {topBackRightVertex, topBackLeftVertex, topFrontLeftVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
   }

   if (leftCloser)
   {
      //create triangles for left side
      if (bottomBackLeftCollides || bottomFrontLeftCollides || topFrontLeftCollides)
	  {
		  btVector3 vertices[3] = {bottomBackLeftVertex, bottomFrontLeftVertex, topFrontLeftVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
      if (topFrontLeftCollides || topBackLeftCollides || bottomBackLeftCollides)
	  {
		  btVector3 vertices[3] = {topFrontLeftVertex, topBackLeftVertex, bottomBackLeftVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
   }
   else
   {
      //create triangles for right side
      if (bottomBackRightCollides || bottomFrontRightCollides || topFrontRightCollides)
	  {
		  btVector3 vertices[3] = {bottomBackRightVertex, bottomFrontRightVertex, topFrontRightVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
      if (topFrontRightCollides || topBackRightCollides || bottomBackRightCollides)
	  {
		  btVector3 vertices[3] = {topFrontRightVertex, topBackRightVertex, bottomBackRightVertex};
          callback->processTriangle(vertices, 0, 0);
	  }
   }
}

void btSphericalTerrainShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{
	(void)mass;

	//moving concave objects not supported
	
	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
}

void btSphericalTerrainShape::setLocalScaling(const btVector3& scaling)
{
	m_localScaling = scaling;
}
const btVector3& btSphericalTerrainShape::getLocalScaling() const
{
	return m_localScaling;
}