#include "planar.h"
#include "vertex.h"

#include "mathlib.h"
#include "datalib.h"

FLOAT CalculateArea(CList<Point *, UINT_8> *pVertices)
{
  CListIter<Point *, UINT_8> VtxIter;
  Point *pPt1, *pPt2;
  FLOAT fTriangleArea, fPolygonArea = 0.0;
  Vector vCross;

  VtxIter.Init(pVertices);
  pPt1 = VtxIter.PeekFirst();
  while (pPt2 =  VtxIter.PeekNext()) {
	vCross = (pPt1->Cross(*pPt2))/2.0;
	ASSERT (vCross(X) == 0.0);
	ASSERT (vCross(Y) == 0.0);
	fTriangleArea = vCross(Z);

	fPolygonArea += fTriangleArea;
	pPt1 = pPt2;
  }

  if (fPolygonArea < 0.0) 
	fPolygonArea = - fPolygonArea;
  return fPolygonArea;

}

//////////////////////////////////////////////////////////////////////////////
// CPlanar Class
//

CPlanar::CPlanar()
{
}

CPlanar::~CPlanar()
{
  //  Uninit();
}

VOID CPlanar::Init()
{
  CGeometry::Init();
}

VOID CPlanar::Uninit()
{
  CGeometry::Uninit();
}

BOOL CPlanar::Valid()
{
  if ( !CGeometry::Valid() )
    {
      return FALSE;
    }

  return TRUE;
}

VOID CPlanar::CalculateBound(CBound *pCBound)
{
  CPlanarIter *pCPlanarIter;
  CVertex *pCVertex;

  ASSERT( pCBound != NULL );

  pCPlanarIter = (CPlanarIter*)AllocateIter();
  ASSERT( pCPlanarIter != NULL );

  for ( pCVertex = pCPlanarIter->PeekFirstCVertex(); 
	pCVertex != NULL;
	pCVertex = pCPlanarIter->PeekNextCVertex() )
    {
      pCBound->Insert(pCVertex->GetPoint());
    }

//end:
  if ( pCPlanarIter != NULL )
    {
      FreeIter(pCPlanarIter);
    }
}

VOID CPlanar::CalculateTexCoords()
{
  CPlanarIter *pCPlanarIter;
  CVertex *pCVertex;
  DOUBLE dTheta, dDeltaTheta;
  Point pt;

  pCPlanarIter = (CPlanarIter*)AllocateIter();
  ASSERT( pCPlanarIter != NULL );

  if ( GetVertexCount() == 4 )
    {
      UINT_32 i;

      i = 0;
      for ( pCVertex = pCPlanarIter->PeekFirstCVertex();
	    pCVertex != NULL;
	    pCVertex = pCPlanarIter->PeekNextCVertex() )
	{
	  pt.Set((FLOAT)(((i>>1) & 0x1) ^ (i & 0x1)),
		 (FLOAT)((i>>1) & 0x1),
		 0.0);
	  pCVertex->SetTexPoint(pt);
	  i++;
	}
    }
  else
    {
      ASSERT( GetVertexCount() > 0 );
      dDeltaTheta = 2.0*PI/(DOUBLE)GetVertexCount();
      
      dTheta = PI/2.0;
      if ( (GetVertexCount() & 0x1) == 0 )
	{
	  dTheta += dDeltaTheta / 2.0;
	}
      
      for ( pCVertex = pCPlanarIter->PeekFirstCVertex();
	   pCVertex != NULL;
	   pCVertex = pCPlanarIter->PeekNextCVertex() )
	{
	  pt.Set(0.5f*(1.0f + (FLOAT)cos(dTheta)),
		 0.5f*(1.0f + (FLOAT)sin(dTheta)),
		 0.0f);
	  pCVertex->SetTexPoint(pt);
	  dTheta += dDeltaTheta;					   
	}
    }

//end:
  if ( pCPlanarIter != NULL )
    {
      FreeIter(pCPlanarIter);
    }
}

BOOL CPlanar::CheckTopology()
{
  return CheckPlanar();
}

