#include <algorithm>
#include <complex>
using std::complex;
// std::sqrt is invoked explicitly, because the real version is also
// used.  Probably an unnecessary precaution, but so what?
using std::real;
using std::imag;
using std::pow;

#include <math.h>
#include "GR_BdryPatch2D.h"
#include "GR_Bdry2D.h"
#include "GR_List.h"


// *******************************************************************
//
// BdryCubicParam2D
// -------
//
// Another descendant of BdryPatch2D.
// Implements features for general cubic parametric curves.
//
// *******************************************************************

BdryCubicParam2D::
BdryCubicParam2D () :
  BdryPatch2D(), iEqnTypeX(3), iEqnTypeY(3),
  iXNumFirstRoots(0), iYNumFirstRoots(0),
  iXNumSecondRoots(0), iYNumSecondRoots(0), dOrientationChange(0),
  LdOrderedRoots(), LdOrientationChangeAtRoots()
{
  iBCLeft = iBCRight = iInvalidBC;
  iRegionLeft = iRegionRight = iInvalidRegion;
  iBeginPt = iEndPt = -1;
  eGeom2DType = eCubicParam;
}

BdryCubicParam2D::
BdryCubicParam2D(Bdry2D* pBdryObject, const int iBCL, const int iBCR,
		 const int iRL, const int iRR,
		 const int iBPt, const int iEPt,
		 const double dax, const double dbx,
		 const double dcx, const double ddx,
		 const double day, const double dby,
		 const double dcy, const double ddy):
  BdryPatch2D (pBdryObject, iBCL, iBCR, iRL, iRR, iBPt, iEPt),
  iEqnTypeX(3), iEqnTypeY(3), iXNumFirstRoots(0), iYNumFirstRoots(0),
  iXNumSecondRoots(0), iYNumSecondRoots(0), dOrientationChange(0),
  LdOrderedRoots(), LdOrientationChangeAtRoots()
{
  // The verts HAVE to be valid..
  assert(iBPt >= 0);
  assert(iEPt >= 0);	
  // Set boundary type
  eGeom2DType = eCubicParam;
  // Store the coefficients
  adCoeffs[0][0] = dax; adCoeffs[0][1] = dbx; adCoeffs[0][2] = dcx; adCoeffs[0][3] = ddx;
  adCoeffs[1][0] = day; adCoeffs[1][1] = dby; adCoeffs[1][2] = dcy; adCoeffs[1][3] = ddy;
  // Calculate information about patch
  vRefresh ();
}

// Constructor needed for descendants
BdryCubicParam2D::
BdryCubicParam2D(Bdry2D* pBdryObject, const int iBCL, const int iBCR,
		 const int iRL,  const int iRR,
		 const int iBPt, const int iEPt):
  BdryPatch2D (pBdryObject, iBCL, iBCR, iRL, iRR, iBPt, iEPt),
  iEqnTypeX(3), iEqnTypeY(3), iXNumFirstRoots(0), iYNumFirstRoots(0),
  iXNumSecondRoots(0), iYNumSecondRoots(0), dOrientationChange(0),
  LdOrderedRoots(), LdOrientationChangeAtRoots()
{
  // Nothing more to do
}


void BdryCubicParam2D::
vAssignData (Bdry2D* pBdryObject, const int iBCL, const int iBCR,
	     const int iRL, const int iRR,
	     const int iBPt, const int iEPt)
{
  BdryPatch2D::vAssignData (pBdryObject, iBCL, iBCR, iRL, iRR, iBPt, iEPt);
  // Compute data..
  vRefresh ();
}

void BdryCubicParam2D::
vGetAllCoefficients(double *dAx, double *dBx, double *dCx, double *dDx,
		    double *dAy, double *dBy, double *dCy, double *dDy)
{
  *dAx = adCoeffs[0][0];
  *dBx = adCoeffs[0][1];
  *dCx = adCoeffs[0][2];
  *dDx = adCoeffs[0][3];
  *dAy = adCoeffs[1][0];
  *dBy = adCoeffs[1][1];
  *dCy = adCoeffs[1][2];
  *dDy = adCoeffs[1][3];	
}

void   BdryCubicParam2D::
vSetAllCoefficients(const double dAx, const double dBx, const double dCx, const double dDx,
		    const double dAy, const double dBy, const double dCy, const double dDy)
{
  // Get the end coordinates;
  double adB[2], adE[2];
  vGetBeginPoint(adB);
  vGetEndPoint(adE);
	
  assert(iFuzzyComp(dDx, adB[0]) == 0);
  assert(iFuzzyComp(dDy, adB[1]) == 0);
  assert(iFuzzyComp(dAx + dBx + dCx + dDx, adE[0]) == 0);
  assert(iFuzzyComp(dAy + dBy + dCy + dDy, adE[1]) == 0);
	
	// Can assign them then...
  adCoeffs[0][0] = dAx;
  adCoeffs[0][1] = dBx;
  adCoeffs[0][2] = dCx;
  adCoeffs[0][3] = dDx;
  adCoeffs[1][0] = dAy;
  adCoeffs[1][1] = dBy;
  adCoeffs[1][2] = dCy;
  adCoeffs[1][3] = dDy; 								
	
  vRefresh();
}

void BdryCubicParam2D::
vDetermineEqnTypes()
{
  iEqnTypeX = 3; iEqnTypeY = 3;
	
  if (iFuzzyComp(adCoeffs[0][0], 0.0) == 0) {
    iEqnTypeX--;
    if (iFuzzyComp(adCoeffs[0][1], 0.0) == 0) {
      iEqnTypeX--;
      if (iFuzzyComp(adCoeffs[0][2], 0.0) == 0) {
	iEqnTypeX--;
      }
    }
  }

  if (iFuzzyComp(adCoeffs[1][0], 0.0) == 0) {
    iEqnTypeY--;
    if (iFuzzyComp(adCoeffs[1][1], 0.0) == 0) {
      iEqnTypeY--;
      if (iFuzzyComp(adCoeffs[1][2], 0.0) == 0) {
	iEqnTypeY--;
      }
    }
  }
		
}

void BdryCubicParam2D::
vSolveCubic(double dA, double dB, double dC, double dD,
	    int *iNumRoots, double adRoots[3]) const
{
  // Find the roots for the cubicc dA x*x*x + dB x*x + dC x + dD == 0

  // First find the order of the equation
  int iOrder = 3;

  if (iFuzzyComp(dA, 0.0) == 0) {
    iOrder--;
    if (iFuzzyComp(dB, 0.0) == 0) {
      iOrder--;
      if (iFuzzyComp(dC, 0.0) == 0) {
	iOrder--;
      }
    }
  }	
	
  if (iOrder >= 1) {
    if (iOrder >= 2) {
      if (iOrder == 3) {
				// Really a cubic equation...
				// Make a change of variables first
				// Put it in form y^3 + p y^2 + q y + r = 0
	double dp = dB/dA;
	double dq = dC/dA;
	double dr = dD/dA;
				
	double da, db;
	int iResult;
				
				// Now, let y == x - p/3; end up with x^3 + a x + b == 0
				// with a and b :
				
	da = (3.0*dq - dp*dp)/3.0;
	db = (2.0*dp*dp*dp - 9.0*dp*dq + 27.0*dr)/27.0;
				
				// Calculate first radical
	double dRad = ((db*db)/4.0) + ((da*da*da)/27.0);
				// Create complex variable
	complex<double> dcRad = dRad;
				// Take sqrt of it
	dcRad = std::sqrt(dcRad);
				
	complex<double> dcRad1 = -(db/2.0) + dcRad;
	complex<double> dcRad2 = -(db/2.0) - dcRad;
				
				// Calculate A and B, which are used to determine the roots

	double dFrac = 1.0/3.0;
	complex<double> dcA;
	complex<double> dcB;
								
				// Avoid the "cubic root of a negative non-complex number" problem
	if ( (iFuzzyComp(imag(dcRad1), 0.0) == 0) &&
	     (iFuzzyComp(real(dcRad1), 0.0) < 0) ) {
	  dcA = -pow(-dcRad1, dFrac);
	}
	else {
	  dcA = pow(dcRad1, dFrac);
	}
				
	if ( (iFuzzyComp(imag(dcRad2), 0.0) == 0) &&
	     (iFuzzyComp(real(dcRad2), 0.0) < 0) ) {
	  dcB = -pow(-dcRad2, dFrac);
	}
	else {
	  dcB = pow(dcRad2, dFrac);
	}
				
				// Now look at the radical to determine which roots you want
	iResult = iFuzzyComp(dRad, 0.0);
				
	switch (iResult) {
	case -1 : {
	  // Three unequal and real roots
	  *iNumRoots = 3;
	  // Define sqrt(-3);
	  complex<double> dcRt3(-3., 0.);
	  complex<double> dcr1, dcr2, dcr3;
							
	  dcRt3 = std::sqrt(dcRt3);
	  dcr1 = dcA + dcB;
	  dcr2 = -(dcr1/2.0) + ((dcA - dcB)/2.0)*dcRt3;
	  dcr3 = -(dcr1/2.0) - ((dcA - dcB)/2.0)*dcRt3;
														
	  // These are the roots for x^3 + a x +b = 0
	  adRoots[0] = real(dcr1);
	  adRoots[1] = real(dcr2);
	  adRoots[2] = real(dcr3);
							
	  // Remember, y = x - p/3;
	  adRoots[0] -= dp/3.0;
	  adRoots[1] -= dp/3.0;
	  adRoots[2] -= dp/3.0;
							
	}
	break;
	case 0  : {
	  // Three real roots, but at least two are equal
	  *iNumRoots =3;
	  // Define sqrt(-3);
	  complex<double> dcRt3(-3., 0.);
	  complex<double> dcr1, dcr2, dcr3;
	  dcRt3 = std::sqrt(dcRt3);
							
	  dcr1 = dcA + dcB;
	  dcr2 = -(dcr1/2.0) + ((dcA - dcB)/2.0)*dcRt3;
	  dcr3 = -(dcr1/2.0) - ((dcA - dcB)/2.0)*dcRt3;
												
	  adRoots[0] = real(dcr1);
	  adRoots[1] = real(dcr2);
	  adRoots[2] = real(dcr3);

	  // Remember, y = x - p/3;
	  adRoots[0] -= dp/3.0;
	  adRoots[1] -= dp/3.0;
	  adRoots[2] -= dp/3.0;
														
	}
	break;
	case 1  : {
	  // Only one real root, two conjugate complex roots
	  *iNumRoots = 1;
	  complex<double> dcr1, dcr2, dcr3;
	  complex<double> dcRt3(-3.0, 0.0);
#ifndef NDEBUG
	  bool qFound = false;
#endif
	  dcRt3 = std::sqrt(dcRt3);
							
	  dcr1 = dcA + dcB;
	  dcr2 = -(dcr1/2.0) + ((dcA - dcB)/2.0)*dcRt3;
	  dcr3 = -(dcr1/2.0) - ((dcA - dcB)/2.0)*dcRt3;
							
	  // Find which root it is...
	  if (iFuzzyComp(imag(dcr1), 0.0) == 0) {
	    adRoots[0] = real(dcr1);
#ifndef NDEBUG
	    qFound = true;
#endif
	  }
								
	  if (iFuzzyComp(imag(dcr2), 0.0) == 0) {
	    adRoots[0] = real(dcr2);
#ifndef NDEBUG
	    qFound = true;
#endif
	  }
							
	  if (iFuzzyComp(imag(dcr3), 0.0) == 0) {
	    adRoots[0] = real(dcr3);
#ifndef NDEBUG
	    qFound = true;
#endif
	  }
							
	  assert2(qFound, "Could not find a real root");
							
	  adRoots[0] -= dp/3.0;
	}
	break;
	}
				
				
      }
      else {
				// Quadratic Equation
	double dRoot1, dRoot2;
	vSolveQuadratic(dB, dC, dD, iNumRoots, &dRoot1, &dRoot2);
	adRoots[0] = dRoot1;
	adRoots[1] = dRoot2;
      }
    }
    else {
      // Linear equation
      *iNumRoots = 1;
      adRoots[0] = -dD / dC;
    }
  }
  else {
    // There are no roots
    *iNumRoots = 0;
  }
	
} 						


