//-----------------------------------------------------------------------------
// Torque Game Engine
// Copyright (C) GarageGames.com, Inc.
//-----------------------------------------------------------------------------

#include "dgl/dgl.h"
#include "core/dataChunker.h"
#include "collision/collision.h"
#include "sceneGraph/sceneGraph.h"
#include "sim/sceneObject.h"
#include "terrain/terrData.h"
#include "collision/convex.h"
#include "collision/gjk.h"

//----------------------------------------------------------------------------
//----------------------------------------------------------------------------

static DataChunker sChunker;

CollisionStateList CollisionStateList::sFreeList;
CollisionWorkingList CollisionWorkingList::sFreeList;
F32 sqrDistanceEdges(const Point3F& start0,
                     const Point3F& end0,
                     const Point3F& start1,
                     const Point3F& end1,
                     Point3F*       is,
                     Point3F*       it);


//----------------------------------------------------------------------------
// Collision State
//----------------------------------------------------------------------------

CollisionState::CollisionState()
{
   mLista = mListb = 0;
}

CollisionState::~CollisionState()
{
   if (mLista)
      mLista->free();
   if (mListb)
      mListb->free();
}

void CollisionState::swap()
{
}

void CollisionState::set(Convex* a,Convex* b,const MatrixF& a2w, const MatrixF& b2w)
{
}


F32 CollisionState::distance(const MatrixF& a2w, const MatrixF& b2w, const F32 dontCareDist,
                       const MatrixF* w2a, const MatrixF* _w2b)
{
   return 0;
}

void CollisionState::render()
{
}



//----------------------------------------------------------------------------
// Feature Collision
//----------------------------------------------------------------------------

bool ConvexFeature::collide(ConvexFeature& cf,CollisionList* cList, F32 tol)
{
   // Our vertices vs. other faces
   const Point3F* vert = mVertexList.begin();
   const Point3F* vend = mVertexList.end();
   while (vert != vend) {
      cf.testVertex(*vert,cList,false, tol);
      vert++;
   }

   // Other vertices vs. our faces
   vert = cf.mVertexList.begin();
   vend = cf.mVertexList.end();
   while (vert != vend) {
      U32 storeCount = cList->count;
      testVertex(*vert,cList,true, tol);

      // Fix up last reference.  material and object are copied from this rather
      //  than the object we're colliding against.
      if (storeCount != cList->count) {
         cList->collision[cList->count - 1].material = cf.material;
         cList->collision[cList->count - 1].object   = cf.object;
      }
      vert++;
   }

   // Edge vs. Edge
   const Edge* edge = mEdgeList.begin();
   const Edge* eend = mEdgeList.end();
   while (edge != eend) {
      cf.testEdge(this,mVertexList[edge->vertex[0]],
         mVertexList[edge->vertex[1]],cList, tol);
      edge++;
   }

   return true;
}

inline bool isInside(const Point3F& p, const Point3F& a, const Point3F& b, const VectorF& n)
{
   VectorF v;
   mCross(n,b - a,&v);
   return mDot(v,p - a) < 0.0f;
}

void ConvexFeature::testVertex(const Point3F& v,CollisionList* cList,bool flip, F32 tol)
{
   // Test vertex against all faces
   const Face* face = mFaceList.begin();
   const Face* end  = mFaceList.end();
   for (; face != end; face++) {
      if (cList->count >= CollisionList::MaxCollisions)
         return;

      const Point3F& p0 = mVertexList[face->vertex[0]];
      const Point3F& p1 = mVertexList[face->vertex[1]];
      const Point3F& p2 = mVertexList[face->vertex[2]];

      // Point near the plane?
      F32 distance = mDot(face->normal,v - p0);
      if (distance > tol || distance < -tol)
         continue;

      // Make sure it's within the bounding edges
      if (isInside(v,p0,p1,face->normal) && isInside(v,p1,p2,face->normal) &&
            isInside(v,p2,p0,face->normal)) {

         // Add collision to this face
         Collision& info = cList->collision[cList->count++];
         info.point = v;
         info.normal = face->normal;
         if (flip)
            info.normal.neg();
         info.material = material;
         info.object = object;
         info.distance = distance;
      }
   }
}

