#include "distance.hpp"
#include "log_trace.hpp"

inline Scalar my_abs(Scalar val) {return (val > (Scalar) 0.0 ? val : -val);}

//==============================================================
// distance_sqr
//==============================================================
Scalar distance_sqr(const Position & point,
                    const Triangle & triangle,
                    Scalar * SParam,
                    Scalar * TParam)
{
  Vector3 kDiff = triangle.origin - point;
  Scalar fA00 = triangle.edge0.mag2();
  Scalar fA01 = dot(triangle.edge0,triangle.edge1);
  Scalar fA11 = triangle.edge1.mag2();
  Scalar fB0 = dot(kDiff, triangle.edge0);
  Scalar fB1 = dot(kDiff, triangle.edge1);
  Scalar fC = kDiff.mag2();
  Scalar fDet = my_abs(fA00*fA11-fA01*fA01);
  Scalar fS = fA01*fB1-fA11*fB0;
  Scalar fT = fA01*fB0-fA00*fB1;
  Scalar fSqrDist;
  
  if ( fS + fT <= fDet )
  {
    if ( fS < (Scalar)0.0 )
    {
      if ( fT < (Scalar)0.0 )  // region 4
      {
        if ( fB0 < (Scalar)0.0 )
        {
          fT = (Scalar)0.0;
          if ( -fB0 >= fA00 )
          {
            fS = (Scalar)1.0;
            fSqrDist = fA00+((Scalar)2.0)*fB0+fC;
          }
          else
          {
            fS = -fB0/fA00;
            fSqrDist = fB0*fS+fC;
          }
        }
        else
        {
          fS = (Scalar)0.0;
          if ( fB1 >= (Scalar)0.0 )
          {
            fT = (Scalar)0.0;
            fSqrDist = fC;
          }
          else if ( -fB1 >= fA11 )
          {
            fT = (Scalar)1.0;
            fSqrDist = fA11+((Scalar)2.0)*fB1+fC;
          }
          else
          {
            fT = -fB1/fA11;
            fSqrDist = fB1*fT+fC;
          }
        }
      }
      else  // region 3
      {
        fS = (Scalar)0.0;
        if ( fB1 >= (Scalar)0.0 )
        {
          fT = (Scalar)0.0;
          fSqrDist = fC;
        }
        else if ( -fB1 >= fA11 )
        {
          fT = (Scalar)1.0;
          fSqrDist = fA11+((Scalar)2.0)*fB1+fC;
        }
        else
        {
          fT = -fB1/fA11;
          fSqrDist = fB1*fT+fC;
        }
      }
    }
    else if ( fT < (Scalar)0.0 )  // region 5
    {
      fT = (Scalar)0.0;
      if ( fB0 >= (Scalar)0.0 )
      {
        fS = (Scalar)0.0;
        fSqrDist = fC;
      }
      else if ( -fB0 >= fA00 )
      {
        fS = (Scalar)1.0;
        fSqrDist = fA00+((Scalar)2.0)*fB0+fC;
      }
      else
      {
        fS = -fB0/fA00;
        fSqrDist = fB0*fS+fC;
      }
    }
    else  // region 0
    {
      // minimum at interior point
      Scalar fInvDet = ((Scalar)1.0)/fDet;
      fS *= fInvDet;
      fT *= fInvDet;
      fSqrDist = fS*(fA00*fS+fA01*fT+((Scalar)2.0)*fB0) +
        fT*(fA01*fS+fA11*fT+((Scalar)2.0)*fB1)+fC;
    }
  }
  else
  {
    Scalar fTmp0, fTmp1, fNumer, fDenom;
    
    if ( fS < (Scalar)0.0 )  // region 2
    {
      fTmp0 = fA01 + fB0;
      fTmp1 = fA11 + fB1;
      if ( fTmp1 > fTmp0 )
      {
        fNumer = fTmp1 - fTmp0;
        fDenom = fA00-2.0f*fA01+fA11;
        if ( fNumer >= fDenom )
        {
          fS = (Scalar)1.0;
          fT = (Scalar)0.0;
          fSqrDist = fA00+((Scalar)2.0)*fB0+fC;
        }
        else
        {
          fS = fNumer/fDenom;
          fT = (Scalar)1.0 - fS;
          fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
            fT*(fA01*fS+fA11*fT+((Scalar)2.0)*fB1)+fC;
        }
      }
      else
      {
        fS = (Scalar)0.0;
        if ( fTmp1 <= (Scalar)0.0 )
        {
          fT = (Scalar)1.0;
          fSqrDist = fA11+((Scalar)2.0)*fB1+fC;
        }
        else if ( fB1 >= (Scalar)0.0 )
        {
          fT = (Scalar)0.0;
          fSqrDist = fC;
        }
        else
        {
          fT = -fB1/fA11;
          fSqrDist = fB1*fT+fC;
        }
      }
    }
    else if ( fT < (Scalar)0.0 )  // region 6
    {
      fTmp0 = fA01 + fB1;
      fTmp1 = fA00 + fB0;
      if ( fTmp1 > fTmp0 )
      {
        fNumer = fTmp1 - fTmp0;
        fDenom = fA00-((Scalar)2.0)*fA01+fA11;
        if ( fNumer >= fDenom )
        {
          fT = (Scalar)1.0;
          fS = (Scalar)0.0;
          fSqrDist = fA11+((Scalar)2.0)*fB1+fC;
        }
        else
        {
          fT = fNumer/fDenom;
          fS = (Scalar)1.0 - fT;
          fSqrDist = fS*(fA00*fS+fA01*fT+((Scalar)2.0)*fB0) +
            fT*(fA01*fS+fA11*fT+((Scalar)2.0)*fB1)+fC;
        }
      }
      else
      {
        fT = (Scalar)0.0;
        if ( fTmp1 <= (Scalar)0.0 )
        {
          fS = (Scalar)1.0;
          fSqrDist = fA00+((Scalar)2.0)*fB0+fC;
        }
        else if ( fB0 >= (Scalar)0.0 )
        {
          fS = (Scalar)0.0;
          fSqrDist = fC;
        }
        else
        {
          fS = -fB0/fA00;
          fSqrDist = fB0*fS+fC;
        }
      }
    }
    else  // region 1
    {
      fNumer = fA11 + fB1 - fA01 - fB0;
      if ( fNumer <= (Scalar)0.0 )
      {
        fS = (Scalar)0.0;
        fT = (Scalar)1.0;
        fSqrDist = fA11+((Scalar)2.0)*fB1+fC;
      }
      else
      {
        fDenom = fA00-2.0f*fA01+fA11;
        if ( fNumer >= fDenom )
        {
          fS = (Scalar)1.0;
          fT = (Scalar)0.0;
          fSqrDist = fA00+((Scalar)2.0)*fB0+fC;
        }
        else
        {
          fS = fNumer/fDenom;
          fT = (Scalar)1.0 - fS;
          fSqrDist = fS*(fA00*fS+fA01*fT+((Scalar)2.0)*fB0) +
            fT*(fA01*fS+fA11*fT+((Scalar)2.0)*fB1)+fC;
        }
      }
    }
    }
    
    if ( SParam )
      *SParam = fS;
    
    if ( TParam )
      *TParam = fT;
    
    return my_abs(fSqrDist);}
    
    
    //==============================================================
    // distance
    //==============================================================
    Scalar distance(const Position & point,
      const Triangle & triangle,
      Scalar * SParam,
      Scalar * TParam)
    {
      return sqrt(distance_sqr(point, triangle, SParam, TParam));
    }
    
    //==============================================================
    // distance_sqr_point_triangle
    //==============================================================
    Scalar distance_sqr_point_triangle(const Position & point,
      const Position & tri_pos0,
      const Position & tri_pos1,
      const Position & tri_pos2,
      Position & point_on_triangle,
      bool & point_on_triangle_edge)
    {
      Triangle triangle(tri_pos0, tri_pos1 - tri_pos0, tri_pos2 - tri_pos0);
      Scalar S, T;
      
      Scalar dist_sqr = distance_sqr(point, triangle, &S, &T);
      point_on_triangle = triangle.origin + S * triangle.edge0 + T * triangle.edge1;
      /*
      TRACE("=======================\n");
      point.show("point");
      tri_pos0.show("tri0");
      tri_pos1.show("tri1");
      tri_pos2.show("tri2");
      point_on_triangle.show("tri point");
      TRACE("distance = %5.2f\n", sqrt(dist_sqr));
      */
      const Scalar epsilon = 0.00001f;
      if ( (S < epsilon) ||
        (T < epsilon) ||
        ( (S + T) > (0.5f - epsilon) ) )
        point_on_triangle_edge = true;
      else
        point_on_triangle_edge = false;
      
      return dist_sqr;
    }
    