void BdryCubicParam2D::
vSolveQuadratic(const double dA, const double dB, const double dC,
		int *iNumRoots, double *dRoot1, double *dRoot2) const
{
  // Find the roots for the quadratic dA x*x + dB x + dC == 0

  // First find the order of the equation
  int iOrder = 2;
  double dRadical;
	
  *dRoot1 = 0.0;
  *dRoot2 = 0.0;
	
  if (iFuzzyComp(dA, 0.0) == 0) {
    iOrder--;
    if (iFuzzyComp(dB, 0.0) == 0) {
      iOrder--;
    }
  }	
	
  if (iOrder >= 1) {
    if (iOrder == 2) {
      // Really a quadratic equation

      // Calculate stuff under radical first
      dRadical = dB*dB - 4.0*dA*dC;
			
      if (iFuzzyComp(dRadical, 0.0) == -1) {
				// Stuff under radical is negative -> no roots
	*iNumRoots = 0;
      }
      else {
	if (iFuzzyComp(dRadical, 0.0) == 0) {
	  // Stuff under radical is 0, only one root
	  *iNumRoots = 1;
	  *dRoot1 = - dB / (2.0*dA);		
	}
	else {
	  // Stuff under radical is positive -> two roots
	  *iNumRoots = 2;
	  *dRoot1 = (-dB + sqrt(dRadical))/(2.0*dA);
	  *dRoot2 = (-dB - sqrt(dRadical))/(2.0*dA);
	}
      }												
    }
    else {
      // Linear equation
      *iNumRoots = 1;
      *dRoot1 = -dC / dB;
    }
  }
  else {
    // There are no roots
    *iNumRoots = 0;
  }
	
} 						


static bool
qRootCanBeAdded(double dRoot, List<double> LdList) {

  // Go through LdList, make sure dRoot is not in there
  // If not, insert.....
	
  bool qFound = false;
	
  for (int i = 0; i < LdList.iLength(); i++) {
    double dItem = LdList[i];
    if (iFuzzyComp(dItem, dRoot) == 0) qFound = true;
  }
  return !qFound;
}

void BdryCubicParam2D::
vCalculateRoots()
{
  // Find all the roots for the first and second derivatives of the x and y eqns
  // x'  = 3 ax t^2 + 2 bx t + cx == 0
  // x'' = 6 ax t + 2 bx          == 0
  // Also find the point where the curvature goes exactly to zero.
  // Then, order the roots in LdOrderedRoots

  int iNumRoots;
		
  iXNumFirstRoots = 0;
  iYNumFirstRoots = 0;
  iXNumSecondRoots = 0;
  iYNumSecondRoots = 0;
  int iNumCurvRoots = 0;
	
	
  double dR;
  double dR1, dR2;
  double adCurvRoots[2];
	
	// x'' and y'' are easy...
	// x first..
  if (iEqnTypeX < 3) {
    // Equation was not cubic...
    // There are no second roots
  }
  else {
    // Root is -bx/(3*ax)
    dR = -adCoeffs[0][1]/(3.0*adCoeffs[0][0]);
		
    if ( (iFuzzyComp(dR, 0.0) >= 0) && (iFuzzyComp(dR, 1.0) <= 0) ) {
      // Root is between 0..1, so it is "valid" for our purposes
      if (dR < 0.0) dR = 0.0;
      if (dR > 1.0) dR = 1.0;
			
      iXNumSecondRoots++;
      adSecondRoots[0] = dR;
    }
  }

  // y now..
  if (iEqnTypeY < 3) {
    // There are no second roots
  }
  else {
    // Root is -by/(3*ay)
    dR = -adCoeffs[1][1]/(3.0*adCoeffs[1][0]);

    if ( (iFuzzyComp(dR, 0.0) >= 0) && (iFuzzyComp(dR, 1.0) <= 0) ) {
      // Root is between 0..1, so it is "valid" for our purposes
      if (dR < 0.0) dR = 0.0;
      if (dR > 1.0) dR = 1.0;
      iYNumSecondRoots++;
      adSecondRoots[1] = dR;
    }
				
  }
	
  // First derivative is a little bit more tricky...
	
	// x first..	
  if (iEqnTypeX >= 2) {
    // Find roots	
    vSolveQuadratic(3.0*adCoeffs[0][0], 2.0*adCoeffs[0][1], adCoeffs[0][2],
		    &iNumRoots, &dR1, &dR2);

    if (iNumRoots >= 1) {																				
      if ( (iFuzzyComp(dR1, 0.0) >= 0) && (iFuzzyComp(dR1, 1.0) <= 0) ) {
				// Root is between 0..1, so it is "valid" for our purposes
	if (dR1 < 0.0) dR1 = 0.0;
	if (dR1 > 1.0) dR1 = 1.0;
	adFirstRoots[0][iXNumFirstRoots++] = dR1;
      }
      if (iNumRoots == 2) {
	if ( (iFuzzyComp(dR2, 0.0) >= 0) && (iFuzzyComp(dR2, 1.0) <= 0) ) {
	  // Root is between 0..1, so it is "valid" for our purposes
	  if (dR2 < 0.0) dR2 = 0.0;
	  if (dR2 > 1.0) dR2 = 1.0;
	  adFirstRoots[0][iXNumFirstRoots++] = dR2;
	}			
      }
    }												
  }

  // Now do y
  if (iEqnTypeY >= 2) {
    // Find roots	
    vSolveQuadratic(3.0*adCoeffs[1][0], 2.0*adCoeffs[1][1], adCoeffs[1][2],
		    &iNumRoots, &dR1, &dR2);

    if (iNumRoots >= 1) {																				
      if ( (iFuzzyComp(dR1, 0.0) >= 0) && (iFuzzyComp(dR1, 1.0) <= 0) ) {
				// Root is between 0..1, so it is "valid" for our purposes
	if (dR1 < 0.0) dR1 = 0.0;
	if (dR1 > 1.0) dR1 = 1.0;				
	adFirstRoots[1][iYNumFirstRoots++] = dR1;
      }
      if (iNumRoots == 2) {
	if ( (iFuzzyComp(dR2, 0.0) >= 0) && (iFuzzyComp(dR2, 1.0) <= 0) ) {
	  // Root is between 0..1, so it is "valid" for our purposes
	  if (dR2 < 0.0) dR2 = 0.0;
	  if (dR2 > 1.0) dR2 = 1.0;
	  adFirstRoots[1][iYNumFirstRoots++] = dR2;
	}			
      }
    }			
  }	
		
  // Now look at the points where the curvature is exactly zero.
  // The expression for curvature is a quadratic -- possibly two
  // roots
  // Eqn is: 6*(bx*ay - ax*by)*t^2 + 6*(cx*ay - ax*cy)*t + 2*(cx*by - bx*cy)
	
  double d2coeff, d1coeff, d0coeff;
	
  d2coeff = 6.0*(adCoeffs[0][1]*adCoeffs[1][0] - adCoeffs[0][0]*adCoeffs[1][1]);
  d1coeff = 6.0*(adCoeffs[0][2]*adCoeffs[1][0] - adCoeffs[0][0]*adCoeffs[1][2]);
  d0coeff = 2.0*(adCoeffs[0][2]*adCoeffs[1][1] - adCoeffs[0][1]*adCoeffs[1][2]);
	
  vSolveQuadratic(d2coeff, d1coeff, d0coeff, &iNumRoots, &dR1, &dR2);
	
  if (iNumRoots >= 1) {																				
    if ( (iFuzzyComp(dR1, 0.0) >= 0) && (iFuzzyComp(dR1, 1.0) <= 0) ) {
      // Root is between 0..1, so it is "valid" for our purposes
      if (dR1 < 0.0) dR1 = 0.0;
      if (dR1 > 1.0) dR1 = 1.0;				
      adCurvRoots[iNumCurvRoots++] = dR1;
    }
    if (iNumRoots == 2) {
      if ( (iFuzzyComp(dR2, 0.0) >= 0) && (iFuzzyComp(dR2, 1.0) <= 0) ) {
				// Root is between 0..1, so it is "valid" for our purposes
	if (dR2 < 0.0) dR2 = 0.0;
	if (dR2 > 1.0) dR2 = 1.0;
	adCurvRoots[iNumCurvRoots++] = dR2;
      }			
    }
  }			
	
	
  // Order the roots
  LdOrderedRoots.vClear();
  List<double> LdOtherList;
  LdOtherList.vClear();
  // Add them first
  if (iXNumSecondRoots > 0) if (qRootCanBeAdded(adSecondRoots[0], LdOtherList)) LdOtherList.vAppendItem(adSecondRoots[0]);
  if (iYNumSecondRoots > 0) if (qRootCanBeAdded(adSecondRoots[1], LdOtherList)) LdOtherList.vAppendItem(adSecondRoots[1]);
	
  int i;
  for (i = 0; i < iXNumFirstRoots; i++) {
    if (qRootCanBeAdded(adFirstRoots[0][i], LdOtherList)) LdOtherList.vAppendItem(adFirstRoots[0][i]);
  }
  for (i = 0; i < iYNumFirstRoots; i++) {
    if (qRootCanBeAdded(adFirstRoots[1][i], LdOtherList)) LdOtherList.vAppendItem(adFirstRoots[1][i]);
  }
  for (i = 0; i < iNumCurvRoots; i++) {
    if (qRootCanBeAdded(adCurvRoots[i], LdOtherList)) LdOtherList.vAppendItem(adCurvRoots[i]);
  }

	
  //	vMessage(1, "Unordered roots\n");
  //	for (int i = 0; i < LdOtherList.iLength(); i++) {
  //		vMessage(1, "   %f\n", LdOtherList[i]);
  //	}

	
  double dMinRoot;
  iNumRoots = LdOtherList.iLength();
	
  while (LdOrderedRoots.iLength() != iNumRoots) {
    dMinRoot = 2.0;
    for (i = 0; i < LdOtherList.iLength(); i++) {
      // Find smallest root, insert it in LdOrderedRoots
      if (LdOtherList[i] < dMinRoot) dMinRoot = LdOtherList[i];
    }
    LdOrderedRoots.vAppendItem(dMinRoot);
    // Delete from old list
    LdOtherList.vDelete(dMinRoot);
  }

  //	vMessage(1, "Ordered roots for cubic patch\n");
  //	for (int i = 0; i < LdOrderedRoots.iLength(); i++) {
  //		vMessage(1, "   %f\n", LdOrderedRoots[i]);
  //	}
	
		
}