// This uses Newell's Algorithm for finding the best fit planar equation
// for a set of points in 3 space (pg. 476 Foley and van Dam)
// I modified it so that the normal would be positive for counter-clockwise
// polygons, and I removed the 1/2 coefficient because we are concerned 
// with relative areas, not absolute areas.
BOOL CPlanar::CalculateNormal(Vector &vNormal)
{
  BOOL bCorrect;
  CPlanarIter *pCPlanarIter;
  CVertex *pCVertex;
  Point ptStart, ptPrev, ptCurr;

  bCorrect = TRUE;
  pCPlanarIter = (CPlanarIter*)AllocateIter();
  ASSERT( pCPlanarIter != NULL );

  vNormal.Null();

  pCVertex = pCPlanarIter->PeekFirstCVertex();
  ASSERT( pCVertex != NULL );
  ptStart = pCVertex->GetPoint();
  ptPrev = ptStart;

  for ( pCVertex = pCPlanarIter->PeekNextCVertex();
       pCVertex != NULL;
       pCVertex = pCPlanarIter->PeekNextCVertex() )
    {
      ptCurr = pCVertex->GetPoint();

      vNormal[X] += (ptPrev[Z] + ptCurr[Z])*(ptPrev[Y] - ptCurr[Y]);
      vNormal[Y] += (ptPrev[X] + ptCurr[X])*(ptPrev[Z] - ptCurr[Z]);
      vNormal[Z] += (ptPrev[Y] + ptCurr[Y])*(ptPrev[X] - ptCurr[X]);

      ptPrev = ptCurr;
    }

  ptCurr = ptStart;
  vNormal[X] += (ptPrev[Z] + ptCurr[Z])*(ptPrev[Y] - ptCurr[Y]);
  vNormal[Y] += (ptPrev[X] + ptCurr[X])*(ptPrev[Z] - ptCurr[Z]);
  vNormal[Z] += (ptPrev[Y] + ptCurr[Y])*(ptPrev[X] - ptCurr[X]);
  
  if ( !vNormal.Normalize() )
    {
      HandleError("Null normal because of zero area polygon");
      bCorrect = FALSE;
      goto end;
    }

end:
  if ( pCPlanarIter != NULL )
    {
      FreeIter(pCPlanarIter);
    }

  return bCorrect;
}

VOID CPlanar::CalculateCentroid(Point &ptCentroid)
{
  CPlanarIter *pCPlanarIter;
  UINT_32 i;
  CVertex *pCVertex;

  pCPlanarIter = (CPlanarIter*)AllocateIter();
  ASSERT( pCPlanarIter != NULL );

  ptCentroid.Null();

  i = 0;
  for ( pCVertex = pCPlanarIter->PeekFirstCVertex();
       pCVertex != NULL;
       pCVertex = pCPlanarIter->PeekNextCVertex() )
    {
      ptCentroid = ptCentroid + pCVertex->GetPoint();
      i++;
    }

  ptCentroid = ptCentroid / (FLOAT)i;

//end:
  if ( pCPlanarIter != NULL )
    {
      FreeIter(pCPlanarIter);
    }
}

// Make sure that all the vertices fit the plane equation
// a*X + b*Y + c*Z + d = 0
// within a precision slab (some fixed offset from the plane 
// through the centroid )
BOOL CPlanar::CheckPlanar()
{
  BOOL bCorrect;
  CPlanarIter *pCPlanarIter;
  CVertex *pCVertex;
  FLOAT d, fDistance;
  Point pt;
  Vector vNormal;
  Point ptCentroid;

  bCorrect = TRUE;
  pCPlanarIter = (CPlanarIter*)AllocateIter();
  ASSERT( pCPlanarIter != NULL );

  vNormal = GetNormal();
  ptCentroid = GetCentroid();

  d = -((vNormal[X] * ptCentroid[X]) + 
        (vNormal[Y] * ptCentroid[Y]) + 
        (vNormal[Z] * ptCentroid[Z]));

  for ( pCVertex = pCPlanarIter->PeekFirstCVertex();
       pCVertex != NULL;
       pCVertex = pCPlanarIter->PeekNextCVertex() )
    {
      pt = pCVertex->GetPoint();

      fDistance = (vNormal[X] * pt[X]) + 
                  (vNormal[Y] * pt[Y]) + 
	          (vNormal[Z] * pt[Z]) + d;

      if ( ABS(fDistance) > EPSILON )
	{
	  HandleError("Non-planar polygon");
	  bCorrect = FALSE;
	  goto end;
	}
    }

end:
  if ( pCPlanarIter != NULL )
    {
      FreeIter(pCPlanarIter);
    }

  return bCorrect;
}