//==============================================================
// distance_sqr_point_ellipsoid
//==============================================================
Scalar distance_sqr(Vector3 rkPoint, 
  const Ellipsoid_shape & rkEllipsoid,
  Vector3 & rkClosest)
{
#define ACCURATE_BUT_WRONG
#ifdef ACCURATE_BUT_WRONG
  // goes wrong when the dimensions are < 1 or so... due to numerical accuracy I think.
  // so scale everything up...
  const Scalar scale = 10.0f;
    
  const Vector3 afExtent = scale * rkEllipsoid.extents;
  rkPoint *= scale;

  Scalar fA2 = afExtent[0]*afExtent[0];
  Scalar fB2 = afExtent[1]*afExtent[1];
  Scalar fC2 = afExtent[2]*afExtent[2];
  Scalar fU2 = rkPoint[0]*rkPoint[0];
  Scalar fV2 = rkPoint[1]*rkPoint[1];
  Scalar fW2 = rkPoint[2]*rkPoint[2];
  Scalar fA2U2 = fA2*fU2, fB2V2 = fB2*fV2, fC2W2 = fC2*fW2;
  
  // initial guess
  Scalar fURatio = rkPoint[0]/afExtent[0];
  Scalar fVRatio = rkPoint[1]/afExtent[1];
  Scalar fWRatio = rkPoint[2]/afExtent[2];
  Scalar fT;
  if ( fURatio*fURatio+fVRatio*fVRatio+fWRatio*fWRatio < (Scalar)1.0 )
  {
    fT = (Scalar)0.0f;
  }
  else
  {
    Scalar fMax = afExtent[0];
    if ( afExtent[1] > fMax )
      fMax = afExtent[1];
    if ( afExtent[2] > fMax )
      fMax = afExtent[2];
    
    fT = fMax*rkPoint.mag();
  }
  
  // Newton's method
  const int iMaxIteration = 64;
  Scalar fP, fQ, fR;
  for (int i = 0; i < iMaxIteration; i++)
  {
    fP = fT+fA2;
    fQ = fT+fB2;
    fR = fT+fC2;
    Scalar fP2 = fP*fP;
    Scalar fQ2 = fQ*fQ;
    Scalar fR2 = fR*fR;
    Scalar fS = fP2*fQ2*fR2-fA2U2*fQ2*fR2-fB2V2*fP2*fR2-fC2W2*fP2*fQ2;
    if ( fabs(fS) < 1e-16 )
      break;
    
    Scalar fPQ = fP*fQ, fPR = fP*fR, fQR = fQ*fR, fPQR = fP*fQ*fR;
    Scalar fDS = ((Scalar)2.0)*(fPQR*(fQR+fPR+fPQ)-fA2U2*fQR*(fQ+fR)-
      fB2V2*fPR*(fP+fR)-fC2W2*fPQ*(fP+fQ));
    fT -= fS/fDS;
  }
  
  rkClosest[0] = fA2*rkPoint[0]/fP;
  rkClosest[1] = fB2*rkPoint[1]/fQ;
  rkClosest[2] = fC2*rkPoint[2]/fR;
  Vector3 kDiff = rkClosest - rkPoint;
  rkClosest /= scale;
  kDiff /= scale;
  return kDiff.mag2();
#else
  // this is inaccurate, but gives a sensible result for in and out
  Position position = rkPoint;
  const Vector3 & afExtent = rkEllipsoid.extents;
  position[0] /= afExtent[0];
  position[1] /= afExtent[1];
  position[2] /= afExtent[2];
  
  Scalar rad = position.mag();
  rkClosest = position * 1.0f / rad;
  
  rkClosest[0] *= afExtent[0];
  rkClosest[1] *= afExtent[1];
  rkClosest[2] *= afExtent[2];
  
  Vector3 kDiff = rkClosest - rkPoint;
  return kDiff.mag2();
#endif
}