double BdryCubicParam2D::
dOrientationChangeAtParameter(const double dt)  const
{
  // We already have all of the possible roots of the curve...
  // All that is needed is to find the orientation of the curve at
  // each root, and take the absolute change between each two roots.
	
  // Start at end, list is ordered in decreasing order
	
  assert ( (dt >= 0.0) && (dt <= 1.0) );

  // Find which roots are before the value of dt
  int iRootUsed = LdOrderedRoots.iLength()-1;

  if (iRootUsed > -1) {	
    if (dt > LdOrderedRoots[0]) {
      while ( (dt < LdOrderedRoots[iRootUsed]) &&
	      (iRootUsed > 0) ) iRootUsed--;
    }
    else {
      iRootUsed = -1;
    }
  }
	
  double dTotalChange;
  double adTan[2];
  double dPrevOr, dNewOr;
  double dDiff;
	
	
  if (iRootUsed >= 0) {
    // Starting point
    dTotalChange = LdOrientationChangeAtRoots[iRootUsed];
	
		// Get what the orientation was at that last root
    vTangentAtParameterRatio(LdOrderedRoots[iRootUsed], adTan);
  }
  else {
    dTotalChange = 0.0;
    vTangentAtParameterRatio(0.0, adTan);
  }

  dPrevOr = atan2(adTan[1], adTan[0]);	
  // Calculate remaining part	
  vTangentAtParameterRatio(dt, adTan);
  dNewOr = atan2(adTan[1], adTan[0]);
		
  dDiff = fabs(dNewOr-dPrevOr);
  if (dDiff > M_PI) dDiff = 2.0*M_PI - dDiff;
		
  dTotalChange += dDiff;

  return dTotalChange;
			
}

void BdryCubicParam2D::
vCalculateOrientationChange()
{
  // We already have all of the possible roots of the curve...
  // All that is needed is to find the orientation of the curve at
  // each root, and take the absolute change between each two roots.
	
  double dTotalChange = 0.0;
  double adTan[2];
  double dPrevOr, dNewOr;
  double dDiff;
  double dNewt;
	
  LdOrientationChangeAtRoots.vClear();
	
  // Get orientation at t=0.0;
	
  vTangentAtParameterRatio(0.0, adTan);
  dPrevOr = atan2(adTan[1], adTan[0]);
	
	// Calculate orientation change at roots...
  for (int i = 0; i < LdOrderedRoots.iLength(); i++) {
    dNewt = LdOrderedRoots[i];
    vTangentAtParameterRatio(dNewt, adTan);
    dNewOr = atan2(adTan[1], adTan[0]);
			
    dDiff = fabs(dNewOr-dPrevOr);
    if (dDiff > M_PI) dDiff = 2.0*M_PI - dDiff;
		
    dTotalChange += dDiff;
		
    // Save the orientation change at each root too...		
    LdOrientationChangeAtRoots.vAppendItem(dTotalChange);
		
    dPrevOr = dNewOr;
  }
	
  // Calculate remaining part
	
  vTangentAtParameterRatio(1.0, adTan);
  dNewOr = atan2(adTan[1], adTan[0]);
			
  dDiff = fabs(dNewOr-dPrevOr);
  if (dDiff > M_PI) dDiff = 2.0*M_PI - dDiff;
		
  dTotalChange += dDiff;

  // Total orientation change is the following value
  dOrientationChange = dTotalChange;

  //	dOrientationChange = dOrientationChangeAtParameter(1.0);
  //	vMessage(3, "Total orientation change for cubic: %f\n", dOrientationChange);	
  //	vMessage(3, "Compare with old method: %f\n", dOrientationChangeAtParameter(1.0));
}

double BdryCubicParam2D::
dCurvature(const double adLoc[2]) const
{
  // Get the curvature here...
  double dt;
  vParameterClosestToPoint(adLoc, &dt);
	
  // K =  | vec(v) x vec(a) | / ( |v| )^3
	
	// Velocity vector is the first derivative...
	// 3*a*t^2 + 2*b*t + c
  double dVVec[2];
  dVVec[0] = 3.0*adCoeffs[0][0]*dt*dt + 2.0*adCoeffs[0][1]*dt + adCoeffs[0][2];
  dVVec[1] = 3.0*adCoeffs[1][0]*dt*dt + 2.0*adCoeffs[1][1]*dt + adCoeffs[1][2];

  // Acceleration vector is the second derivative...
  // 6*a*t + 2*b
  double dAVec[2];
  dAVec[0] = 6.0*adCoeffs[0][0]*dt + 2.0*adCoeffs[0][1];
  dAVec[1] = 6.0*adCoeffs[1][0]*dt + 2.0*adCoeffs[1][1];
	
	// Magnitude of V ...
  double dMag = hypot(dVVec[0], dVVec[1]);
	
  double dCross = dVVec[0]*dAVec[1] - dVVec[1]*dAVec[0];

  double dCurv = 0.0;
	
  if (iFuzzyComp(fabs(dCross), 0.0) != 0) {	
    dCurv = fabs(dCross) / (dMag*dMag*dMag);
  }
  return dCurv;
}