void ConvexFeature::testEdge(ConvexFeature* cf,const Point3F& s1, const Point3F& e1, CollisionList* cList, F32 tol)
{
   F32 tolSquared = tol*tol;

   // Test edges against edges
   const Edge* edge = mEdgeList.begin();
   const Edge* end  = mEdgeList.end();
   for (; edge != end; edge++) {
      if (cList->count >= CollisionList::MaxCollisions)
         return;

      const Point3F& s2 = mVertexList[edge->vertex[0]];
      const Point3F& e2 = mVertexList[edge->vertex[1]];

      // Get the distance and closest points
      Point3F i1,i2;
      F32 distance = sqrDistanceEdges(s1, e1, s2, e2, &i1, &i2);
      if (distance > tolSquared)
         continue;
      distance = mSqrt(distance);

      // Need to figure out how to orient the collision normal.
      // The current test involves checking to see whether the collision
      // points are contained within the convex volumes, which is slow.
      if (inVolume(i1) || cf->inVolume(i2))
         distance = -distance;

      // Contact normal
      VectorF normal = i1 - i2;
      if ( mIsZero( distance ) )
         normal.zero();
      else
      normal *= 1 / distance;

      // Return a collision
      Collision& info = cList->collision[cList->count++];
      info.point    = i1;
      info.normal   = normal;
      info.distance = distance;
      info.material = material;
      info.object   = object;
   }
}

bool ConvexFeature::inVolume(const Point3F& v)
{
   // Test the point to see if it's inside the volume
   const Face* face = mFaceList.begin();
   const Face* end  = mFaceList.end();
   for (; face != end; face++) {
      const Point3F& p0 = mVertexList[face->vertex[0]];
      if (mDot(face->normal,v - p0) > 0)
         return false;
   }
   return true;
}


//----------------------------------------------------------------------------
// Collision State management
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------

CollisionStateList::CollisionStateList()
{
   mPrev = mNext = this;
   mState = NULL;
}

void CollisionStateList::linkAfter(CollisionStateList* ptr)
{
   mPrev = ptr;
   mNext = ptr->mNext;
   ptr->mNext = this;
   mNext->mPrev = this;
}

void CollisionStateList::unlink()
{
   mPrev->mNext = mNext;
   mNext->mPrev = mPrev;
   mPrev = mNext = this;
}

CollisionStateList* CollisionStateList::alloc()
{
   if (!sFreeList.isEmpty()) {
      CollisionStateList* nxt = sFreeList.mNext;
      nxt->unlink();
      nxt->mState = NULL;
      return nxt;
   }
   return constructInPlace((CollisionStateList*)sChunker.alloc(sizeof(CollisionStateList)));
}

void CollisionStateList::free()
{
   unlink();
   linkAfter(&sFreeList);
}


//----------------------------------------------------------------------------

CollisionWorkingList::CollisionWorkingList()
{
   wLink.mPrev = wLink.mNext = this;
   rLink.mPrev = rLink.mNext = this;
}

void CollisionWorkingList::wLinkAfter(CollisionWorkingList* ptr)
{
   wLink.mPrev = ptr;
   wLink.mNext = ptr->wLink.mNext;
   ptr->wLink.mNext = this;
   wLink.mNext->wLink.mPrev = this;
}

void CollisionWorkingList::rLinkAfter(CollisionWorkingList* ptr)
{
   rLink.mPrev = ptr;
   rLink.mNext = ptr->rLink.mNext;
   ptr->rLink.mNext = this;
   rLink.mNext->rLink.mPrev = this;
}

void CollisionWorkingList::unlink()
{
   wLink.mPrev->wLink.mNext = wLink.mNext;
   wLink.mNext->wLink.mPrev = wLink.mPrev;
   wLink.mPrev = wLink.mNext = this;

   rLink.mPrev->rLink.mNext = rLink.mNext;
   rLink.mNext->rLink.mPrev = rLink.mPrev;
   rLink.mPrev = rLink.mNext = this;
}

CollisionWorkingList* CollisionWorkingList::alloc()
{
   if (sFreeList.wLink.mNext != &sFreeList) {
      CollisionWorkingList* nxt = sFreeList.wLink.mNext;
      nxt->unlink();
      return nxt;
   }
   return constructInPlace((CollisionWorkingList*)sChunker.alloc(sizeof(CollisionWorkingList)));
}

void CollisionWorkingList::free()
{
   unlink();
   wLinkAfter(&sFreeList);
}


//----------------------------------------------------------------------------
// Convex Base Class
//----------------------------------------------------------------------------

U32 Convex::sTag = (U32)-1;

//----------------------------------------------------------------------------

Convex::Convex()
{
   mNext = mPrev = this;
   mTag = 0;
}

Convex::~Convex()
{
   // Unlink from Convex Database
   unlink();

   // Delete collision states
   while (mList.mNext != &mList)
      delete mList.mNext->mState;

   // Free up working list
   while (mWorking.wLink.mNext != &mWorking)
      mWorking.wLink.mNext->free();

   // Free up references
   while (mReference.rLink.mNext != &mReference)
      mReference.rLink.mNext->free();
}