BOOL CPlanar::CheckValidConvex()
{
  BOOL bCorrect;
  CPlanarIter *pCPlanarIter;
  CVertex *pCVertex;
  Vector vStart, vPrev, vCurr, vNorm;
  Point ptStart, ptPrev, ptCurr;
  Vector vNormal;

  vNormal = GetNormal();

  bCorrect = TRUE;
  pCPlanarIter = (CPlanarIter*)AllocateIter();
  ASSERT( pCPlanarIter != NULL );

  pCVertex = pCPlanarIter->PeekFirstCVertex();
  ASSERT( pCVertex != NULL );
  ptStart = pCVertex->GetPoint();

  pCVertex = pCPlanarIter->PeekNextCVertex();
  ASSERT( pCVertex != NULL );
  ptPrev = pCVertex->GetPoint();

  vStart = ptPrev - ptStart;
  vStart = vStart - (vNormal * (vNormal.Dot(vStart)));
  if ( !vStart.Normalize() )
    {
      HandleError("Zero Length Edge");
      bCorrect = FALSE;
      goto end;
    }
  vPrev = vStart;

  for ( pCVertex = pCPlanarIter->PeekNextCVertex();
       pCVertex != NULL;
       pCVertex = pCPlanarIter->PeekNextCVertex() )
    {
      ptCurr = pCVertex->GetPoint();
      
      vCurr = ptCurr - ptPrev;
      vCurr = vCurr - (vNormal * (vNormal.Dot(vCurr)));
      if ( !vCurr.Normalize() )
	{
	  HandleError("Zero Length Edge");
	  bCorrect = FALSE;
	  goto end;
	}
      
      vNorm = vPrev * vCurr;
      if ( !vNorm.Normalize() )
	{
	  if ( vPrev.Dot(vCurr) < 0 )
	    {
	      HandleError("Coincident adjacent edges");
	      bCorrect = FALSE;
	      goto end;
	    }
	}
      else if ( vNormal.Dot(vNorm) < 0 )
	{
	  HandleError("Bad Facet: Concave Polygon");
	  bCorrect = FALSE;
	  goto end;
	}

      ptPrev = ptCurr;
      vPrev = vCurr;
    }

  {
    vCurr = ptStart - ptPrev;
    vCurr = vCurr - (vNormal * (vNormal.Dot(vCurr)));
    if ( !vCurr.Normalize() )
      {
	HandleError("Zero Length Edge");
	bCorrect = FALSE;
	goto end;
      }
    
    vNorm = vPrev * vCurr;
    if ( !vNorm.Normalize() )
      {
	if ( vPrev.Dot(vCurr) < 0 )
	  {
	    HandleError("Coincident adjacent edges");
	    bCorrect = FALSE;
	    goto end;
	  }
      }
    else if ( vNormal.Dot(vNorm) < 0 )
      {
	HandleError("Bad Facet: Concave Polygon");
	bCorrect = FALSE;
	goto end;
      }
    
    vPrev = vCurr;
  }
  
  {
    vNorm = vPrev * vStart;
    if ( !vNorm.Normalize() )
      {
	if ( vPrev.Dot(vStart) < 0 )
	  {
	    HandleError("Coincident adjacent edges");
	    bCorrect = FALSE;
	    goto end;
	  }
      }
    else if ( vNormal.Dot(vNorm) < 0 )
      {
	HandleError("Bad Facet: Concave Polygon");
	bCorrect = FALSE;
	goto end;
      }
  }
    
end:
  if ( pCPlanarIter != NULL )
    {
      FreeIter(pCPlanarIter);
    }

  return bCorrect;
}