double BdryCubicParam2D::
dParameterAtPoint(const double adLoc[2]) const
{
  // Basically have to solve two cubic equations here.....
  // i.e. adLoc[0] == Ax t^3 + Bx t^2 + Cx t + Dx
  //   ---> Possibly get three values of t here
  // and  adLoc[1] == Ay t^3 + By t^2 + Cy t + Dy
  //   ---> Get three other values of t here
  // and then find the value of t that is present in both sets

  double drx[3]; // Roots for x-eqn
  double dry[3]; // Roots for y-eqn
  int iRootX, iRootY;
  double droots[3]; // The ones that match...
  int iFound = 0;
  double adPtB[2];
  double adPtE[2];
	
  // Get begin point
  vGetBeginPoint(adPtB);
	
  // Check endpoints first
  if ( (iFuzzyComp(adLoc[0], adPtB[0]) == 0) &&
       (iFuzzyComp(adLoc[1], adPtB[1]) == 0) ) {
    // That's the point
    return 0.0;	
  }

  // Get end point
  vGetEndPoint(adPtE);
	
  // Check endpoints first
  if ( (iFuzzyComp(adLoc[0], adPtE[0]) == 0) &&
       (iFuzzyComp(adLoc[1], adPtE[1]) == 0) ) {
    // That's the point
    return 1.0;	
  }
	
		
  // Solve for x...
  vSolveCubic(adCoeffs[0][0], adCoeffs[0][1], adCoeffs[0][2],
	      adCoeffs[0][3] - adLoc[0], &iRootX, drx); 
  // Solve for y....							
  vSolveCubic(adCoeffs[1][0], adCoeffs[1][1], adCoeffs[1][2],
	      adCoeffs[1][3] - adLoc[1], &iRootY, dry); 
  int i;
  for (i = 0; i < iRootX; i++) {
    for (int j = 0; j < iRootY; j++) {
				// Check is a bit complicated
				// Are they the same?
      if (iFuzzyComp(drx[i], dry[j]) == 0) {
	// If so, then we've found a root...
	droots[iFound++] = drx[i];
      }
      else {
	double dDiff = fabs(drx[i] - dry[j]);
	// Not the same, but are they close?
	if (iFuzzyComp(dDiff, 1e-8) <= 0) {
	  // The roots are close. Check if either root works out to
	  // yield the right answer in the OTHER equation
	  double dxanswer, dyanswer;
	  double dry3, dry2, drx3, drx2;
	  drx2 = drx[i]*drx[i];
	  drx3 = drx[i]*drx2;
	  dry2 = dry[j]*dry[j];
	  dry3 = dry[j]*dry2;
	  dxanswer = adCoeffs[0][0]*dry3 + adCoeffs[0][1]*dry2 + adCoeffs[0][2]*dry[j] + adCoeffs[0][3];
	  dyanswer = adCoeffs[1][0]*drx3 + adCoeffs[1][1]*drx2 + adCoeffs[1][2]*drx[i] + adCoeffs[1][3];
						
	  if (iFuzzyComp(dxanswer, adLoc[0]) == 0) {
	    // Discrepancy is because the cubic returns a similar value for a slightly different root
	    droots[iFound++] = dry[j];
	  }
	  else {
	    if (iFuzzyComp(dyanswer, adLoc[1]) == 0) {
	      droots[iFound++] = drx[i];
	    }
	  }
	} // if iFuzzyComp(dDiff, 1e-8) <= 0)
      }
    }
  }
	
  for (i = 0; i < iFound; i++) {
    // Check to make sure it's between 0 and 1...
    if ( (iFuzzyComp(droots[i], -1e-8) >= 0) && (iFuzzyComp(droots[i], 1.0 + 1e-8) <= 0)  ) {
      //		if ( (iFuzzyComp(droots[i], 0.0) >= 0) && (iFuzzyComp(droots[i], 1.0) <= 0)  ) {
      if (droots[i] < 0.0) droots[i] = 0.0;
      if (droots[i] > 1.0) droots[i] = 1.0;
      return droots[i];
    }
  }
	
  assert2(0, "BdryCubicParam2D::dParameterAtPoint() -- Could not find matching roots");
  // An illegal return value, which should hopefully cause
  // the caller to die abruptly rather than lingeringly.
  return(LARGE_DBL);
}


void BdryCubicParam2D::
vTangentAtParameterRatio(const double dRatio, double a2dTangent[2]) const
{
  // dRatio is between 0 and 1 and is t for parametric curves
  assert ( (dRatio >= 0.0) && (dRatio <= 1.0) );
  double dt, dt2;
  dt = dRatio;
  dt2 = dt*dt;
  // Tangent is easily calculated
  a2dTangent[0] = 3*adCoeffs[0][0]*dt2 + 2*adCoeffs[0][1]*dt + adCoeffs[0][2];
  a2dTangent[1] = 3*adCoeffs[1][0]*dt2 + 2*adCoeffs[1][1]*dt + adCoeffs[1][2];
		
	// Make it unit tangent...
  double dMag = hypot(a2dTangent[0], a2dTangent[1]);
	
  if (iFuzzyComp(dMag, 0.0) == 0) {
    // Use the second derivatives instead
    a2dTangent[0] = 6*adCoeffs[0][0]*dt + 2*adCoeffs[0][1];
    a2dTangent[1] = 6*adCoeffs[1][0]*dt + 2*adCoeffs[1][1];
    if (iFuzzyComp(dRatio, 0.5) > 0) {
      a2dTangent[0] = -a2dTangent[0];
      a2dTangent[1] = -a2dTangent[1];
    }
    dMag = hypot(a2dTangent[0], a2dTangent[1]);
    assert(dMag > 0.0);
  }
	
  a2dTangent[0] /= dMag;
  a2dTangent[1] /= dMag;

}


void BdryCubicParam2D::
vUnitNormal(const double adLoc[], double adNorm[]) const
{
  // Returns the normal at location adLoc[].
  double dParam = dParameterAtPoint(adLoc);

  vNormalAtParameterRatio(dParam, adNorm);
}

void BdryCubicParam2D::
vNormalAtParameterRatio(const double dRatio, double adNorm[2]) const
{
  double a2dTan[2];

  // Get the tangent vector..
  vTangentAtParameterRatio(dRatio, a2dTan);

  // Transform this into a normal vector pointing towards the region
  // OR in the direction that would yield a positive cross-product with
  // the tangent

  int iSide = iRegionWhichSide();

  if ( (iSide == REGION_SIDE_BOTH) || (iSide == REGION_SIDE_LEFT) ) {
    adNorm[0] = -a2dTan[1];
    adNorm[1] =  a2dTan[0];
  } else {
    // region is to the right
    adNorm[0] = a2dTan[1];
    adNorm[1] =-a2dTan[0];
  }
  // done
}

void BdryCubicParam2D::
vRefresh ()
{
  // Make sure the pre-calculated items are up-to-date...
  vDetermineEqnTypes();
  vCalculateRoots();
  vCalculateOrientationChange();
  // Make sure the length is computed properly...
  dMaxParam = 1.0;
}

void BdryCubicParam2D::
vPointAtParameterRatio (const double dRatio, double adLoc[2]) const
{
  // Return the point located at the ratio between pVBegin and pVEnd..
  // Ratio must be between 0 and 1 ...

  // Make sure ratio is correct..
  assert ((dRatio >= 0.0) && (dRatio <= 1.0));

  double dt, dt2, dt3;
  dt = dRatio;
  dt2 = dt*dt;
  dt3 = dt2*dt;

  // Pretty simple with a parametric curve..
  adLoc[0] = adCoeffs[0][0]*dt3 + adCoeffs[0][1]*dt2 + adCoeffs[0][2]*dt + adCoeffs[0][3];
  adLoc[1] = adCoeffs[1][0]*dt3 + adCoeffs[1][1]*dt2 + adCoeffs[1][2]*dt + adCoeffs[1][3];

}