//----------------------------------------------------------------------------

void Convex::collectGarbage()
{
   // Delete unreferenced Convex Objects
   for (Convex* itr = mNext; itr != this; itr = itr->mNext) {
      if (itr->mReference.rLink.mNext == &itr->mReference) {
         Convex* ptr = itr;
         itr = itr->mPrev;
         delete ptr;
      }
   }
}

void Convex::nukeList()
{
   // Delete all Convex Objects
   for (Convex* itr = mNext; itr != this; itr = itr->mNext) {
      Convex* ptr = itr;
      itr = itr->mPrev;
      delete ptr;
   }
}

void Convex::registerObject(Convex *convex)
{
   convex->linkAfter(this);
}


//----------------------------------------------------------------------------

void Convex::linkAfter(Convex* ptr)
{
   mPrev = ptr;
   mNext = ptr->mNext;
   ptr->mNext = this;
   mNext->mPrev = this;
}

void Convex::unlink()
{
   mPrev->mNext = mNext;
   mNext->mPrev = mPrev;
   mPrev = mNext = this;
}


//----------------------------------------------------------------------------

Point3F Convex::support(const VectorF&) const
{
   return Point3F(0,0,0);
}

void Convex::getFeatures(const MatrixF&,const VectorF&,ConvexFeature* f)
{
   f->object = NULL;
}

const MatrixF& Convex::getTransform() const
{
   return mObject->getTransform();
}

const Point3F& Convex::getScale() const
{
   return mObject->getScale();
}

Box3F Convex::getBoundingBox() const
{
   return mObject->getWorldBox();
}

Box3F Convex::getBoundingBox(const MatrixF& mat, const Point3F& scale) const
{
   Box3F wBox = mObject->getObjBox();
   wBox.min.convolve(scale);
   wBox.max.convolve(scale);
   mat.mul(wBox);
   return wBox;
}

void Convex::render()
{
   for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext)
      if (itr->mState->mLista == itr)
         itr->mState->render();
}


//----------------------------------------------------------------------------

void Convex::addToWorkingList(Convex* ptr)
{
   CollisionWorkingList* cl = CollisionWorkingList::alloc();
   cl->wLinkAfter(&mWorking);
   cl->rLinkAfter(&ptr->mReference);
   cl->mConvex = ptr;
};


//----------------------------------------------------------------------------

void Convex::updateWorkingList(const Box3F& box, const U32 colMask)
{
   sTag++;

   // Clear objects off the working list that are no longer intersecting
   for (CollisionWorkingList* itr = mWorking.wLink.mNext; itr != &mWorking; itr = itr->wLink.mNext) {
      itr->mConvex->mTag = sTag;
      if ((!box.isOverlapped(itr->mConvex->getBoundingBox())) || (!itr->mConvex->getObject()->isCollisionEnabled())) {
         CollisionWorkingList* cl = itr;
         itr = itr->wLink.mPrev;
         cl->free();
      }
   }

   // Special processing for the terrain and interiors...
   AssertFatal(mObject->getContainer(), "Must be in a container!");

   SimpleQueryList sql;
   mObject->getContainer()->findObjects(box, colMask,SimpleQueryList::insertionCallback, &sql);
   for (U32 i = 0; i < sql.mList.size(); i++)
      sql.mList[i]->buildConvex(box, this);
}

// ---------------------------------------------------------------------------

void Convex::updateStateList(const MatrixF& mat, const Point3F& scale, const Point3F* displacement)
{
   Box3F box1 = getBoundingBox(mat, scale);
   box1.min -= Point3F(1, 1, 1);
   box1.max += Point3F(1, 1, 1);
   if (displacement) {
      Point3F oldMin = box1.min;
      Point3F oldMax = box1.max;

      box1.min.setMin(oldMin + *displacement);
      box1.min.setMin(oldMax + *displacement);
      box1.max.setMax(oldMin + *displacement);
      box1.max.setMax(oldMax + *displacement);
   }
   sTag++;

   // Destroy states which are no longer intersecting
   for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext) {
      Convex* cv = (itr->mState->a == this)? itr->mState->b: itr->mState->a;
      cv->mTag = sTag;
      if (!box1.isOverlapped(cv->getBoundingBox())) {
         CollisionState* cs = itr->mState;
         itr = itr->mPrev;
         delete cs;
      }
   }

   // Add collision states for new overlapping objects
   for (CollisionWorkingList* itr0 = mWorking.wLink.mNext; itr0 != &mWorking; itr0 = itr0->wLink.mNext) {
      register Convex* cv = itr0->mConvex;
      if (cv->mTag != sTag && box1.isOverlapped(cv->getBoundingBox())) {
         CollisionState* state = new GjkCollisionState;
         state->set(this,cv,mat,cv->getTransform());
         state->mLista->linkAfter(&mList);
         state->mListb->linkAfter(&cv->mList);
      }
   }
}