BOOL CPlanar::CheckValidConcave()
{
  BOOL bCorrect;
  CPlanarIter *pCPlanarIter;
  CVertex *pCVertex;
  Vector vStart, vPrev, vCurr, vNorm;
  Point ptStart, ptPrev, ptCurr;
  Vector vNormal;

  vNormal = GetNormal();

  bCorrect = TRUE;
  pCPlanarIter = (CPlanarIter*)AllocateIter();
  ASSERT( pCPlanarIter != NULL );

  pCVertex = pCPlanarIter->PeekFirstCVertex();
  ASSERT( pCVertex != NULL );
  ptStart = pCVertex->GetPoint();

  pCVertex = pCPlanarIter->PeekNextCVertex();
  ASSERT( pCVertex != NULL );
  ptPrev = pCVertex->GetPoint();

  vStart = ptPrev - ptStart;
  vStart = vStart - (vNormal * (vNormal.Dot(vStart)));
  if ( !vStart.Normalize() )
    {
      HandleError("Zero Length Edge");
      bCorrect = FALSE;
      goto end;
    }
  vPrev = vStart;

  for ( pCVertex = pCPlanarIter->PeekNextCVertex();
       pCVertex != NULL;
       pCVertex = pCPlanarIter->PeekNextCVertex() )
    {
      ptCurr = pCVertex->GetPoint();
      
      vCurr = ptCurr - ptPrev;
      vCurr = vCurr - (vNormal * (vNormal.Dot(vCurr)));
      if ( !vCurr.Normalize() )
	{
	  HandleError("Zero Length Edge");
	  bCorrect = FALSE;
	  goto end;
	}
      
      vNorm = vPrev * vCurr;
      if ( !vNorm.Normalize() )
	{
	  if ( vPrev.Dot(vCurr) < 0 )
	    {
	      HandleError("Coincident adjacent edges");
	      bCorrect = FALSE;
	      goto end;
	    }
	}
      /*
      else if ( vNormal.Dot(vNorm) < 0 )
	{
	  HandleError("Bad Facet: Concave Polygon");
	  bCorrect = FALSE;
	  goto end;
	}
	*/

      ptPrev = ptCurr;
      vPrev = vCurr;
    }

  {
    vCurr = ptStart - ptPrev;
    vCurr = vCurr - (vNormal * (vNormal.Dot(vCurr)));
    if ( !vCurr.Normalize() )
      {
	HandleError("Zero Length Edge");
	bCorrect = FALSE;
	goto end;
      }
    
    vNorm = vPrev * vCurr;
    if ( !vNorm.Normalize() )
      {
	if ( vPrev.Dot(vCurr) < 0 )
	  {
	    HandleError("Coincident adjacent edges");
	    bCorrect = FALSE;
	    goto end;
	  }
      }
    /*
    else if ( vNormal.Dot(vNorm) < 0 )
      {
	HandleError("Bad Facet: Concave Polygon");
	bCorrect = FALSE;
	goto end;
      }
      */
    
    vPrev = vCurr;
  }
  
  {
    vNorm = vPrev * vStart;
    if ( !vNorm.Normalize() )
      {
	if ( vPrev.Dot(vStart) < 0 )
	  {
	    HandleError("Coincident adjacent edges");
	    bCorrect = FALSE;
	    goto end;
	  }
      }
    /*
    else if ( vNormal.Dot(vNorm) < 0 )
      {
	HandleError("Bad Facet: Concave Polygon");
	bCorrect = FALSE;
	goto end;
      }
      */
  }
    
end:
  if ( pCPlanarIter != NULL )
    {
      FreeIter(pCPlanarIter);
    }

  return bCorrect;
}

Vector CPlanar::GetNormal()
{
  Vector vNormal;

  CalculateNormal(vNormal);

  return vNormal;
}

Point CPlanar::GetCentroid()
{
  Point ptCentroid;

  CalculateCentroid(ptCentroid);

  return ptCentroid;
}

//////////////////////////////////////////////////////////////////////////////
// CPlanarIter Class
//

CPlanarIter::CPlanarIter()
{
}

CPlanarIter::~CPlanarIter()
{
}

VOID CPlanarIter::Init(CPlanar *pCPlanar)
{
  ASSERT( pCPlanar != NULL );

  CGeometryIter::Init(pCPlanar);
}

VOID CPlanarIter::Uninit()
{
  CGeometryIter::Uninit();
}