void BdryCubicParam2D::
vPointAtDistFromStart(const double dDist, double adLoc[2]) const {
  // Find a point located at cartesian distance dDist from the beginning of the
  // curve.
  // Complicated problem for a cubic curve.
  //
  // Strategy:
  // - Find the point on a circle of radius dDist centered at the beginning of
  //   the curve that is at angle equal to the tangent at the beginning of the
  //   curve
  // - Point chosen throughout the algorithm are always on that circle of radius
  //   dDist. Only the angle changes.
  // - Figure on which side of the curve the newly found point resides.
  // - Find a point that is located on the other side of the curve.
  // - Iterate from there to find the intersection between the circle and
  //   the cubic curve.
  //
  double adCirclePt1[2], adCirclePt2[2];
  double adCenter[2];
  double adTan[2];
  double dDist1, dDist2 = LARGE_DBL;
  double dAngle1, dAngle2;
  int    iSide1, iSide2;
	
  // Center is pV0
  vGetBeginPoint(adCenter);
	
  // Initial angle is the tangent angle..	
  vTangentAtParameterRatio(0.0, adTan);
  dAngle1 = atan2(adTan[1], adTan[0]);
	
  // Find initial point
  adCirclePt1[0] = adCenter[0] + dDist*cos(dAngle1);
  adCirclePt1[1] = adCenter[1] + dDist*sin(dAngle1);
	
  // Where is that point?
  dDist1 = dDistFromBdry(adCirclePt1, &iSide1);
	
  // Hey, maybe we landed on it by luck
  if (iFuzzyComp(dDist1, 0.0) == 0) {
    // Cool.
    adLoc[0] = adCirclePt1[0];
    adLoc[1] = adCirclePt1[1];
    return;		
  }
  else {
    // this is most likely
    // Find a point on the other side now...
    bool qDirection = true;
    bool qJustChanged = false;
		
    iSide2 = iSide1;
			
    if (qDirection) {
      dAngle2 = dAngle1 + 5.0*M_PI/180.0;
    } else {
      dAngle2 = dAngle1 - 5.0*M_PI/180.0;
    }
		
    while (iSide1 == iSide2) {						
      // Find initial point
      adCirclePt2[0] = adCenter[0] + dDist*cos(dAngle2);
      adCirclePt2[1] = adCenter[1] + dDist*sin(dAngle2);			
      // Where is that point?
      dDist2 = dDistFromBdry(adCirclePt2, &iSide2);
		
      if (iFuzzyComp(dDist2, 0.0) == 0) {
				// Cool again
	adLoc[0] = adCirclePt2[0];
	adLoc[1] = adCirclePt2[1];
	return;
      }
			
      if (iSide1 == iSide2) {
	if (dDist2 > dDist1) {
	  qDirection = !qDirection;
	  qJustChanged = true;
	}
      }
			
      if (qDirection) {
	dAngle2 = dAngle2 + 5.0*M_PI/180.0;
      }
      else {
	if (qJustChanged) {
	  dAngle2 = dAngle2 - 10.0*M_PI/180.0;
	  qJustChanged = false;
	} else {
	  dAngle2 = dAngle2 - 5.0*M_PI/180.0;
	}				
      }			
    } // while iSide1 == iSide2
		
    // We now have dAngle1 and dAngle2 and they are located on either
    // side of the curve. Need to pinpoint the location of the curve..
    double dMidAngle, dMidDist, adMidPoint[2];
    int iMidSide;		
    while (iFuzzyComp(dAngle1-dAngle2, 0.0) != 0) {
      // Find a new angle..
      double dRatio = dDist1/(dDist1 + dDist2);
			
      if (dRatio > 0.9999) {
	dAngle1 = dAngle1 + 0.999*(dAngle2 - dAngle1);
	dRatio = 0.5;
      }
			
      if (dRatio < 0.0001) {
	dAngle2 = dAngle1 + 0.001*(dAngle2 - dAngle1);
	dRatio = 0.5;
      }
			
      dMidAngle = dAngle1 + dRatio*(dAngle2 - dAngle1);
      // Find point
      adMidPoint[0] = adCenter[0] + dDist*cos(dMidAngle);
      adMidPoint[1] = adCenter[1] + dDist*sin(dMidAngle);
      // Find where it is..
      dMidDist = dDistFromBdry(adMidPoint, &iMidSide);
			
      // Check the side..
      if (iMidSide == iSide1) {
	dAngle1 = dMidAngle;
	dDist1 = dMidDist;
      }
			
      if (iMidSide == iSide2) {
	dAngle2 = dMidAngle;
	dDist2 = dMidDist;
      }
			
      if (iMidSide == SIDE_UNDETERMINED) {
				// Pretty much hit the jackpot I think
	if (iFuzzyComp(dMidDist, 0.0) == 0) {
	  dAngle1 = dMidAngle;
	  dAngle2 = dMidAngle;
	}
      }
    } // While angles are different
		
    // The angles are the same now..
    adLoc[0] = adCenter[0] + dDist*cos(dAngle1);
    adLoc[1] = adCenter[1] + dDist*sin(dAngle1);
    return;
  }

}

void BdryCubicParam2D::
vPointAtDistFromEnd(const double dDist, double adLoc[2]) const {
  // Find a point located at cartesian distance dDist from the beginning of the
  // curve.
  // Complicated problem for a cubic curve.
  //
  // Strategy:
  // - Find the point on a circle of radius dDist centered at the beginning of
  //   the curve that is at angle equal to the tangent at the beginning of the
  //   curve
  // - Point chosen throughout the algorithm are always on that circle of radius
  //   dDist. Only the angle changes.
  // - Figure on which side of the curve the newly found point resides.
  // - Find a point that is located on the other side of the curve.
  // - Iterate from there to find the intersection between the circle and
  //   the cubic curve.
  //
  double adCirclePt1[2], adCirclePt2[2];
  double adCenter[2];
  double adTan[2];
  double dDist1, dDist2 = LARGE_DBL;
  double dAngle1, dAngle2;
  int    iSide1, iSide2;
	
  // Center is pV1
  vGetEndPoint(adCenter);
		
  // Initial angle is in the direction of the tangent angle..	
  vTangentAtParameterRatio(1.0, adTan);
  dAngle1 = atan2(-adTan[1], -adTan[0]);
	
  // Find initial point
  adCirclePt1[0] = adCenter[0] + dDist*cos(dAngle1);
  adCirclePt1[1] = adCenter[1] + dDist*sin(dAngle1);
	
  // Where is that point?
  dDist1= dDistFromBdry(adCirclePt1, &iSide1);
	
  // Hey, maybe we landed on it by luck
  if (iFuzzyComp(dDist1, 0.0) == 0) {
    // Cool.
    adLoc[0] = adCirclePt1[0];
    adLoc[1] = adCirclePt1[1];
    return;		
  }
  else {
    // this is most likely
    // Find a point on the other side now...
    bool qDirection = true;
    bool qJustChanged = false;
		
    iSide2 = iSide1;
			
    if (qDirection) {
      dAngle2 = dAngle1 + 5.0*M_PI/180.0;
    } else {
      dAngle2 = dAngle1 - 5.0*M_PI/180.0;
    }
		
    while (iSide1 == iSide2) {						
      // Find initial point
      adCirclePt2[0] = adCenter[0] + dDist*cos(dAngle2);
      adCirclePt2[1] = adCenter[1] + dDist*sin(dAngle2);			
      // Where is that point?
      dDist2 = dDistFromBdry(adCirclePt2, &iSide2);		
		
      if (iFuzzyComp(dDist2, 0.0) == 0) {
				// Cool again
	adLoc[0] = adCirclePt2[0];
	adLoc[1] = adCirclePt2[1];
	return;
      }
			
      if (iSide1 == iSide2) {
	if (dDist2 > dDist1) {
	  qDirection = !qDirection;
	  qJustChanged = true;
	}
      }
			
      if (qDirection) {
	dAngle2 = dAngle2 + 5.0*M_PI/180.0;
      }
      else {
	if (qJustChanged) {
	  dAngle2 = dAngle2 - 10.0*M_PI/180.0;
	  qJustChanged = false;
	} else {
	  dAngle2 = dAngle2 - 5.0*M_PI/180.0;
	}				
      }			
    } // while iSide1 == iSide2
		
    // We now have dAngle1 and dAngle2 and they are located on either
    // side of the curve. Need to pinpoint the location of the curve..
    double dMidAngle, dMidDist, adMidPoint[2];
    int iMidSide;		
    while (iFuzzyComp(dAngle1-dAngle2, 0.0) != 0) {
      // Find a new angle..
      double dRatio = dDist1/(dDist1 + dDist2);
			
      if (dRatio > 0.9999) {
	dAngle1 = dAngle1 + 0.999*(dAngle2 - dAngle1);
	dRatio = 0.5;
      }
			
      if (dRatio < 0.0001) {
	dAngle2 = dAngle1 + 0.001*(dAngle2 - dAngle1);
	dRatio = 0.5;
      }
			
      dMidAngle = dAngle1 + dRatio*(dAngle2 - dAngle1);
      // Find point
      adMidPoint[0] = adCenter[0] + dDist*cos(dMidAngle);
      adMidPoint[1] = adCenter[1] + dDist*sin(dMidAngle);
      // Find where it is..
      dMidDist = dDistFromBdry(adMidPoint, &iMidSide);
			
      // Check the side..
      if (iMidSide == iSide1) {
	dAngle1 = dMidAngle;
	dDist1 = dMidDist;
      }
			
      if (iMidSide == iSide2) {
	dAngle2 = dMidAngle;
	dDist2 = dMidDist;
      }
			
      if (iMidSide == SIDE_UNDETERMINED) {
				// Pretty much hit the jackpot I think
	if (iFuzzyComp(dMidDist, 0.0) == 0) {
	  dAngle1 = dMidAngle;
	  dAngle2 = dMidAngle;
	}
      }
    } // While angles are different
		
    // The angles are the same now..
    adLoc[0] = adCenter[0] + dDist*cos(dAngle1);
    adLoc[1] = adCenter[1] + dDist*sin(dAngle1);
    return;
  }

}