//----------------------------------------------------------------------------

CollisionState* Convex::findClosestState(const MatrixF& mat, const Point3F& scale, const F32 dontCareDist)
{
   updateStateList(mat, scale);
   F32 dist = +1E30;
   CollisionState *st = 0;

   // Prepare scaled version of transform
   MatrixF axform = mat;
   axform.scale(scale);
   MatrixF axforminv(true);
   MatrixF temp(mat);
   axforminv.scale(Point3F(1.0f/scale.x,1.0f/scale.y,1.0f/scale.z));
   temp.affineInverse();
   axforminv.mul(temp);

   for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext) {
      CollisionState* state = itr->mState;
      if (state->mLista != itr)
         state->swap();

      // Prepare scaled version of transform
      MatrixF bxform = state->b->getTransform();
      temp = bxform;
      Point3F bscale = state->b->getScale();
      bxform.scale(bscale);
      MatrixF bxforminv(true);
      bxforminv.scale(Point3F(1.0f/bscale.x,1.0f/bscale.y,1.0f/bscale.z));
      temp.affineInverse();
      bxforminv.mul(temp);

      //
      F32 dd = state->distance(axform, bxform, dontCareDist, &axforminv, &bxforminv);
      if (dd < dist) {
         dist = dd;
         st = state;
      }
   }
   if (dist < dontCareDist)
      return st;
   else
      return NULL;
}


//----------------------------------------------------------------------------

bool Convex::getCollisionInfo(const MatrixF& mat, const Point3F& scale, CollisionList* cList,F32 tol)
{
   for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext) {
      CollisionState* state = itr->mState;
      if (state->mLista != itr)
         state->swap();
      if (state->dist <= tol) {
         ConvexFeature fa,fb;
         VectorF v;

         // The idea is that we need to scale the matrix, so we need to
         // make a copy of it, before we can pass it in to getFeatures.
         // This is used to scale us for comparison against the other
         // convex, which is correctly scaled.
         MatrixF omat = mat;
         omat.scale(scale);
         MatrixF imat = omat;
         imat.inverse();
         imat.mulV(-state->v,&v);
         getFeatures(omat,v,&fa);

         imat = state->b->getTransform();
         imat.scale(state->b->getScale());
         MatrixF bxform = imat;
         imat.inverse();
         imat.mulV(state->v,&v);
         state->b->getFeatures(bxform,v,&fb);

         fa.collide(fb,cList,tol);
      }
   }
   return (cList->count != 0);
}

void Convex::getPolyList(AbstractPolyList*)
{

}


//-----------------------------------------------------------------------------
// This function based on code orignally written for the book:
// 3D Game Engine Design, by David H. Eberly
//
F32 sqrDistanceEdges(const Point3F& start0, const Point3F& end0,
   const Point3F& start1, const Point3F& end1,
   Point3F* is, Point3F* it)
{
   Point3F direction0 = end0 - start0;
   F32 fA00 = direction0.lenSquared();

   Point3F direction1 = end1 - start1;
   F32 fA11 = direction1.lenSquared();
   F32 fA01 = -mDot(direction0, direction1);

   Point3F kDiff = start0 - start1;
   F32 fC   = kDiff.lenSquared();
   F32 fB0  = mDot(kDiff, direction0);
   F32 fDet = mAbs((S32)(fA00*fA11 - fA01*fA01));

   // Since the endpoints are tested as vertices, we're not interested
   // in parallel lines, and intersections that don't involve end-points.
   if (fDet >= 0.00001) {

      // Calculate time of intersection for each line
      F32 fB1 = -mDot(kDiff, direction1);
      F32 fS  = fA01*fB1-fA11*fB0;
      F32 fT  = fA01*fB0-fA00*fB1;

      // Only interested in collisions that don't involve the end points
      if (fS >= 0.0 && fS <= fDet && fT >= 0.0 && fT <= fDet) {
         F32 fInvDet = 1.0 / fDet;
         fS *= fInvDet;
         fT *= fInvDet;
         F32 fSqrDist = (fS*(fA00*fS + fA01*fT + 2.0*fB0) +
            fT*(fA01*fS + fA11*fT + 2.0*fB1) + fC);

         // Intersection points.
         *is = start0 + direction0 * fS;
         *it = start1 + direction1 * fT;
         return mFabs(fSqrDist);
      }
   }

   // Return a large number in the cases where endpoints are involved.
   return 1e10f;
}