void BdryCubicParam2D::
vParameterClosestToPoint(const double adLoc[2], double *dLen) const
{

  //
  // * Calculate the difference between the normal and the vector from the point to
  //   the location of the equation's roots (maxima, minima, inflexion points). Also
  //   store the distance
  //
  // * Can isolate the section in which the closest point will be this way. Then, a
  //   simple bisection algorithm can be used to pinpoint the exact location
  //

  List<double> LdExtRoots; // Roots that include end points
  List<double> LdDistances; // Distances to roots
  List<double> LdCross;      // Positive or negative orientation to root (-1, 1)
	
  double dIns = 0.0;
	
  LdExtRoots.vAddItem(dIns);
  int i;
  for (i = 0; i < LdOrderedRoots.iLength(); i++) {
    LdExtRoots.vAppendItem(LdOrderedRoots[i]);
  }
  dIns = 1.0;
  LdExtRoots.vAddItem(dIns);
	
  // Check every root
  for (i = 0; i < LdExtRoots.iLength(); i++) {
    // get point and normal for root
    double adPt[2];
    double adNorm[2];
    double adVec[2];
    double dDistance;
    vPointAtParameterRatio(LdExtRoots[i], adPt);
    vNormalAtParameterRatio(LdExtRoots[i], adNorm);		
    // Get distance
    dDistance = dDIST2D(adPt, adLoc);
    LdDistances.vAppendItem(dDistance);
    // Get vector to point
    adVec[0] = adPt[0] - adLoc[0];
    adVec[1] = adPt[1] - adLoc[1];
    // Calculate cross product
    double dCross = adVec[0]*adNorm[1] - adVec[1]*adNorm[0];
    LdCross.vAppendItem(dCross);
  }

  // Check the cross products. We want two consecutive cross products that have a
  // different sign. Check at the same time for ZERO cross product, which indicates
  // that the point is already on the boundary
	
  List<int> LiIntervals; // Stores the intervals we will have to examine later
  // First interval is between points 0 and 1, etc...
  bool qFoundPoint = false;
  double dSmallestDist = 1E15;
  double dC0, dC1;
  int    iC0, iC1;
	
  for (i = 0; i < LdExtRoots.iLength()-1; i++) {
    dC0 = LdCross[i];
    dC1 = LdCross[i+1];
    iC0 = iFuzzyComp(dC0, 0.0);
    iC1 = iFuzzyComp(dC1, 0.0);
			
    if (iC0 == 0) {
      qFoundPoint = true;
      if (LdDistances[i] < dSmallestDist) {
	//					vPointAtParameterRatio(LdExtRoots[i], adPoint);
	*dLen = LdExtRoots[i];
	dSmallestDist = LdDistances[i];
      }				
    }
			
    if (iC1 == 0) {
      qFoundPoint = true;
      if (LdDistances[i+1] < dSmallestDist) {
	//					vPointAtParameterRatio(LdExtRoots[i+1], adPoint);
	*dLen = LdExtRoots[i+1];
	dSmallestDist = LdDistances[i+1];
      }				
    }
			
    if ( ((iC0 == 1) && (iC1 == -1)) || ((iC0 == -1) && (iC1 == 1)) ) {
				// If the signs of the cross products of the two neighbouring roots are different
				
				// Mark that as one we have to watch
      LiIntervals.vAppendItem(i);				
    }
						
  }
	
  if (!qFoundPoint) {
    // No need to do anything else if a point has been found already..
		
    if (LiIntervals.iLength() == 0) {
      // One of the two endpoints is to be chosen then...
      if (LdDistances[0] <= LdDistances[LdDistances.iLength()-1]) {
	//				vPointAtParameterRatio(LdExtRoots[0], adPoint);
	*dLen = LdExtRoots[0];
      }
      else {
	//				vPointAtParameterRatio(LdExtRoots[LdExtRoots.iLength()-1], adPoint);
	*dLen = LdExtRoots[LdExtRoots.iLength()-1];
      }
    }
    else {
      dSmallestDist = 1E15;
      // We have at least one interval to investigate
      // Find the smallest distance for each one of them
      for (i = 0; i < LiIntervals.iLength(); i++) {
	double dDist;				
	double dTop = LdExtRoots[LiIntervals[i]];
	double dBot = LdExtRoots[LiIntervals[i]+1];
	double dMid = (dTop + dBot) / 2;
	double adMidPt[2], adTopPt[2], adBotPt[2];
	double adMidNorm[2], adTopNorm[2], adBotNorm[2];
	double adMidVec[2], adTopVec[2], adBotVec[2];
	double dBotCross, dTopCross, dMidCross;
	int    iBot, iTop, iMid; // Orientations
				
	dMidCross = 1.0;
				
	vPointAtParameterRatio(dTop, adTopPt);
	vPointAtParameterRatio(dBot, adBotPt);
				// find normals
	vNormalAtParameterRatio(dTop, adTopNorm);
	vNormalAtParameterRatio(dBot, adBotNorm);
				// Find vecs
	adBotVec[0] = adLoc[0] - adBotPt[0];
	adBotVec[1] = adLoc[1] - adBotPt[1];
	adTopVec[0] = adLoc[0] - adTopPt[0];
	adTopVec[1] = adLoc[1] - adTopPt[1];				
				// Find cross products
	dBotCross = adBotVec[0]*adBotNorm[1] - adBotVec[1]*adBotNorm[0];
	dTopCross = adTopVec[0]*adTopNorm[1] - adTopVec[1]*adTopNorm[0];
				// Find orientations
	iBot = iFuzzyComp(dBotCross, 0.0);
	iTop = iFuzzyComp(dTopCross, 0.0);
				
				// This should be true here
	assert(iBot != iTop);
				
	iMid = 1;
	//				int iLast = -1;
	//				int iNumLast = -1;
	double dRatio;
	int iNumTrips = 0;
				
	while (iFuzzyComp((dTop-dBot)*90.0, 0.0) != 0 &&
	       iNumTrips < 100) {
	  iNumTrips++;
	  // Find a better split point than just the middle
	  dRatio = (fabs(dBotCross))/(fabs(dBotCross)+fabs(dTopCross));
	  if (dRatio > 0.999) {
	    dBot = dBot + 0.99*(dTop-dBot);	
	    vPointAtParameterRatio(dBot, adBotPt);
	    vNormalAtParameterRatio(dBot, adBotNorm);
	    adBotVec[0] = adLoc[0] - adBotPt[0];
	    adBotVec[1] = adLoc[1] - adBotPt[1];
	    dBotCross = adBotVec[0]*adBotNorm[1] - adBotVec[1]*adBotNorm[0];
	    dRatio = 0.5;
	  }
	  if (dRatio < 0.001) {
	    dTop = dBot + 0.01*(dTop-dBot);
	    vPointAtParameterRatio(dTop, adTopPt);
	    vNormalAtParameterRatio(dTop, adTopNorm);
	    adTopVec[0] = adLoc[0] - adTopPt[0];
	    adTopVec[1] = adLoc[1] - adTopPt[1];
	    dTopCross = adTopVec[0]*adTopNorm[1] - adTopVec[1]*adTopNorm[0];
	    dRatio = 0.5;
	  }
	  dMid = dBot + dRatio*(dTop-dBot);
	  // Find point there
	  vPointAtParameterRatio(dMid, adMidPt);
	  // Find normal there
	  vNormalAtParameterRatio(dMid, adMidNorm);
	  // Find vec
	  adMidVec[0] = adLoc[0] - adMidPt[0];
	  adMidVec[1] = adLoc[1] - adMidPt[1];
	  // Find cross product
	  dMidCross = adMidVec[0]*adMidNorm[1] - adMidVec[1]*adMidNorm[0];
	  // Find orientation
	  iMid = iFuzzyComp(dMidCross/10, 0.0);
					
	  if (iMid == iTop) {
	    // Replace iTop by iMid
	    adTopVec[0] = adMidVec[0];
	    adTopVec[1] = adMidVec[1];
	    adTopPt[0] = adMidPt[0];
	    adTopPt[1] = adMidPt[1];
	    dTop = dMid;
	    dTopCross = dMidCross;
	    iTop = iMid;
	  }
					
	  if (iMid == iBot) {
	    // Replace iBot by iMid
	    adBotVec[0] = adMidVec[0];
	    adBotVec[1] = adMidVec[1];
	    adBotPt[0] = adMidPt[0];
	    adBotPt[1] = adMidPt[1];
	    dBot = dMid;
	    dBotCross = dMidCross;
	    iBot = iMid;
	  }
					
	  if (iMid == 0) {
	    // We're right on it...
	    dBot = dMid;
	    dTop = dMid;	
	  }
										
	} // while (iFuzzyComp(dTop-dBot, 0.0) != 0)
				
	dDist = dDIST2D(adMidPt, adLoc);
	if (dDist < dSmallestDist) {
	  dSmallestDist = dDist;
	  *dLen = dMid;
	}
				
      }	// For loop over all intervals			
			
    } // if number of intervals greater than 0
	
  } // if no points were found in the first loop



}


void BdryCubicParam2D::
vBdryPointClosestToPoint(const double adLoc[2], double adPoint[2]) const
{

  // Find closest parameter
  double dLen;
  vParameterClosestToPoint(adLoc, &dLen);
  vPointAtParameterRatio(dLen, adPoint);
}

double BdryCubicParam2D::
dDistFromBdry(const double adLoc[2], int * const piSide) const {
	
  double adPt[2], dCross, dX, dY, adTan[2], dLen;	
  
  // Find closest location first
  vParameterClosestToPoint(adLoc, &dLen);
  vPointAtParameterRatio(dLen, adPt);
	
  // Get the tangent there..
  // Using the tangent, and a cross product, it's easy to know on
  // which side of the curve the point is...
  vTangentAtParameterRatio(dLen, adTan);

  // Calculate distance..
  dX = adPt[0] - adLoc[0];
  dY = adPt[1] - adLoc[1];

  // Determine on which side of the curve the point lies
  // Use cross-product
  dCross = adTan[0]*dY - adTan[1]*dX;

  double dDist = hypot(dX, dY);

  int iCross = iFuzzyComp(dCross, 0.0);


  // -ve dot means opposite direction of normal
  if (piSide) {
    if (iCross < 0) {
      *piSide = SIDE_LEFT;
    }
    if (iCross > 0) {
      *piSide = SIDE_RIGHT;
    }
    
    if (iCross == 0) *piSide = SIDE_UNDETERMINED;
  }
  return dDist;
}

void BdryCubicParam2D::
vRatioBetweenTwoPoints(const double dRatio, const double adLoc1[2],
		       const double adLoc2[2], double adPoint[2]) const
{
  // - Find the parameter for the two points (dt1, dt2)
  // - Find the total orientation for each point (dOr1, dOr2)
  // - Find the point with orientation change dRatio*(dOr2-dOr1) starting from
  //   dOr1
	
  assert ( (dRatio > 0.0) && (dRatio < 1.0) );
	
  double dtdum; // dummy var	
  // Get parameters
  double dt1 = dParameterAtPoint(adLoc1);
  double dt2 = dParameterAtPoint(adLoc2);	
  // Order them properly
  if (dt1 > dt2) {
    dtdum = dt2;
    dt2 = dt1;
    dt1 = dtdum;
  }	
  // Find orientation changes
  double dOr1 = dOrientationChangeAtParameter(dt1);
  double dOr2 = dOrientationChangeAtParameter(dt2);	

  if (fabs(dOr2-dOr1) < 0.01) {
    dtdum = dt1 + dRatio*(dt2-dt1);
  }
  else {
    dtdum = dParameterAtOrientationChange(dRatio*fabs(dOr2-dOr1), dt1);				
  }
  // Get point
  assert( (dtdum >= dt1) && (dtdum <= dt2) );	
  vPointAtParameter(dtdum, adPoint);
  // done	
}											

double BdryCubicParam2D::
dXMax() {
  // The potential maximum points are at the endpoints or
  // at the location of the first derivative roots...
  double adPt[2];
  double dx1, dx2;
  double adB[2], adE[2];
  double dXB, dXE;
	
  vGetBeginPoint(adB);
  vGetEndPoint(adE);
	
  dXB = adB[0];
  dXE = adE[0];
	
  switch (iXNumFirstRoots) {
  case 0 : 
    return std::max(dXB, dXE);
  case 1 : 
    // Find point
    vPointAtParameterRatio(adFirstRoots[0][0], adPt);
    dx1 = adPt[0];
    return std::max(dx1, std::max(dXB, dXE));
  case 2 : 
    // Find points
    vPointAtParameterRatio(adFirstRoots[0][0], adPt);
    dx1 = adPt[0];
    vPointAtParameterRatio(adFirstRoots[0][1], adPt);
    dx2 = adPt[0];
    return std::max(std::max(dx1, dx2), std::max(dXB, dXE));
  default :
    assert(0);
    return(0); // Never returned in debug mode; any value unsafe.
  }
}

double BdryCubicParam2D::
dYMax() {
  // The potential maximum points are at the endpoints or
  // at the location of the first derivative roots...
  double adPt[2];
  double dy1, dy2;
  double dYB, dYE;
  double adB[2], adE[2];

  vGetBeginPoint(adB);
  vGetEndPoint(adE);
		
  dYB = adB[1];
  dYE = adE[1];
	
  switch (iYNumFirstRoots) {
  case 0 : 
    return std::max(dYB, dYE);
  case 1 : // Find point
    vPointAtParameterRatio(adFirstRoots[1][0], adPt);
    dy1 = adPt[1];
    return std::max(dy1, std::max(dYB, dYE));
  case 2 : // Find points
    vPointAtParameterRatio(adFirstRoots[1][0], adPt);
    dy1 = adPt[1];
    vPointAtParameterRatio(adFirstRoots[1][1], adPt);
    dy2 = adPt[1];
    return std::max(std::max(dy1, dy2), std::max(dYB, dYE));
  default :
    assert(0);
    return(0); // Never returned in debug mode; any value unsafe.
  }
}

double BdryCubicParam2D::
dXMin() {
  // The potential maximum points are at the endpoints or
  // at the location of the first derivative roots...
  double adPt[2];
  double dx1, dx2;
  double dXB, dXE;
  double adB[2], adE[2];
	
  vGetBeginPoint(adB);
  vGetEndPoint(adE);
	
  dXB = adB[0];
  dXE = adE[0];
	
  switch (iXNumFirstRoots) {
  case 0 : 
    return std::min(dXB, dXE);
  case 1 : // Find point
    vPointAtParameterRatio(adFirstRoots[0][0], adPt);
    dx1 = adPt[0];
    return std::min(dx1, std::min(dXB, dXE));
  case 2 : // Find points
    vPointAtParameterRatio(adFirstRoots[0][0], adPt);
    dx1 = adPt[0];
    vPointAtParameterRatio(adFirstRoots[0][1], adPt);
    dx2 = adPt[0];
    return std::min(std::min(dx1, dx2), std::min(dXB, dXE));
  default :
    assert(0);
    return(0); // Never returned in debug mode; any value unsafe.
  }
}

double BdryCubicParam2D::
dYMin() {
  // The potential maximum points are at the endpoints or
  // at the location of the first derivative roots...
  double adPt[2];
  double dy1, dy2;
  double dYB, dYE;
  double adB[2], adE[2];
	
  vGetBeginPoint(adB);
  vGetEndPoint(adE);
	
  dYB = adB[1];
  dYE = adE[1];
	
  switch (iYNumFirstRoots) {
  case 0 : 
    return std::min(dYB, dYE);
  case 1 : // Find point
    vPointAtParameterRatio(adFirstRoots[1][0], adPt);
    dy1 = adPt[1];
    return std::min(dy1, std::max(dYB, dYE));
  case 2 : // Find points
    vPointAtParameterRatio(adFirstRoots[1][0], adPt);
    dy1 = adPt[1];
    vPointAtParameterRatio(adFirstRoots[1][1], adPt);
    dy2 = adPt[1];
    return std::min(std::min(dy1, dy2), std::min(dYB, dYE));
  default :
    assert(0);
    return(0); // Never returned in debug mode; any value unsafe.
  }
}

double BdryCubicParam2D::
dGetOrientationChange()
{
  return dOrientationChange;
}


List<double> BdryCubicParam2D::
LdOriginalDiscretization(  						
			 bool *qInsertVertPointers,
			 bool *qCloseLoop										
			 )
{
  // Determine the original discretization of the patch.
  // Works like this:
  // - Determine the total (absolute) change in orientation of the patch
  // - Number of points to insert = (int)(total change in orientation)
  // - Find the proper locations, i.e. at every (change in orientation)/(numpoints+1)
  //   (that one will be interesting)
	
  int iNumPoints = 2 * static_cast<int>(floor(dOrientationChange));
  List<double> LdPointList;
  double adPt[2];
  double dPrevt = 0.0;
  double dNewt = 0.0;
	
  LdPointList.vClear();
	
	// In general, these are set this way...
  *qCloseLoop = false;
  *qInsertVertPointers = true;
	
	// Orientation change along each discretization segment
  double dChange = dOrientationChange/(iNumPoints + 1);
	
  // Find the other points in the middle
  for (int k = 1; k <= iNumPoints; k++) {
    // Get t
    dNewt = dParameterAtOrientationChange(dChange, dPrevt);		
    // Find point
    vPointAtParameterRatio(dNewt, adPt);
    // Insert in list
    LdPointList.vAppendItem(adPt[0]);		
    LdPointList.vAppendItem(adPt[1]);
    // Make sure to update dPrevt
    dPrevt = dNewt;
  }
	
  if ( iBeginPointIndex() == iEndPointIndex() ) {
    // The curve loops on itself
    *qCloseLoop = true;
  }
		
  return LdPointList;
}

bool BdryCubicParam2D::
qIsColinear(BdryPatch2D*, Vert* , int* ) {
  // iCase is modified this way:
  //    - iCase = 1:  If both patches are pointing in the same direction
  //    - iCase = 2:  If both patches are pointing towards the common vertex
  //    - iCase = 3:  If both patches are pointing towars the "other" vertex
  return false;
}

double BdryCubicParam2D::
dParameterAtOrientationChange(const double dChange, const double dStart) const
{
  // Find the value of parameter t for the location where the orientation of
  // the curve as changed value dChange from the value it for t = dStart.
  // The search is always in the positive direction (increasing t)
	
  // Find in which section of the curve (related to the location of the roots)
  // we are first located.
	
  assert( (iFuzzyComp(dStart, 0.0) >= 0) && (iFuzzyComp(dStart, 1.0) <= 0) );
	
  double adTan[2];
  List <double> LdFullList;
	
  LdFullList.vClear();
  // Build the list of roots to include the endpoints of the curve

  double dIns = 0.0;	
	
  if (LdOrderedRoots.iLength() > 0) {
    if (LdOrderedRoots[0] > 0.0) LdFullList.vAppendItem(dIns);
  }
  else {
    LdFullList.vAppendItem(dIns);
  }
	
  for (int i = 0; i < LdOrderedRoots.iLength(); i++) {
    LdFullList.vAppendItem(LdOrderedRoots[i]);
  }
	
  dIns = 1.0;
  if (LdOrderedRoots.iLength() > 0) {
    if (LdOrderedRoots[LdOrderedRoots.iLength()-1] < 1.0) LdFullList.vAppendItem(dIns);
  }
  else {
    LdFullList.vAppendItem(dIns);
  }
	
  // Get the orientation change at the starting point
  double dOrChangeAtStart = dOrientationChangeAtParameter(dStart);
	
  // Does the curve have enough orientation change from that point to the end to
  // satisfy the request?
  if ( (dOrientationChange - dOrChangeAtStart) <= dChange) {
    // return the endpoint then...
    return 1.0;
  }
	
  // Want to find the root that has orientation change bigger than the target
  // and the root that has orientation change smaller than the target
	
  // Find the smallest root bigger than dStart
  int iBiggerRoot = 0;
  while (dStart >= LdFullList[iBiggerRoot]) iBiggerRoot++;
	
  int iBiggerOrChangeRoot = iBiggerRoot;	
  while (iFuzzyComp(
		    dOrientationChangeAtParameter(LdFullList[iBiggerOrChangeRoot]) -
		    dOrChangeAtStart, dChange) < 0)
    iBiggerOrChangeRoot++;
	
  double dLower, dUpper, dNew;			
			
  if (iBiggerOrChangeRoot == iBiggerRoot) {
    dLower = dStart;
  }
  else {
    dLower = LdFullList[iBiggerOrChangeRoot-1];
  }
  dUpper = LdFullList[iBiggerOrChangeRoot];

  double dLowerOr, dUpperOr, dNewOr;
  double dLowerOrChange, dUpperOrChange;
		
	// Find orientations there
  vTangentAtParameterRatio(dLower, adTan);
  dLowerOr = atan2(adTan[1], adTan[0]);
  vTangentAtParameterRatio(dUpper, adTan);
  dUpperOr = atan2(adTan[1], adTan[0]);
	
	// Get the orientation changes there...
  dLowerOrChange = dOrientationChangeAtParameter(dLower);
  dUpperOrChange = dOrientationChangeAtParameter(dUpper);
	
	// This should be true
  int iCompLower = iFuzzyComp(dLowerOrChange-dOrChangeAtStart, dChange);
  int iCompUpper = iFuzzyComp(dUpperOrChange-dOrChangeAtStart, dChange);
	
  assert( iCompLower <= 0);
  assert( iCompUpper >= 0);
	
	// Check if we hit the jackpot
  if (iCompLower == 0) {
    return dLower;
  }
  if (iCompUpper == 0) {
    return dUpper;
  }
  //	dPrevOr = dStartOr;
	
/*	
	while ( (iFuzzyComp(dRootChangeOr, dChange) < 0) && (!qReachedEnd) ) {

		dOldRootChangeOr = dRootChangeOr;
				
		if (iPrevRoot == 0) {
			qReachedEnd = true;
		}	
	
		vTangentAtParameterRatio(LdFullList[iPrevRoot], adTan);
		iPrevRoot--;
		
		// Find the orientation there
		dNextOr = atan2(adTan[1], adTan[0]);
	
		// Calculate orientation change
		dDiff = fabs(dNextOr - dPrevOr);
		// adjust change in orientation
		if (dDiff > M_PI) dDiff = 2.0*M_PI-dDiff;
		// Add to total
		dRootChangeOr += dDiff;
		// Switch the orientations
		dPrevOr = dNextOr;
	} // while we haven't found the right place yet
	
	if (qReachedEnd) {
		// If we've reached the end of the curve, and we still don't have
		// enough change... well.. return the endpoint
		if (iFuzzyComp(dRootChangeOr, dChange) < 0) return 1.0;
	}
	
	// We now have an upper and lower limit...
	// Upper == LdOrderedRoots[iPrevRoot+1];
	// Lower is root just before that... or dStart, whichever is bigger
//	double dLower;
	
	if (iPrevRoot + 2 < LdFullList.iLength()) {
		dLower = LdFullList[iPrevRoot+2];
	}
	else {
		dLower = dStart;
	}
	
	if (iFuzzyComp(dLower, dStart) < 0) dLower = dStart;
	
	dUpper = LdFullList[iPrevRoot+1];
	
	double dNew;
	double dNewOr;
	
	// Basically can now start some sort of bisection algorithm...
	
	bool qFinished = false;
	double dOriginalLowerOr;
	
	// Remember the orientation at the original lower limit..
	// This is where we are comparing from.
	
	vTangentAtParameterRatio(dLower, adTan);
	dOriginalLowerOr = atan2(adTan[1], adTan[0]);
*/
	
  dNew = (dLower + dUpper)/2.0;
  //	dLowerOr = dOriginalLowerOr;
  //	vTangentAtParameterRatio(dUpper, adTan);
  //	dUpperOr = atan2(adTan[1], adTan[0]);
  double dTargetOr;
  double dAbsLowerOr, dAbsUpperOr, dAbsNewOr;
  double dChangeLeft = dOrChangeAtStart+dChange - dLowerOrChange;
  bool   qGoingPositive = true;
	
  dAbsLowerOr = dLowerOr;
  dAbsUpperOr = dUpperOr;
	
	// Find the target orientation then...
	
	// Put everything positive to calculate target
  if (iFuzzyComp(dLowerOr, 0.0) < 0) dAbsLowerOr += 2.0*M_PI;
  if (iFuzzyComp(dUpperOr, 0.0) < 0) dAbsUpperOr += 2.0*M_PI;
	
  if ( iFuzzyComp(dAbsUpperOr, dAbsLowerOr) > 0 ) {
    // Upper Orientation is greater than Lower orientation
    if (iFuzzyComp(dAbsUpperOr-dAbsLowerOr, M_PI) > 0) {
      // Upper is more than PI greater than lower -- use the other way around
      dTargetOr = dAbsLowerOr - dChangeLeft;
      qGoingPositive = false;
    }
    else {
      dTargetOr = dAbsLowerOr + dChangeLeft;
    }
		
  }
  else {
    // Lower orientation is greater than upper orientation
    if (iFuzzyComp(dAbsLowerOr-dAbsUpperOr, M_PI) > 0) {
      // Lower is more than PI greater dans Upper
      dTargetOr = dAbsLowerOr + dChangeLeft;
    }
    else {
      dTargetOr = dAbsLowerOr - dChangeLeft;
      qGoingPositive = false;
    }
  }
	
  double dAbsTargetOr = dTargetOr;
	
	// Put the target back in +ve and -ve values
  if (iFuzzyComp(dTargetOr, M_PI) > 0) dTargetOr -= 2.0*M_PI;
  int iResult = 1;
	
  while ( iResult != 0) {
    // get orientation there
    vTangentAtParameterRatio(dNew, adTan);
    // Get angle
    dNewOr = atan2(adTan[1], adTan[0]);
    dAbsNewOr = dNewOr;
    if (iFuzzyComp(dNewOr, 0.0) < 0) dAbsNewOr += 2.0*M_PI;
			
    // Are we there yet?
    iResult = iFuzzyComp( (dTargetOr-dNewOr)*2.0, 0.0);
		
    if (iResult == 1) {
				// Target still higher
      if (qGoingPositive) {
	dLower = dNew;
	dAbsLowerOr = dAbsNewOr;
      }
      else {
	dUpper = dNew;
	dAbsUpperOr = dAbsNewOr;
      }
    }
		
    if (iResult == -1) {
      // Not enough change...
      if (qGoingPositive) {
	dUpper = dNew;
	dAbsUpperOr = dAbsNewOr;
      }
      else {
	dLower = dNew;
	dAbsLowerOr = dAbsNewOr;
      }
    }
		
    double dUpDiff, dLowDiff;
    double dRatio;
    dUpDiff = fabs(dAbsUpperOr - dAbsTargetOr);
    if (dUpDiff > M_PI) dUpDiff = 2.0*M_PI - dUpDiff;
    dLowDiff = fabs(dAbsLowerOr - dAbsTargetOr);
    if (dLowDiff > M_PI) dUpDiff = 2.0*M_PI - dUpDiff;
    dRatio = dLowDiff/(dUpDiff + dLowDiff);
		
    if (dRatio > 0.999) {
      // Much closer to dUpper than to dLower
      dLower = dLower + 0.5*(dUpper-dLower);
      dRatio = 0.5;
    }
    if (dRatio < 0.001) {
      // Much closer to dLower than to dUpper
      dUpper = dLower + 0.5*(dUpper-dLower);
      dRatio = 0.5;
    }
		
    // Find new guess
    dNew = dLower + dRatio*(dUpper-dLower);
		
		
    //		}
  }
			
  return dNew;
}

void BdryCubicParam2D::
vGetBezierPoints(double dP1[2], double dP2[2], double dP3[2], double dP4[2]) const
{
  // Calculate the Bezier Points...
  // Easy stuff.
	
  dP1[0] = adCoeffs[0][3];
  dP1[1] = adCoeffs[1][3];
	
  dP2[0] = adCoeffs[0][3] + adCoeffs[0][2]/3.0;
  dP2[1] = adCoeffs[1][3] + adCoeffs[1][2]/3.0;
	
  dP3[0] = adCoeffs[0][3] + 2.0*adCoeffs[0][2]/3.0 + adCoeffs[0][1]/3.0;
  dP3[1] = adCoeffs[1][3] + 2.0*adCoeffs[1][2]/3.0 + adCoeffs[1][1]/3.0;
	
  dP4[0] = adCoeffs[0][3] + adCoeffs[0][2] + adCoeffs[0][1] + adCoeffs[0][0];
  dP4[1] = adCoeffs[1][3] + adCoeffs[1][2] + adCoeffs[1][1] + adCoeffs[1][0];

}

double BdryCubicParam2D::dSpeed(const double dParam) const
{
  // Need to compute \sqrt((dx/dt)^2 + (dy/dt)^2).
  double dXDeriv = 3*adCoeffs[0][0]*dParam*dParam + 2*adCoeffs[0][1]*dParam
    + adCoeffs[0][2]; 
  double dYDeriv = 3*adCoeffs[1][0]*dParam*dParam + 2*adCoeffs[1][1]*dParam
    + adCoeffs[1][2];
  return (hypot(dXDeriv, dYDeriv));
}

BdryCubicParam2D::~BdryCubicParam2D ()
{
  // Destructor
}

