beziermesh.C

Go to the documentation of this file.

#ifdef HAVE_CONFIG_H
#include <tumble-conf.h>
#endif /* HAVE_CONFIG_H */

#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <limits>
#include <stack>

#include "beziermesh.h"

#include "boundarymesh.h"
#include "datastore.h"
#include "fibonacci_heap.h"
#include "hash_map.h"
#include "spline.h"

#include "sqpsmooth.h"


/* Turn on several debugging flags, for even more rigour and lots of printing */
//#define DEBUG

/* All the debug flags */

//#define DEBUG_FLIP
//#define DEBUG_SMOOTH
//#define DEBUG_INSERTS
//#define DEBUG_LOCATOR
//#define DEBUG_DEVILLERS

/* Turn on several flags to do more rigorous assertions. */

//#define CHECK_COARSEN
//#define CHECK_INVERTED
//#define CHECK_SMOOTH
//#define CHECK_FLIP


/* allow for visual debugging */
#define VISUAL_DEBUG

/* use a new delaunay algortithm */
//#define NEW_DELAUNAY_ALG

#ifdef DEBUG
    #include "meshio.h"

    /* check on insertions */
    #define DEBUG_INSERTS

    /* debug the point locator */
    //#define DEBUG_LOCATOR

    #ifdef CHECK_SMOOTH
        /* Turn on smooth debugging */
        #define DEBUG_SMOOTH
    #endif
    #ifdef CHECK_FLIP
        /* Turn on flip debugging */
       #define DEBUG_FLIP

       /* Turn on  devilliers debugging */
       #define DEBUG_DEVILLERS
    #endif
#endif


#if defined(DEBUG_SMOOTH) || defined(DEBUG_FLIP)
    #include "meshio.h"
#endif

#ifdef VISUAL_DEBUG
    #include "visualization.h"

    #ifdef DEBUG_SMOOTH
        /* Draw smooth output */
        #define DRAW_SMOOTH
    #endif

    #ifdef DEBUG_FLIP
        /* Draw flip output */
        #define DRAW_FLIP
    #endif
#endif


using namespace std;
using namespace hashers;
using namespace Tumble;

BezierMesh::BezierMesh(PersistantStore& pstore, DataStore *dstore)
: BezierComplex(pstore)
, boundarymesh(NULL), datastore(dstore)
, linear_flips(false), func_angle_x_cuttoff(-1.0)
{
}

void BezierMesh::set_bdry_mesh(BoundaryMesh* bdry) {
  assert(!boundarymesh);
  boundarymesh = bdry;
}

BezierMesh::~BezierMesh() {
}

BezierVertex* BezierMesh::add_bezier_vertex( const Point2D &_p)
{
    LinearData _d;
    return add_bezier_vertex(_p, _d);
}

BezierVertex* BezierMesh::add_bezier_vertex( const Point2D &_p,
                                             const LinearData &_d )
{
  return add_bezier_vertex(datastore->add_cp( _p ), _d);
}

BezierVertex* BezierMesh::add_bezier_vertex(ControlPoint cp)
{
    LinearData d;
    return add_bezier_vertex( cp, d );
}


BezierVertex* BezierMesh::add_bezier_vertex( ControlPoint cp, const LinearData &_d )
{
    DataPoint dp = datastore->add_dp( cp, _d );
    BezierVertex * bv = new BezierVertex(get_store(), cp, dp);
    add_vertex( bv );
    return bv;
}

BezierEdge* BezierMesh::add_bezier_edge( const Point2D &_p, BezierVertex *v0, BezierVertex *v1 )
{
  LinearData _d;
  return add_bezier_edge(_p, _d, v0, v1);
}

BezierEdge* BezierMesh::add_bezier_edge( const Point2D &_p, const LinearData &_d, BezierVertex *v0, BezierVertex *v1 )
{
  ControlPoint cp = datastore->add_cp( _p );
  return add_bezier_edge(cp, _d, v0, v1);
}

BezierEdge* BezierMesh::add_bezier_edge( ControlPoint cp, BezierVertex *v0, BezierVertex *v1 )
{
  LinearData d;
  return add_bezier_edge(cp, d, v0, v1);
}

BezierEdge* BezierMesh::add_bezier_edge( ControlPoint cp, const LinearData &_d, BezierVertex *v0, BezierVertex *v1 )
{
    // Add the datapoint first, since the BezierEdge will then go find it.
    datastore->add_dp( cp, _d );
    BezierEdge *be = new BezierEdge(get_store(), v0, v1, cp, *datastore);

    assert(be->get_data_point(1) == datastore->get_data(cp));
    add_edge(be);
    return be;
}

/*
 *
 */
BezierEdge* BezierMesh::add_straight_bezier_edge(BezierVertex *v1,
                                                        BezierVertex *v2)
{
    LinearData d = (v1->get_dp() + v2->get_dp()) / 2.0;
    return add_straight_bezier_edge(d, v1, v2);
}
BezierEdge* BezierMesh::add_straight_bezier_edge(const LinearData& d,
                                                        BezierVertex *v1,
                                                        BezierVertex *v2)
{
    Point2D midpt = (v1->get_cp() + v2->get_cp()) / 2.0;
    return add_bezier_edge(midpt, d, v1, v2);
}

BezierTriangle* BezierMesh::add_bezier_triangle( BoundaryFace *face, BezierEdge *e0,  BezierEdge *e1, BezierEdge *e2, bool inverse_handed )
{
  BezierEdge* es[3] = {e0, e1, e2};

  BezierTriangle * bt = new BezierTriangle(get_store(), es, *datastore,inverse_handed);
  if (face) {
    bt->set_boundary_face(face);
  }
  add_face( bt, es, 3, inverse_handed );
  sample_id_map[bt] = sampler.size();
  sampler.push_back(bt);
  return bt;
}

void BezierMesh::delete_vertex(BezierVertex *v)
{
    datastore->rem_dp( v->get_control_point(), v->get_data_point() );

    /* if this vertex is on the bdry then the spline or vertex in the bdry mesh
     * owns the control point, so leave it alone
     */
    if(!v->is_boundary()) {
      datastore->rem_cp(v->get_control_point());
    }

    // this is copied from the CellComplex, which is obviously wrong.
    while(v->num_edges() > 0) {
      delete_edge(v->get_any_edge());
    }
    BezierComplex::delete_vertex(v);
}

void BezierMesh::delete_edge(BezierEdge *edge){
    datastore->rem_dp( edge->get_control_point(1), edge->get_data_point(1) );

    /* If this edge is on the bdry then its get_cp(1) is owned by a
     * spline in the bdry mesh, we will let the spline deal with
     * it when it removes a knot
     */
    if( !edge->is_boundary() ) datastore->rem_cp( edge->get_control_point(1) );
    BezierComplex::delete_edge( edge );
}

void BezierMesh::delete_face(BezierTriangle *face){
  /* Remove the face from the list of faces, and update the indices */
  assert(sample_id_map.count(face) != 0);
  int index = sample_id_map[face];
  sample_id_map.erase(face);
  sampler[index] = sampler.back();
  sampler.pop_back();
  sample_id_map[sampler[index]] = index;

  BezierComplex::delete_face(face);
}

void BezierMesh::replace_control_point(BezierVertex* v,
                                       const ControlPoint& newp)
{
  ControlPoint oldp = v->get_control_point();

  /* replace in the BezierVertex's control point */
  v->replace_cp(newp);

  /* replace for all the BezierEdge's and BezierTriangles's ControlPoint lists*/
  BezierTuple start = get_tuple(v);
  BezierTuple tup = start;
  do {
    tup.e->replace_cp(oldp, newp);
    tup.f->replace_cp(oldp, newp);
    tup = Switch(1, Switch(2, tup));
  } while(tup != start);

  /* resync the data_hash mapping in the datastore */
  datastore->replace_cp(oldp, newp);
}

void BezierMesh::replace_control_point(BezierEdge* e,
                                       const ControlPoint& newp)
{
  ControlPoint oldp = e->get_control_point(1);
  assert( oldp != newp );

  /* replace in the edge's control point list */
  e->replace_cp(oldp, newp);

  /* replace the poin in the ControlPoint lists of the faces */
  BezierTuple tup = get_tuple(e);
  tup.f->replace_cp(oldp, newp);
  if(e->num_faces() == 2) {
    tup = Switch(2, tup);
    tup.f->replace_cp(oldp, newp);
  }

  /* resync the data_hash mapping in the datastore */
  datastore->replace_cp(oldp, newp);
}

int BezierMesh::orient_edge_triangle(const BezierEdge *e, const BezierTriangle *t) const
{
    assert( e->has_face(t) ); /* ensure that e and t are related */

    ControlPoint edge_cp0 = e->get_control_point(0);
    ControlPoint edge_cp2 = e->get_control_point(2);
    ControlPoint tri_cp0 = t->get_control_point( 0 );
    ControlPoint tri_cp2 = t->get_control_point( 2 );
    if ( tri_cp0 == edge_cp0 ) {
        if ( tri_cp2 == edge_cp2 ) return 0;
        else return 5;
    } else if ( tri_cp2 == edge_cp0 ) {
        if ( tri_cp0 == edge_cp2 ) return 3;
        else return 1;
    } else {
        if ( tri_cp0 == edge_cp2 ) return 2;
        return 4;
    }
}

void BezierMesh::tri_cps_off_edge(BezierEdge *e, BezierTriangle *t, ControlPoint &cp3, ControlPoint &cp4, ControlPoint &cp5)
{
    switch( orient_edge_triangle(e,t) ) {
        case 0:
            cp3 = t->get_control_point(3); cp4 = t->get_control_point(4); cp5 = t->get_control_point(5);
            return;
        case 1:
            cp3 = t->get_control_point(1); cp4 = t->get_control_point(3); cp5 = t->get_control_point(0);
            return;
        case 2:
            cp3 = t->get_control_point(4); cp4 = t->get_control_point(1); cp5 = t->get_control_point(2);
            return;
        case 3:
            cp3 = t->get_control_point(4); cp4 = t->get_control_point(3); cp5 = t->get_control_point(5);
            return;
        case 4:
            cp3 = t->get_control_point(3); cp4 = t->get_control_point(1); cp5 = t->get_control_point(0);
            return;
        case 5:
            cp3 = t->get_control_point(1); cp4 = t->get_control_point(4); cp5 = t->get_control_point(2);
            return;
    }
}

void BezierMesh::edge_param_to_tri_param(const BezierEdge *e,
                                         const BezierTriangle *t,
                                         double u, double &alpha, double &beta )
const
{
    switch( orient_edge_triangle(e,t) ) {
        case 0:
            alpha = 0;     beta = u;
            return;
        case 1:
            alpha = u;     beta = 1 - u;
            return;
        case 2:
            alpha = 1 - u; beta = 0;
            return;
        case 3:
            alpha = 0;     beta = 1 - u;
            return;
        case 4:
            alpha = 1 - u; beta = u;
            return;
        case 5:
            alpha = u;     beta = 0;
            return;
    }
    assert(0);
}

void BezierMesh::tri_param_to_edge_param(const BezierEdge *e,
                                         const BezierTriangle *t,
                                         double& u, double alpha, double beta)
const
{
  /* A failed assertion means alpha/beta do not correspond to e */
  switch( orient_edge_triangle(e,t) ) {
    case 0:
      //assert(is_zero(alpha));
      u = beta;
      return;
    case 1:
      u = alpha;
      //assert(are_equal(beta, 1.0 - alpha));
      return;
    case 2:
      u = 1 - alpha;
      //assert(is_zero(beta));
      return;
    case 3:
      //assert(is_zero(alpha));
      u = 1 - beta;
      return;
    case 4:
      //assert(are_equal(alpha, 1.0 - beta));
      u = beta;
      return;
    case 5:
      u = alpha;
      //assert(is_zero(beta));
      return;
  }
  assert(0);
}

bool BezierMesh::robust_smooth( Point2D poly[], const Point2D &primary, const Point2D &secondary, Point2D &new_pt)
{
    double x[6], y[6];
    for (int i = 0; i < 6 ;i++ ) {
        x[i] = poly[i][0];
        y[i] = poly[i][1];
    }
    if( smooth_point(x, y, primary,   new_pt) ) return true;
    if( smooth_point(x, y, secondary, new_pt) ) return true;
    return false;
}


bool BezierMesh::smooth_point( double x[], double y[], const Point2D &p, Point2D &new_pt)
{
    double optimal;
    double quality_values[12];
    int violator;
    int inform;
    int niterations;

    Point2D area_pt;
    Point2D mean_ratio_pt;
    int star_valid;

    /* First check that the given star is valid*/
    star_valid = smCheckStarValidity( x, y, 6, p.x(), p.y(), &violator );

    /* Case 4: This is a good point in the kernel, the smooth should not fail here */
    if( star_valid == SUCCESS ){
        mean_ratio_pt = p;
        smSmooth( SM_MEAN_RATIO, x, y, 6, &mean_ratio_pt.coords[0], &mean_ratio_pt.coords[1], &optimal, quality_values, &inform, &niterations );
        if ( inform == SUCCESS ) {
            new_pt = mean_ratio_pt;
            return true;
        } else {
            /* This case might be indicative of internal errors in the smoothing code */
            WARNING(cout<<"BezierMesh::smooth_point -- Possible internal error in smoothing code - Fix This!"<<endl);
        WARNING(cout<<"In this case the polygon is star-shaped with a valid initial guess, yet smoothing does not converge."<<endl);
        WARNING(cout << "x is "<<p.x()<<" and y is "<<p.y()<<endl);
        for (int i=0;i<6;i++)
          {
        WARNING(cout << x[i] <<", "<<y[i]<<endl);
          }
        WARNING(cout << "Got back "<<mean_ratio_pt.x()<<", "<<mean_ratio_pt.y()<<endl);
        WARNING(cout << "Got back inform = "<<inform<<endl);
        }

    }


    /* Now we have a point that is not in the kernel of the ploy
     * try to use an AREA smooth to get a point in the kernel
     */
    area_pt = p;
    smSmooth( SM_AREA, x, y, 6, &area_pt.coords[0], &area_pt.coords[1], &optimal, quality_values, &inform, &niterations );

    /* Case 1 failure: The polygon is not star-shaped */
    /* Smoothing is impossible */
    if( inform != SUCCESS ) {
      WARNING(cout<<"Informed returned "<<inform<<", see cfsqp (smoother) readme"<<endl);
        WARNING(cout<<"BezierMesh::smooth_point -- Polygon is not star-shaped Case 1 Failure!"<<endl
            <<"          ** You are playing with inverted triangles, not a good idea!"<<endl);
        new_pt = p;
        return false;
    }

    /* Check to see if the point from AREA smooth is in kernel */
    star_valid = smCheckStarValidity( x, y, 6, area_pt.x(), area_pt.y(), &violator );

    /* Case 2 failure: The polygon has no kernel */
    /* Smoothing is impossible */
    if( star_valid != SUCCESS ){
        cout<<"BezierMesh::smooth_point -- Polygon has no kernel"<<endl
            <<"          ** Either a Type 'B' impossible flip, or an unsmoothable edge"<<endl;
        new_pt = p;
        return false;
    }

    /* Case 3:  Poly is OK, initial point was outside kernel, but we fixed it with AREA smooth */
    /* Essentially we now have turned Case 3 into Case 4*/
    mean_ratio_pt = area_pt;
    smSmooth( SM_MEAN_RATIO, x, y, 6, &mean_ratio_pt.coords[0], &mean_ratio_pt.coords[1], &optimal, quality_values, &inform, &niterations );


    if( inform != SUCCESS ){
        /* This case might be indicative of internal errors in the smoothing code */
        WARNING(cout<<"BezierMesh::smooth_point -- Area smooth claimed that it fixed star, but RATIO failed!"<<endl);
        WARNING(cout<<"          ** Possible internal error in smoothing code - Fix This!"<<endl);
        new_pt = area_pt;
        return true;
    }

    new_pt = mean_ratio_pt;
    return true;
}


bool BezierMesh::should_flip( BezierEdge *e )
{
    assert(e!=NULL);
    if ( e->num_faces() != 2) return false;
    if ( e->is_boundary() ) return false;

    BezierTuple bt = get_tuple( e );
    Point2D p1, p2, v1, v2;
    p1 = bt.v->get_cp();
    bt = Switch( 0, Switch( 1, bt ) );
    v1 = bt.v->get_cp();
    bt = Switch( 0, Switch( 1, bt ) );
    p2 = bt.v->get_cp();
    bt = Switch( 2, Switch( 1, bt ) );
    bt = Switch( 0, Switch( 1, bt ) );
    v2 = bt.v->get_cp();
    return v2.in_circle_test( p1, p2, v1 );
}


void BezierMesh::get_edge_neighbor_cells( BezierEdge *e, BezierVertex* vs[], BezierEdge* es[], BezierTriangle* ts[] )
{
    assert( e && !e->is_boundary() );
    /*
     *       v1
     *       /\
     *      /  \
     *    e1    e0
     *    /  t0  \
     *   /        \
     * v2----e-----v0
     *   \        /
     *    \  t1  /
     *    e2    e3
     *      \  /
     *       \/
     *       v3
     */

    BezierTuple tup = get_tuple( e );
    tup = Switch( 1, Switch( 0, tup ) );
    vs[0] = tup.v;
    es[0] = tup.e;
    ts[0] = tup.f;
    tup = Switch( 1, Switch( 0, tup ) );
    vs[1] = tup.v;
    es[1] = tup.e;
    tup = Switch( 1, Switch( 2, Switch( 1, Switch( 0, tup ) ) ) );
    vs[2] = tup.v;
    es[2] = tup.e;
    ts[1] = tup.f;
    tup = Switch( 1, Switch( 0, tup ) );
    vs[3] = tup.v;
    es[3] = tup.e;
}

bool BezierMesh::can_flip( BezierEdge *e )
{
    bool is_typeA, is_typeB;
    return can_flip(e, is_typeA, is_typeB);
}

bool BezierMesh::can_flip( BezierEdge *e, bool &is_typeA, bool &is_typeB)
{
    BezierVertex* vs[4];
    BezierEdge* es[4];
    BezierTriangle* ts[2];

    get_edge_neighbor_cells(e, vs, es, ts);
    Point2D poly[6];
    Point2D primary = e->get_cp(1);
    Point2D secondary = (vs[1]->get_cp() + vs[3]->get_cp())*0.5;
    Point2D temp;
    get_flip_polygon(poly,vs,es);

    /* Check if we are a type B */
    is_typeB = !robust_smooth( poly, primary, secondary, temp);

    /* Check if we are a type A */
    /* Check that the outised flips in the control mesh are possible */
    is_typeA = ! (    es[0]->get_cp(1).is_left_of(es[3]->get_cp(1), vs[0]->get_cp())
                        && es[2]->get_cp(1).is_left_of(es[1]->get_cp(1), vs[2]->get_cp()) );

    bool flippable = !is_typeB && !is_typeA;

    return flippable;
}

void BezierMesh::split_return_adjacent(BezierEdge *e, vector<BezierEdge*> &new_edges)
{
  list<BezierEdge*> cavity_edges;
  find_adjacent_edges(e, cavity_edges);

  // Add the cavity edges to the list
  new_edges.insert(new_edges.end(), cavity_edges.begin(), cavity_edges.end());

  // Split the edge
  BezierVertex *v = insert_edge_midpoint(e, 0.5);

  // Add the spokes from the new vertex to the list
  upper(v, new_edges);

  return;
}

void BezierMesh::split_edge_near_unflippable(BezierEdge *e, vector<BezierEdge*> &new_edges )
{
    assert(e->num_faces() == 2);
    list<BezierEdge*> cavity_edges;
    find_adjacent_edges(e, cavity_edges);

    // Find the most curved cavity edge
    list<BezierEdge*>::iterator i = cavity_edges.begin();
    BezierEdge* most_curved_cavity_edge = (*i);
    for (; i != cavity_edges.end(); ++i)
      {
    BezierEdge* current = (*i);
    if (current->curvature_angle() > most_curved_cavity_edge->curvature_angle())
      most_curved_cavity_edge = current;
      }

    // Split a cavity edge if it is more curved than theta (currently uses theta = alpha* / 2)
    double theta_bound = 0.5 * e->get_any_face()->get_min_angle();
    if (most_curved_cavity_edge->curvature_angle() > theta_bound)
      split_return_adjacent(most_curved_cavity_edge, new_edges);
    else
      split_return_adjacent(e, new_edges);

    return;
}


bool BezierMesh::debug_flip( BezierEdge *e )
{
    BezierVertex* vs[4];
    BezierEdge* es[4];
    BezierTriangle* ts[2];

    get_edge_neighbor_cells(e, vs, es, ts);
    Point2D poly[6];
    Point2D primary = e->get_cp(1);
    Point2D secondary = (vs[1]->get_cp() + vs[3]->get_cp())*0.5;
    get_flip_polygon(poly,vs,es);

    bool is_typeA, is_typeB;
    bool flippable = can_flip(e, is_typeA, is_typeB);

#ifdef DEBUG_FLIP
    if(!flippable){
        cout << "\nFLIPPING ERROR: ----------------------\n";
        cout << "Unable to flip edge: " << e << " with tris: "
          << ts[0] << " and " << ts[1] << endl;
        if(is_typeA) {
          cout << "Type A unflippable (outer flips cannot be done)\n";
        }
        if(is_typeB) {
          cout << "Type B unflippable (smoothing polygon is bad)\n";
        }
        for(int i = 0; i < 2; i++) {
          if(ts[i]->is_inverted()) {
            cout << ts[i] << " is inverted\n";
          }
        }

        MeshBinaryOutput::write("dumped_flip.geo", this, boundarymesh);

        #ifdef DRAW_FLIP
        if(ts[0]->is_inverted()) {
          cout << "Yellow Triangle is inverted\n";
        }
        if(ts[1]->is_inverted()) {
          cout << "Cyan Triangle is inverted\n";
        }
        draw_smooth_error("flip",e,poly,primary,secondary,ts);
        #endif
    }
#endif

    return flippable;
}

bool BezierMesh::can_smooth( BezierEdge *e )
{
    BezierVertex* vs[4];
    BezierEdge* es[4];
    BezierTriangle* ts[2];
    get_edge_neighbor_cells(e, vs, es, ts);

    Point2D poly[6];
    Point2D primary = e->get_cp(1);
    Point2D secondary = (vs[0]->get_cp() + vs[2]->get_cp())*0.5;
    Point2D temp;
    get_smooth_polygon(poly, vs, es);

    bool smoothable = robust_smooth( poly, primary, secondary, temp);

    return smoothable;
}

bool BezierMesh::debug_smooth( BezierEdge *e )
{
    BezierVertex* vs[4];
    BezierEdge* es[4];
    BezierTriangle* ts[2];
    get_edge_neighbor_cells(e, vs, es, ts);

    Point2D poly[6];
    Point2D primary = e->get_cp(1);
    Point2D secondary = (vs[0]->get_cp() + vs[2]->get_cp())*0.5;
    Point2D temp;
    get_smooth_polygon(poly, vs, es);

    bool smoothable = robust_smooth( poly, primary, secondary, temp);

#ifdef DEBUG_SMOOTH
    if(!smoothable){
        WARNING(cout<<endl<<"SMOOTHING ERROR: ----------------------"<<endl);
        WARNING(cout<<"Unable to smooth edge: "<<*(e)<<" with tris: "<<*(ts[0])<<" and "<<*(ts[1])<<endl);
        if(ts[0]->is_inverted()) 
      {
        WARNING(cout<<"Yellow Triangle is inverted"<<endl);
      }
        if(ts[1]->is_inverted()) 
      {
        WARNING(cout<<"Cyan Triangle is inverted"<<endl);
      }
        #ifdef DRAW_SMOOTH
        draw_smooth_error("smooth",e,poly,primary,secondary,ts);
        #endif
    } 
#endif

    return smoothable;
}


void BezierMesh::draw_smooth_error(const char *name, BezierEdge *e, Point2D poly[], const Point2D &primary, const Point2D &secondary, BezierTriangle* ts[])
{
#ifdef VISUAL_DEBUG
    static int jpeg_num=0;

    Visualization vis(this, this->boundarymesh);
    vis.start_draw();
    vis.draw_smooth_debug(e, poly, primary, secondary);
    vis.finish_draw();
    char temp[80];
    snprintf(temp,80,"%s_error_%04u.ppm",name,jpeg_num);
    vis.write_ppm(temp);
    snprintf(temp,80,"%s_error_%04u.txt",name,jpeg_num);
    ofstream out(temp);
    out<<"Triangle 1:\n"<<*ts[0]<<"\n"
       <<"Triangle 2:\n"<<*ts[1]<<endl;
    out.close();
    cout<<"Dumped "<<temp<<", continuing... "<<endl<<endl;
    vis.zoomout();
    vis.start_draw();
    vis.draw(GREEN, BLUE);
    vis.finish_draw();
    jpeg_num++;
#endif
}

void BezierMesh::draw_insert_error(const char* name, BezierTriangle* bt)
{
  cout<<"Drawing insert error"<<endl;
  #ifdef VISUAL_DEBUG
    static int jpeg_num=0;
    Visualization vis(this, this->boundarymesh);
    cout<<"Created visualization"<<endl;
    Point2D top, bot;
    vis.get_bbox(&bt,1,top,bot);
    vis.start_draw();
    vis.zoomin(top,bot);
    BezierTuple tup = get_tuple(bt);
    for (uint i=0;i<6;i++) { vis.draw_point(bt->get_cp(i),MAGENTA,7.0); }
    vis.draw_tuple(tup, RED, WHITE,GREEN);
    vis.draw_control_net(bt, BLUE);
    vis.finish_draw();
    char temp[80];
    snprintf(temp,80,"%s_error_%04u.ppm",name,jpeg_num);
    vis.write_ppm(temp);
    cout<<"Dumped:"<<temp<<", continuing.. "<<endl<<endl;
    jpeg_num++;
  #endif
}

void BezierMesh::get_flip_polygon(Point2D poly[], BezierVertex* vs[], BezierEdge* es[])
{
    /* counter-clockwise ordering */
    poly[ 0 ] = vs[1]->get_cp();
    poly[ 1 ] = es[1]->get_cp(1);
    poly[ 2 ] = es[2]->get_cp(1);
    poly[ 3 ] = vs[3]->get_cp();
    poly[ 4 ] = es[3]->get_cp(1);
    poly[ 5 ] = es[0]->get_cp(1);
}

void BezierMesh::get_smooth_polygon(Point2D poly[], BezierVertex* vs[], BezierEdge* es[])
{
    /* counter-clockwise ordering */
    poly[ 0 ] = vs[0]->get_cp();
    poly[ 1 ] = es[0]->get_cp(1);
    poly[ 2 ] = es[1]->get_cp(1);
    poly[ 3 ] = vs[2]->get_cp();
    poly[ 4 ] = es[2]->get_cp(1);
    poly[ 5 ] = es[3]->get_cp(1);
}

bool BezierMesh::flip( BezierEdge *e, BezierTuple &tup )
{
    /* Check preconditions */
    assert( e && "Flip attempted on NULL edge");
    assert( (!e->is_boundary()) && "Flip attempted on boundary edge" );
    assert( (e->num_faces() == 2) && "Flip attempted on edge without two triangles" );

    BezierVertex *vs[4];
    BezierEdge *es[4];
    BezierTriangle *ts[2];
    get_edge_neighbor_cells(e, vs, es, ts);

#ifdef DEBUG_FLIP
    cout << "Debugging flip of " << *e;
    cout << "Cavity: " << vs[0] << ", " << vs[1] << ", "
      << vs[2] << ", " << vs[3] << endl;
    cout << "Flipping to:\n\t" << *vs[1] << "\t" << *vs[3];
#endif

#ifdef CHECK_FLIP
    {
      bool bad = false;
      for(int i = 0; i < 2; i++) {
        if (ts[i]->is_definitely_inverted()) {
          cerr << "Flip input has Triangle ts[" << i
            << "] with negative jacobian\n" << *ts[i] << endl;
          bad = true;
        }
      }
      if( !debug_flip(e) ){
        cerr << "Could not flip edge\n";
        bad = true;
      }
      assert(!bad);
    }
#endif



    /* if linear flips are on then do not use the smoother to get
     * the new control point.  instead, make new edge linear
     */
    Point2D new_point;

    if(linear_flips){
        new_point = (vs[1]->get_cp() + vs[3]->get_cp())*0.5;
    } else {
        Point2D poly[6];
        get_flip_polygon(poly, vs, es);
        Point2D primary = e->get_cp(1);
        Point2D secondary = (vs[1]->get_cp() + vs[3]->get_cp()) * 0.5;
        if(!robust_smooth( poly, primary, secondary, new_point )){
            /* Hopefully we will never get here */
            WARNING(cout<<endl<<"BezierMesh::flip --- could not flip edge!"<<endl);
            #ifdef CHECK_FLIP
            cout<<"Darn, Still failed even though we checked for flipability!"<<endl;
            return false;
            #endif

            /* Try to keep going if possible */
            new_point = e->get_cp(1);
        }
    }

    /* Create ghost triangles for reinterpolation */
    GhostTriangle *oldts[2];
    GhostTriangle gt0(*ts[0]);
    GhostTriangle gt1(*ts[1]);
    oldts[0] = &gt0;
    oldts[1] = &gt1;

    /* Use zeroed data as required by reinterpolate.
     * reinterpolate will set this correctly later
     */
    LinearData new_data( e->get_dp(1).length );
    BoundaryFace *face = ts[0]->get_bdry_face_or_null();
    assert(face == ts[1]->get_bdry_face_or_null());

    delete_edge( e );

    BezierEdge *new_edge;
    BezierTriangle *newts[2];

    new_edge = add_bezier_edge( new_point, new_data, vs[1], vs[3] );
    newts[0] = add_bezier_triangle( face, new_edge, es[3], es[0] ); // ccw: tp517 4-16-07
    newts[1] = add_bezier_triangle( face, new_edge, es[1], es[2], true ); //cw: tp517 4-16-07

#ifdef DEBUG_FLIP
    cout << "Added new triangles and new edge "<<(*new_edge)<<endl;
#endif


#ifdef CHECK_FLIP
    {
      bool bad = false;
      for(int i = 0; i < 2; i++) {
        if (newts[i]->is_definitely_inverted()) {
          cerr << "Flip created Triangle newts[" << i
            << "] with negative jacobian\n" << *newts[i] << endl;
          bad = true;
        }
      }
      assert(!bad);
    }
    assert(print_check_inverted("flipping an edge"));
#endif

    /* Reset new_edge->dp[1] via reinterpolation code */
    reinterpolate(new_edge, newts, oldts);
    tup=get_tuple(new_edge, newts[0]);
    return true;
}

bool BezierMesh::smooth_edge( BezierEdge* e )
{
    /* Check preconditions */
    assert( e && "Smooth attempted on NULL edge");
    if(e->is_boundary()) return false;
    if(e->num_faces() != 2) return false;

    BezierVertex *vs[4];
    BezierEdge *es[4];
    BezierTriangle *ts[2];
    get_edge_neighbor_cells(e, vs, es, ts);

#ifdef CHECK_SMOOTH
    if( ts[0]->is_definitely_inverted() ){
      //WARNING(cout<<"Smooth given Triangle ts[0] with negative jacobian"<<endl);
    }
    if( ts[1]->is_definitely_inverted() ){
      //WARNING(cout<<"Smooth created Triangle ts[1] with negative jacobian"<<endl);
    }

    if(!debug_smooth(e)){
        return false;
    }
#endif

    Point2D poly[6];
    Point2D new_center;
    Point2D primary = e->get_cp(1);
    Point2D secondary = (vs[0]->get_cp() + vs[2]->get_cp()) * 0.5;
    get_smooth_polygon(poly, vs, es);
    if(!robust_smooth( poly, primary, secondary, new_center )){
        /* Hopefully we will never get here */
        WARNING(cout<<endl<<"BezierMesh::smooth_edge --- could not smooth edge!"<<endl);
        #ifdef CHECK_SMOOTH
        cout<<"Darn, Still failed even though we checked for smoothability!"<<endl;
        #endif
        new_center = e->get_cp(1);
        return false;
    }

    BezierTriangle* newts[2];
    GhostTriangle *oldts[2];

    /* Get current triangles */
    assert(e->num_faces() == 2);
    newts[0] = *e->begin_faces();
    newts[1] = *++e->begin_faces();

    /* Copy current triangles to ghost triangles before update*/
    GhostTriangle gt0(*newts[0]);
    GhostTriangle gt1(*newts[1]);
    oldts[0] = &gt0;
    oldts[1] = &gt1;

    /* Update edge cp */
    e->set_cp(new_center,1);

#ifdef CHECK_SMOOTH
    if( ts[0]->is_definitely_inverted() ){
      //WARNING(cout<<"Smooth created Triangle ts[0] with negative jacobian"<<endl);

    if(!debug_smooth(e)){
      return false;
    }
    }
    if( ts[1]->is_definitely_inverted() ){
      //WARNING(cout<<"Smooth created Triangle ts[1] with negative jacobian"<<endl);
    if(!debug_smooth(e)){
      return false;
    }
    }
#endif
    /* Let reinterpolate set data for new point*/
    reinterpolate(e, newts, oldts);
    return true;
}




BezierVertex* BezierMesh::insert_point( BezierTriangle *t, double u, double v )
{
    Point2D new_p;
    LinearData new_d;
    Point2D c[ 3 ];
    LinearData d[ 3 ];
    t->evalIntermediate( u, v, new_p, c[ 0 ], c[ 1 ], c[ 2 ] );
    t->dataevalIntermediate( u, v, new_d, d[ 0 ], d[ 1 ], d[ 2 ] );

#ifdef DEBUG_INSERTS
    assert(check_consistency());
#endif

    /* Enumerate vertexs and sides
     *
     *         v2
     *         /\
     *     e2 /  \ e1
     *       /    \
     *   v0 /______\v1
     *         e0
     */
    BezierVertex *v0, *v1, *v2;
    BezierEdge *e0, *e1, *e2;
    BezierTuple tup = get_tuple( t );
    v0 = tup.v;
    e0 = tup.e;
    tup = Switch( 1, Switch( 0, tup ) );
    v1 = tup.v;
    e1 = tup.e;
    tup = Switch( 1, Switch( 0, tup ) );
    v2 = tup.v;
    e2 = tup.e;

    /* Delete face */
    BoundaryFace *face = t->get_bdry_face_or_null();
    delete_face( t );

     /* Create new vertex, edges and triangles
     *
     *          v2
     *          /|\
     *         / |n\
     *        /  |e \
     *       /   |2  \
     *    e2/    nv   \e1
     *     /nt2 / \ nt1\
     *    /   /     \   \
     *   /  /ne0   ne1\  \
     *  / /     nt0     \ \
     * v0-----------------v1
     */
    BezierVertex *nv;
    BezierEdge *ne0, *ne1, *ne2;
    BezierTriangle *nt0, *nt1, *nt2;

    nv = add_bezier_vertex( new_p, new_d );
    ne0 = add_bezier_edge( c[ 0 ], d[ 0 ], nv, v0 );
    ne1 = add_bezier_edge( c[ 2 ], d[ 2 ], nv, v1 );
    ne2 = add_bezier_edge( c[ 1 ], d[ 1 ], nv, v2 );
    nt0 = add_bezier_triangle( face, ne0, e0, ne1 ); //ccw: tp517 4-16-07
    nt1 = add_bezier_triangle( face, ne1, e1, ne2 ); //ccw: tp517 4-16-07
    nt2 = add_bezier_triangle( face, ne2, e2, ne0 ); //ccw: tp517 4-16-07


    // On a really bent triangle, we'll give it the old college try (smooth)
    if (nt0->is_inverted() || nt1->is_inverted() || nt2->is_inverted() )
      {
    smooth_edge(ne0);
    smooth_edge(ne1);
    smooth_edge(ne2);
      }

    print_check_inverted("inserted a point");
    return nv;
}


BezierVertex* BezierMesh::insert_edge_midpoint( BezierEdge *e, double u )
{
    BezierVertex * ev[2];
    BezierVertex * v0, *v1, *v2, *v3 = NULL;
    BezierEdge *e0, *e1, *e2 = NULL, *e3 = NULL;
    BezierTriangle *t0, *t1;
    BoundaryFace *t0face, *t1face = NULL;
    BezierTuple tup;
    double t0_alpha, t0_beta, t1_alpha, t1_beta;
    Point2D t0_newp, t1_newp;
    LinearData t0_newd, t1_newd;
    Point2D t0pc[ 3 ], t1pc[ 3 ];
    LinearData t0dc[ 3 ], t1dc[ 3 ];
    BoundaryEdge *bdry = NULL;

#ifndef NDEBUG
    Point2D insertpt = e->eval(u);
#endif

    /* Is there a triangle on only one side of this edge?
     * If not, than there are two triangle to worry about
     */
    bool one_sided = false;

    /* Is this edge on the boundary?
     * If so we need to ensure a knot is added to the spline, and boundary information is updated
     */
    bool is_bdry = false;

    ev[0] = e->get_vertex(0);
    ev[1] = e->get_vertex(1);

    tup = get_tuple(e);
    t0 = tup.f;
    t1 = Switch(2, tup).f;
    if (t0 == t1) {
      one_sided = true;
    }

    if (e->is_boundary()) {
        is_bdry = true;
        bdry = e->get_bdry_edge();
    }

#ifdef DEBUG_INSERTS
    cout << "Inserting edge point with boundary="<<is_bdry<<" and one_side="<<one_sided<<endl;
    cout << "Trying to insert at "<<u<<" in "<<e<<" = "<<e->eval(u)<<endl;
    cout << "Midway through iemp with "<<e<<" = "<<(*e)<<endl;
#endif

    /* enumerate first triangle's vertices and edges
    *  x=initial tup
    *         v1
    *         /\
    *     e1 /  \ e0
    *       / t0 \
    *   v2 /x_____\v0
    *      \  e   /
    *     e2\    /e3
    *        \t1/
    *         \/
    *         v3
    * Second triangle may not exist if on boundary
    */
    tup = get_tuple( e, t0 );
    v2 = tup.v;
    tup = Switch( 1, Switch( 0, tup ) );
    e0 = tup.e;
    v0 = tup.v;
    tup = Switch( 1, Switch( 0, tup ) );
    v1 = tup.v;
    e1 = tup.e;
    tup = Switch( 0, tup );

    edge_param_to_tri_param(e, t0, u, t0_alpha, t0_beta );
    t0face = t0->get_bdry_face_or_null();
    t0->evalIntermediate( t0_alpha, t0_beta, t0_newp, t0pc[ 0 ], t0pc[ 1 ], t0pc[ 2 ] );
    t0->dataevalIntermediate( t0_alpha, t0_beta, t0_newd, t0dc[ 0 ], t0dc[ 1 ], t0dc[ 2 ] );

    if ( !one_sided ) {
        /* Number second triangle's points and edges */
        tup = Switch( 0, Switch( 1, Switch( 2, Switch( 1, tup ) ) ) );
        v3 = tup.v;
        e2 = tup.e;
        tup = Switch( 1, tup );
        e3 = tup.e;

        edge_param_to_tri_param(e, t1, u, t1_alpha, t1_beta );
        t1face = t1->get_bdry_face_or_null();
        t1->evalIntermediate( t1_alpha, t1_beta, t1_newp, t1pc[ 0 ], t1pc[ 1 ], t1pc[ 2 ] );
        t1->dataevalIntermediate( t1_alpha, t1_beta, t1_newd, t1dc[ 0 ], t1dc[ 1 ], t1dc[ 2 ] );
    }

    /*
      #ifdef DEBUG_INSERTS
      cout << "Orienting t0a = "<<t0_alpha<< " t0b = "<<t0_beta <<endl;
      cout << "Orienting t1a = "<<t1_alpha<< " t1b = "<<t1_beta <<endl;
      #endif
    */

    /* These indexes are used to match up the center point of the new edges with
     * the appropriate t0 evalIntermediate data.
     */
    int ne0index, ne1index, ne2index;
    if ( t0_alpha == 0 ) {
        ne0index = 2;
        ne1index = 0;
        ne2index = 1;
    } else if ( t0_beta == 0 ) {
        ne0index = 0;
        ne1index = 1;
        ne2index = 2;
    } else {
        ne0index = 1;
        ne1index = 2;
        ne2index = 0;
    }


    /* this index is for ne3, the only edge created from the t1 evaluation data(note inefficiency).
     * this must be the edge that does not duplicate ne0 or ne1, so we need to
     * figure out the orientation, to select the right edge
     */
    int ne3index = 0;
    if ( !one_sided ) {
        if ( t1_alpha == 0 ) {
            ne3index = 1;
        } else if ( t1_beta == 0 ) {
            ne3index = 2;
        } else {
            ne3index = 0;
        }
    }

    /* 
       #ifdef DEBUG_INSERTS
       cout << "Decided on keeping split midpoints "<<t0pc[ne0index]<<" and "<<t0pc[ne1index]<<endl;
       cout << "Decided on keeping cross midpoints "<<t0pc[ne2index]<<" and "<<t1pc[ne3index]<<endl;
       #endif 
    */

     /* Delete e, t0, and t1 */
    delete_edge( e );


    /* Now it's time to create new vertex, edges, and triangles
     *
     *           v1
     *           /|\
     *          /n| \
     *         / e|  \
     *     e1 /  2|   \ e0
     *       /nt1 | nt0\
     *   v2 /_____|_____\v0
     *      \ ne1 | ne0 /     nv=new center vertex
     *     e2\nt2 | nt3/e3    one_sided insertion ignores bottom of image
     *        \  n|   /
     *         \ e|  /
     *          \3| /
     *           \|/
     *            v3
     */
    BezierVertex *nv;
    BezierEdge *ne0, *ne1, *ne2, *ne3 = NULL;
    BezierTriangle *nt0, *nt1, *nt2, *nt3;

     if ( is_bdry ) {
       /* if we are on the bdry then nv, ne0, ne1 must use spline's cps created
        * by add_knot*/
        ControlPoint cp0,cp1,cp2;
        double splineu, v0u = 0.0, v2u = 0.0;

        /* get u values of vertexs along this boundary edge*/
        v0u = v0->get_u(bdry,v2);
        v2u = v2->get_u(bdry,v0);
        //assert(!Tumble::are_equal(v0u, v2u));

        if (ev[0] == v0) {
          /*  u*v2u + (1-u)*v0u  */
          splineu = v0u + u*(v2u - v0u);
        } else {
          /* if the edge is backwards from the spline, then u' = 1 - u */
          assert(ev[1] == v0);
          assert(ev[0] == v2);

          /* u'*v2u + (1-u')*v0u == u*v02 + (1-u)*v2u */
          splineu = v2u + u*(v0u - v2u);
        }

        /*insert new knot in spline*/
        bdry->get_spline()->add_knot( splineu,cp0,cp1,cp2 );

        /* add vertex */
        nv = add_bezier_vertex( cp1, t0_newd );
        nv->set_bdry( bdry, splineu );

        /*add edges */
        if(v0u > v2u){
            ne0 = add_bezier_edge( cp2, t0dc[ ne0index ], nv, v0 );
            ne1 = add_bezier_edge( cp0, t0dc[ ne1index ], nv, v2 );
        } else {
            ne0 = add_bezier_edge( cp0, t0dc[ ne0index ], nv, v0 );
            ne1 = add_bezier_edge( cp2, t0dc[ ne1index ], nv, v2 );
        }

        ne0->set_bdry( bdry, splineu, v0u );
        ne1->set_bdry( bdry, splineu, v2u );
    }
    /* otherwise if not on the bdry then nv, ne0, ne1 will create their own cps */
    else {
        /* add vertex */
        nv = add_bezier_vertex( t0_newp, t0_newd );

        /*add edges */
        ne0 = add_bezier_edge( t0pc[ ne0index ], t0dc[ ne0index ], nv, v0 );
        ne1 = add_bezier_edge( t0pc[ ne1index ], t0dc[ ne1index ], nv, v2 );
    }
     //assert(are_equal(nv->get_cp().x(), insertpt.x()));
     //assert(are_equal(nv->get_cp().y(), insertpt.y()));

    ne2 = add_bezier_edge( t0pc[ ne2index ], t0dc[ ne2index ], nv, v1 );
    if ( !one_sided ) ne3 = add_bezier_edge( t1pc[ ne3index ], t1dc[ ne3index ], nv, v3 );

    /* add triangles */
    nt0 = add_bezier_triangle( t0face, ne0, e0, ne2 ); //ccw: tp517 4-16-07
    nt1 = add_bezier_triangle( t0face, ne2, e1, ne1 ); //ccw: tp517 4-16-07
    if ( !one_sided ) {
      nt2 = add_bezier_triangle( t1face, ne1, e2, ne3 ); //ccw: tp517 4-16-07
      nt3 = add_bezier_triangle( t1face, ne3, e3, ne0 ); //ccw: tp517 4-16-07
    }

    assert(print_check_inverted("inserted edge mindpoint"));
    return nv;
}

void BezierMesh::flip_out_cavity( list<BezierEdge*> &cavity, list<BezierEdge*> &new_cavity, BezierVertex *v )
{
    queue<BezierEdge*> todo;
    BezierEdge *e;
    BezierTuple tup;
    list<BezierEdge*>::iterator i;
    bool success;
    for(i = cavity.begin(); i != cavity.end(); ++i){
        todo.push(*i);
        while(!todo.empty()){
            e = todo.front();
            todo.pop();
            if(should_flip(e)){
                success = flip( e, tup );
                assert(success);
                if(tup.v != v) tup = Switch(2, Switch(0, tup));
        assert(tup.v == v);
                tup = Switch(1, Switch(0, tup));
                todo.push(tup.e);
                tup = Switch(1, Switch(2, Switch(1, tup)));
                todo.push(tup.e);
            } else {
                new_cavity.push_back(e);
            }
        }
    }
    assert(print_check_inverted("flipped a cavity"));
}

BezierVertex* BezierMesh::clean_insert_point( BezierTriangle *t, double u, double v, bool force) {
    list<BezierEdge*> cavity, new_cavity;
    BezierVertex *new_vertex;
    Point2D p = t->eval(u, v);
    list<BezierEdge*>::iterator i;

    /* initial cavity is simply edges of t */
    lower( t, cavity );

    /* check for encroachment on initial cavity */
    if (!force)
      {
    for ( i = cavity.begin(); i != cavity.end(); ++i ) {
      BezierEdge *e = *i;
      if ( e->is_boundary() && e->is_encroached( p ) )
        return clean_insert_edge_midpoint( e, 0.5, true );
    }
      }
#ifdef DEBUG_INSERTS
    cout << "[BezierMesh]  inserting point " << p
         << " in triangle " << *t << " at (u,v)=(" << u << "," << v << ")\n";
#endif

    /* if no encroachment then do the insertion and flip out cavity */
    new_vertex = insert_point( t, u, v );
    flip_out_cavity( cavity, new_cavity, new_vertex );

    if (!force) {
    /* now, if e encroaches on any edge in the new cavity we need to back up
     * and do an insert_midpoint  this is inefficient, but hopefully most
     * cases are caught by the first encroachment cases
     */
    for ( i = new_cavity.begin(); i != new_cavity.end(); ++i ) {
      BezierEdge *e = *i;
      if ( e->is_boundary() && e->is_encroached(new_vertex->get_cp()) ) {
#ifdef DEBUG_INSERTS
        printf("BezierMesh::clean_insert_point -- Backing out due to unforeseen encroachment\n");
#endif
        remove_vertex( new_vertex );
        return clean_insert_edge_midpoint( e, 0.5, true);
      }
    }
    }
    return new_vertex;
}

BezierVertex* BezierMesh::clean_insert_edge_midpoint( BezierEdge *e, double u, bool force)
{
    /* If this isn't a forced point but it is on a bdry, we should be adding the midpoint of the
     *  boundary edge instead
     */
    if (!force && e->is_boundary())
      u = 0.5;
#ifdef DEBUG_INSERTS
    cout << "[BezierMesh:clean_insert_edge_mp]  inserting point " << e->eval(u)
         << " on edge " << *e << " at u=" << u << endl;
#endif

    list<BezierEdge*> cavity, new_cavity;
    BezierVertex *new_vertex;

    /* collect all edges around faces of e.  remove e from cavity vector.*/
    for(BezierEdge::face_iterator it = e->begin_faces();
        it != e->end_faces(); ++it) {
      lower(*it, cavity);
    }
    cavity.remove( e );

    //insert and flip out
    new_vertex = insert_edge_midpoint( e, u );


    #ifdef DEBUG_INSERTS
    cout << "Inserted edge midpoint, flipping out cavity around "<<(*new_vertex)<<" ... " << endl;
    #endif

    flip_out_cavity( cavity, new_cavity, new_vertex );

    if (!force && !e->is_boundary())  // If we've inserted a boundary midpoint, don't yield
      {
    /* now, if e encroaches on any edge in the new cavity we need to back up
     * and do an insert_midpoint  this is inefficient, but hopefully most
     * cases are caught by the first encroachment cases
     */
    list<BezierEdge*>::iterator i;
    for ( i = new_cavity.begin(); i != new_cavity.end(); ++i ) {
      e = *i;
      if ( ( e->is_boundary() ) && ( e->is_encroached( new_vertex->get_cp() ) ) ){
#ifdef DEBUG_INSERTS
        WARNING( printf("BezierMesh::clean_insert_edge_midpoint -- Warning: backing out due to unforeseen encroachment\n"); )
#endif
        remove_vertex( new_vertex );

        return clean_insert_edge_midpoint( e, 0.5, true);
      }
    }
      }

#ifdef DEBUG_INSERTS
    cout << "Returning from clean_insert_midpoint" << endl;
#endif

    return new_vertex;
}

BezierVertex* BezierMesh::clean_insert_point(Point2D p,
                                             BezierTriangle* start_tri,
                                             bool over_bdry, bool force)
{
#ifdef DEBUG_INSERTS
  cout<<"Doing a clean insert of point "<<p<<endl;
#endif

    assert ((start_tri != NULL) || over_bdry); // over_bdry makes no sense without a start_tri
    assert (over_bdry || !force); // If we can't walk over boundaries, then we can't force this point in
    double u,v;
    int cd; /* containment dimension */

    BezierTuple cellfound;

    if (start_tri == NULL) {
      cd = blind_locate_point(p, cellfound, u, v);
    } else {
      cd = locate_point(p, start_tri, over_bdry, cellfound, u, v);
    }

    switch(cd) {
      case 0:
        /* Illegal to insert points that already exist. */
    cout << "Attempting to insert point "<<p<<" too close to an existing vertex." << endl;
    cout << "Insertion seems too close to "<<cellfound.v->get_cp()<<endl;
        assert(cd != 0);
        break;

      case -1:
      case 1:
        /* Split the edge we found (either we landed on it, or we're trying
           to cross through it). */
    assert(cellfound.e != NULL);
        assert(v == 0.0);
#ifdef DEBUG_INSERTS
    cout<<" Strange location case" << cd << ". Was trying to insert "
          << p << " could only find it at u=" << u
          << " on edge " << *cellfound.e;
#endif
#ifndef NDEBUG
        if (cd == 1) {
          /* assert that (u,v) lands at p -- unless we hit a boundary */

      /*

      TODO: This needs to be replaced with floating point tolerance

          Point2D ev = cellfound.e->eval(u);
          //assert(are_equal(p.x(), ev.x()));
          //assert(are_equal(p.y(), ev.y()));

      */
        }
#endif
        return clean_insert_edge_midpoint(cellfound.e, u, force);

      case 2:
    assert(cellfound.f != NULL);
#ifndef NDEBUG
        {
          /* assert that (u,v) lands at p */
          Point2D ev = cellfound.f->eval(u,v);
          //assert(are_equal(p.x(), ev.x()));
          //assert(are_equal(p.y(), ev.y()));
        }
#endif
        return clean_insert_point(cellfound.f, u, v, force);
    };
    assert(0);
    return NULL;
}

BezierVertex* BezierMesh::clean_insert_circumcenter(BezierTriangle *t)
{
    return clean_insert_point(t->circumcenter(), t, false, false);
}

void BezierMesh::get_link( BezierTuple start_tup, list<BezierVertex*> &link )
{
    BezierTuple tup = start_tup;
    bool hit_bdry = false;
    BezierVertex *v0=NULL, *v=NULL;

    /* grab initial vertex */
    v0 = Switch( 0, tup ).v;
    link.push_back( v0 );

    /* Switch around vertex (so that tup.v == start_tup.v, always),
     * then at each iter, switch0 out to the link and grab the vertex.
     */
    do {
        if ( tup.e->is_boundary() ) {
            hit_bdry = true;
        } else {
            tup = Switch( 1, Switch( 2, tup ) );
            v = Switch( 0, tup ).v;
            if ( v != v0 )
                link.push_back( v );
        }
    } while ( ( v0 != v ) && ( !hit_bdry ) );

    /* If we hit the bdry, then we must start adding vertexs in the other direction.
     * thus reverse current list and add to the end until bdry reached again.
     */
    if ( hit_bdry ) {
        link.reverse();
        hit_bdry = false;
        tup = Switch( 1, start_tup );
        link.push_back( Switch( 0, tup ).v );
        do {
            if ( tup.e->is_boundary() ) {
                hit_bdry = true;
            } else {
                tup = Switch( 1, Switch( 2, tup ) );
                link.push_back( Switch(0, tup).v );
            }
        } while ( !hit_bdry );
    }

    /* is it possible to do this without reversing? */
    Point2D p0, lp0, lp1;
    p0 = start_tup.v->get_cp();
    lp0 = link.front()->get_cp();
    lp1 = ( *( ++link.begin() ) )->get_cp() ;
    if ( p0.is_right_of( lp0, lp1 ) ) link.reverse();
}


int BezierMesh::check_bdry_vertex_removal_degeneracies( list<BezierVertex*> &link, BezierVertex *v ) {
    Point2D vp = v->get_cp();
    Point2D lp_first = link.front()->get_cp();
    Point2D lp_last = link.back()->get_cp();
    double concave = vp.line_side_test( lp_first, lp_last );

    /* If CONVEX then no problems, simply stop at 1 non-bdry edge adjacent*/
    if ( concave < 0 )
        return 1;

    /* Otherwise we are concave */

    /* if link has no non-bdry edge we must return 0*/
    if ( link.size() == 2 )
        return 0;

    /* If any vertex in the link is in the convex hull of the bdry_edges
     * adjacent to v, then we cannot remove successfully, so return -1
     */
    list<BezierVertex*>::iterator i;
    list<BezierVertex*>::iterator end = --link.end();
    for ( i = ++link.begin(); i != end; ++i ) {
        vp = (*i)->get_cp();
        if ( vp.line_side_test( lp_last, lp_first ) <= 0 )
            return -1;
    }

    /* if link is very close to linear (very slightly concave) then we will return 1 to avoid poor triangles*/
    if ( concave < 0.0001 )
        return 1;

    /* Otherwise this is a valid concave case and we will require 0 non-bdry edges */
    return 0;
}

double BezierMesh::ear_priority( BezierVertex *v, list<BezierVertex*>::iterator ear, const list<BezierVertex*> &link ) {
    Matrix A(4,4);
    Matrix B(3,3);
    /* p[0]-p[2] are ear control points p[3] is center cp*/
    Point2D p[ 4 ];
    int i;
    list<BezierVertex*>::iterator temp;

    /* Get prev vertex, wrap prev pointer around to back if we are at begin*/
    temp = ear;
    if ( temp == link.begin() ) p[ 0 ] = link.back()->get_cp();
    else p[ 0 ] = (*( --temp ))->get_cp();

    /* record center vertex */
    p[ 1 ] = (*ear)->get_cp();

    /* Get next vertex, wrap next pointer around to front if we are at end */
    temp = ear;
    if ( ++temp == link.end() ) p[ 2 ] = link.front()->get_cp();
    else p[ 2 ] = (*temp)->get_cp();

    /* Check for a concave or almost-flat ear. */
    double lineside = p[ 2 ].line_side_test( p[ 0 ], p[ 1 ] );
    if (lineside <= 0.0) {
#ifdef DEBUG_DEVILLERS
      cout << "[Devillers] ear " << *ear << " concave\n";
#endif
      return std::numeric_limits<double>::infinity();
    }

    /* Check the other side of the flip is also convex: this test should
       be of the opposite sign. */
    lineside = p[ 2 ].line_side_test( p[ 0 ], v->get_cp() );
    if (lineside >= 0.0) {
#ifdef DEBUG_DEVILLERS
      cout << "[Devillers] ear " << *ear << " bad flip\n";
#endif
      return std::numeric_limits<double>::infinity();
    }

    p[ 3 ] = v->get_cp();
    for ( i = 0; i < 3; i++ ) {
        A.m[ i ][ 0 ] = p[ i ].coords[ 0 ];
        A.m[ i ][ 1 ] = p[ i ].coords[ 1 ];
        A.m[ i ][ 2 ] = p[ i ].magsq();
        A.m[ i ][ 3 ] = 1;
        B.m[ 0 ][ i ] = p[ i ].coords[ 0 ];
        B.m[ 1 ][ i ] = p[ i ].coords[ 1 ];
        B.m[ 2 ][ i ] = 1;
    }
    A.m[ 3 ][ 0 ] = p[ 3 ].coords[ 0 ];
    A.m[ 3 ][ 1 ] = p[ 3 ].coords[ 1 ];
    A.m[ 3 ][ 2 ] = p[ 3 ].magsq();
    A.m[ 3 ][ 3 ] = 1;
#ifdef DEBUG_DEVILLERS
      cout << "[Devillers] ear " << *ear << " has weight "
        << A.det() / B.det() << endl;
#endif
    return A.det() / B.det();
}

int BezierMesh::devillers( BezierVertex *v, list<BezierVertex*> &link, int stop_degree ) {
    PQueue<list<BezierVertex*>::iterator> queue;
    list<BezierEdge*> edges_to_flip;
    list<BezierVertex*>::iterator i, beg, end, top, next, next_ear, prev_ear, temp;
    bool bdry, is_next_ear, is_prev_ear;
    int final_degree;

    bdry = v->is_boundary();

    assert( link.size() >= 2 );
    if ( link.size() <= (unsigned) stop_degree ) return link.size();

    /* create priority queue only insert beg and end-1 if not on the bdry*/
    beg = link.begin();
    for ( i = beg, next = ++link.begin(); i != link.end(); ++i, ++next ) {
        if ( !bdry || ( i != beg && next != link.end() ) )
            queue.push(i, ear_priority( v, i, link ));
    }

    while ( queue.size() > stop_degree ) {
        /* Remove best flip (top) from queue and add to the edges to flip */
        double topval = queue.front->p;
        queue.pop(top);
        assert( find_common_edge( v, *top) != NULL);

        /* break if we're down to flipping edges to flat. */
        if (topval == std::numeric_limits<double>::infinity()) {
#ifdef DEBUG_DEVILLERS
          cout << "[Devillers] power distance " << topval << " ; not flipping "
            << *top << endl;
#endif
          queue.push(top, topval);
          assert(queue.size() <= 4);
          break;
        }
        edges_to_flip.push_back( find_common_edge( v, *top ) );
#ifdef DEBUG_DEVILLERS
        cout << "[Devillers] power distance " << topval << " ; flipping "
          << *top << " (edge " << edges_to_flip.back() << ")\n";
#endif
        next = top;
        ++next;

        beg = link.begin();
        end = link.end();

        is_next_ear = false;
        is_prev_ear = false;

        /*  If there is a next ear, find it */
        if ( !bdry || next != end ) {
            is_next_ear = true;
            if ( next == end ) next_ear = beg;
            else next_ear = next;
        }

        /* If there is a prev ear, find it */
        temp = top;
        if ( !bdry || top != beg ) {
            is_prev_ear = true;
            if ( top == beg ){
                 while(next != end){ temp = next++;}
                 prev_ear = temp;
                 assert(++temp == link.end());
            }
            else {
                prev_ear = --temp;
            }
        }

        /* remove top from link, so that ears prev and next will now be correct */
        link.erase( top );

        /* remove, and reinsert prev and next */
        if ( is_next_ear && queue.remove( next_ear ) ) queue.push( next_ear, ear_priority( v, next_ear, link ));
        if ( is_prev_ear && queue.remove( prev_ear ) ) queue.push( prev_ear, ear_priority( v, prev_ear, link ));
    }

    final_degree = queue.size();

#ifdef DEBUG_DEVILLERS
    while(queue.size() != 0) {
      double topval = queue.front->p;
      queue.pop(top);
      cout << "[Devillers] power distance " << topval << " ; won't flip "
        << *top << endl;
    }
#endif

    /* do flips in order*/
    #ifdef DEBUG_DEVILLERS
    cout << "Begin Devillers: stop degree " << stop_degree
         << "; actually stopping at " << final_degree << endl;
    #endif
    int nflips = 0;
    while( !edges_to_flip.empty() ){
        BezierTuple tup;
        bool success;
        BezierEdge *toflip = edges_to_flip.front();
        edges_to_flip.pop_front();

        success = flip(toflip, tup);
        assert(success);
        nflips++;
    }
    #ifdef DEBUG_DEVILLERS
    cout << "End Devillers: stop degree " << stop_degree
         << "; actually stopped at " << final_degree
         << " after " << nflips << " flips.\n";
    #endif
    return final_degree;
}

BezierTuple BezierMesh::remove_vertex(BezierVertex *v)
{
    BezierTuple tup;
    list<BezierVertex*> link;

    /* Check if d0 vertex, in this case we cannot remove */
    switch(v->containment_dimension()) {
      case 0:
        return get_tuple(v);

      case 1:
        {
        bool one_sided;
        int stop_degree1, stop_degree2=-1;
        BezierTuple tup2;
        list<BezierVertex*> link2;
        BezierVertex *top, *bot, *left=NULL, *right;
        BezierEdge *etop, *ebot, *espan, *nedge;
        BezierTriangle *nt_left, *nt_right;
        BoundaryFace *face1, *face2=NULL;
        BoundaryEdge *bdryedge;
        QBSpline *spline;
        ControlPoint nedge_center_cp;
        LinearData newd;
        double knot_to_remove;
        double u0, u1;

        bdryedge = v->get_bdry_edge();
        spline = bdryedge->get_spline();
        knot_to_remove = v->get_u();

        /* If spline is too small to have a knot removed then fail */
        if((spline->closed && spline->get_num_deboor() < 6) || (spline->get_num_deboor() < 4)) return tup;

        /* Mesh before coarsening in double sided case
         *         ------][<<____________
         *        /      o top           \
         *       /       \\e      /       \
         *      /         \\t    /         |l
         *     /           \\o  /          |i
         *    l|stop_deg2=0 \\p/           |n
         *    i|    ---------v----------   |k
         *    n|            //\stop_deg1=1 |
         *    k| face2     //e \           /
         *    2|          //b   \    face1/
         *     |         //o     \       /
         *      \       //t       \     /
         *       \      o bot          /
         *        \___>>][____________/
         *
         */
        tup = get_tuple(v);
        face1 = tup.f->get_bdry_face_or_null();
        get_link(tup,link);

        top=link.back();
        bot=link.front();
        etop=find_common_edge(v, top);
        ebot=find_common_edge(v, bot);
        /* new data for edge is average. possible error here? */
        newd = (etop->get_dp(1) + ebot->get_dp(1))*0.5;

        /* Stop degree is number of non-bdry edges adjacent to link after devillers
           if negative then vertex cannot be removed */
        stop_degree1 = check_bdry_vertex_removal_degeneracies(link, v);
        if( stop_degree1 < 0) return tup;

        /* Determine if there is mesh on other side of bdry */
        one_sided = ebot->num_faces() < 2;

        if(!one_sided){
            /* get tuple on other side of bdry */
            /* I _KNOW_ that using get_tuple(v,e) is safe thanks to earlier checks for 2-sidedness*/
            tup2 = Switch(2, get_tuple(v, ebot));
            face2 = tup2.f->get_bdry_face_or_null();
            get_link(tup2,link2);

            /* Make sure links sync up correctly*/
            assert(link2.back() == link.front());
            assert(link2.front() == link.back());

            /* stop_degree2 is number for other side, again cannot remove is stop_degree2 is negative */
            stop_degree2 = check_bdry_vertex_removal_degeneracies(link2, v);
            if( stop_degree2 < 0) return tup;

            /* cannot both be concave */
            assert(stop_degree1>0 || stop_degree2>0);
        }

        devillers(v, link, stop_degree1);
        if(!one_sided) devillers(v, link2, stop_degree2);

        tup=get_tuple(v, ebot);
        if(!one_sided){
            tup2=get_tuple(v, etop);
        }

        /* This is a diagram of the mesh after devillers for a two sided
         *  bdry where the left side had stop_degree2=0 and right stop_degree1=1
         *
         *              top
         *               O \            x=tup
         *              /|\\  \         y=tup2
         *             / | \\e  \
         *            /  |  \\t   \
         *           /  e|   \\o    \
         *   face2  /   s|    \\p     \     face1
         *         /    p|     \\       \
         *        /     a|     y\\        \
         *  left o      n|       v---------o right
         *       \       |      //x       /
         *        \      |    e//       /
         *         \     |   b//      /
         *          \    |  o//     /
         *           \   | t//    /
         *            \  | //   /
         *             \ |//  /
         *               O /
         *              bot
         * now find left, right and espan
         */
        espan=NULL;
        if(stop_degree1==0){
            tup = Switch(2, Switch(1, Switch(0, tup)));
            espan = tup.e;
        }
        tup = Switch(0, Switch(1, tup));
        right = tup.v;

        if(!one_sided){
            if(stop_degree2==0){
                tup2 = Switch(2, Switch(1, Switch(0, tup2)));
                espan = tup2.e;
            }
            tup2 = Switch(0, Switch(1, tup2));
            left = tup2.v;
        }
        /* All data is acquired, so remove v and espan(if it exists) */
        if(espan) delete_edge(espan);
        delete_vertex(v);

        /* Remove the knot, this function will return the new center CP for the
         * new edges, it will also return the u0 and u1 values for the edge
         * We can't rely on top and bot for these u values, because if they are
         * D0 points then they don't have a u value
         */
        spline->remove_knot(knot_to_remove, nedge_center_cp, u0, u1 );

        /* Create new edges and triangles, and set bdry info for edge */
        nedge = add_bezier_edge(nedge_center_cp, newd, bot, top);
        nedge->set_bdry(bdryedge, bot->get_u(bdryedge,top), top->get_u(bdryedge,bot));

        nt_right = add_bezier_triangle(face1, nedge, find_common_edge(bot, right), find_common_edge(top, right), true); // CW: tp517 4/16/07
        if(!one_sided) nt_left = add_bezier_triangle(face2, nedge, find_common_edge(bot, left), find_common_edge(top, left)); // CCW: tp517 4/16/07

        return get_tuple(top, nt_right);
        }

      case 2:
        {

          /* Otherwise we are Containment dimension 2 (not on bdry)
             Normally we'd set stop_degree to 3, but we want to deal
             with the case of removing a vertex that is exactly on
             an edge that will be created (i.e. if we back out of
             insert_edge_midpoint).  So we set it to 4, triangulate
             that, and flip if necessary. */

          /* get link and flip out */
          tup = get_tuple(v);
          get_link(tup, link);
          int stop_degree = devillers(v, link, 3);
          assert(stop_degree == 3 || stop_degree == 4);

          /* Collect the new cavity of size stop_degree:
           *  v0  -- e3 -- v3             v0
           *  |             |            /  \
           *  e0           e2   or     e0    e2
           *  |             |         /        \
           *  v1  -- e1 -- v2       v1 -- e1 -- v2
           * In the case of stop_degree 4, we need to retriangulate.
           * Remember that we got into the case of degree 4 because
           * a triangle was almost flat; be careful not to create that
           * triangle.
           */
          tup = get_tuple(v);
          BoundaryFace *face = tup.f->get_bdry_face_or_null();
          BezierVertex *vlink[stop_degree];
          BezierEdge *elink[stop_degree];
          tup=Switch(1, Switch(0, tup));
          for(int i = 0; i < stop_degree; i++) {
            vlink[i] = tup.v;
            elink[i] = tup.e;
            tup=Switch(1, Switch(2, Switch(1, Switch(0, tup))));
          }
      assert(tup.v == vlink[0]);

          /* Remove vertex */
          delete_vertex(v);

      /* Now create 1-2 new triangles in void */

      // a little examination to get the orientation of canonical edge elink[0]
      bool e0_is_cw = (elink[0]->get_canon_vertex() != vlink[0]);

          /*
           * Just create the new triangle and be done.
           */
          if (stop_degree == 3) {

        BezierTriangle *t = add_bezier_triangle(face, elink[0], elink[1], elink[2], e0_is_cw); 
        //calculated: tp517 4/16/07     
            return get_tuple(t);
          }

      //cout << "Stitching in four new triangles after deletion" << endl;

          /*
           * If the degree was 4, we need to retriangulate the cavity.
           * Create a new cross edge, choosing v0-v2 or v1-v3 depending on
           * which is Delaunay.
           * Remember that the whole point is to avoid creating a flat
           * triangle, so we can't just create a triangle and flip the edge if
           * we created the wrong one.
           */
          if (vlink[0]->get_cp().in_circle_test(vlink[1]->get_cp(),
                vlink[2]->get_cp(), vlink[3]->get_cp())) {

            /* 123 is a bad triangle, so the cross edge must be 02 */
            BezierEdge *cross = add_straight_bezier_edge(vlink[0], vlink[2]);
            add_bezier_triangle(face, elink[0], elink[1], cross, e0_is_cw); //calculated:tp517 4/16/07
            add_bezier_triangle(face, cross, elink[2], elink[3]); // ccw: tp517 4/16/07

        //      cout << "grabbing a souvenir" << endl;

            tup = get_tuple(cross);
          } else {
            /* 123 is a good triangle, so the cross edge must be 13 */
            BezierEdge *cross = add_straight_bezier_edge(vlink[3], vlink[1]);
            add_bezier_triangle(face, elink[0], cross, elink[3], e0_is_cw); //calculated:tp517 4/16/07
            add_bezier_triangle(face, cross, elink[1], elink[2]); //ccw: tp517 4/16/07

            tup = get_tuple(cross);
          }
          return tup;
        }
    }

    /* shouldn't fall through, nor have containment dimension != 0,1,2 */
    assert(0);
    return get_tuple();
}

int BezierMesh::blind_locate_point(Point2D pt, BezierTuple& cellfound,
                                   double &u, double &v)
{
    return locate_point(pt, find_start_triangle(pt), true, cellfound, u, v);
}


BezierTriangle* BezierMesh::find_start_triangle(const Point2D &pt)
{
    BezierTriangle *start=NULL;
    double best_dist = std::numeric_limits<double>::infinity();
    int num_to_check;

    if(get_num_faces() == 0) return NULL;

    /* compute n^(1/3) */
    num_to_check = (int)pow(get_num_faces(), 1.0 / 3.0);
    if(num_to_check < 2) num_to_check=2;

    /* Find a random subset; keep track of the changes we've made and
       undo them after. */
    std::stack<pair<BezierTriangle*,int> > sampler_recovery;
    for(int i=0; i<num_to_check; i++) {
      int r = random() % sampler.size();
      BezierTriangle *t = sampler[r];
      sampler[r] = sampler.back();
      sampler.pop_back();
      sampler_recovery.push(make_pair(t, r));

      Point2D lin_center = (t->get_cp(0) + t->get_cp(2) + t->get_cp(5)) / 3.0;
      double dist = (lin_center - pt).magsq();
      if(dist < best_dist) {
        start = t;
        best_dist = dist;
      }
    }
    while(!sampler_recovery.empty()) {
      BezierTriangle *t = sampler_recovery.top().first;
      int index = sampler_recovery.top().second;
      sampler_recovery.pop();

      sampler.push_back(sampler[index]);
      sampler[index] = t;
    }
    return start;
}

int BezierMesh::locate_point( Point2D pt,
                              BezierTriangle *start,
                              bool over_bdry,
                              BezierTuple& cellfound,
                              double &u,
                              double &v )
{
    assert(start != NULL);

    /* Zero out return values*/
    u=0.0;
    v=0.0;

    /* Locate the point in the underlying linear mesh */
    switch (locate_point_in_linear_mesh(pt, start, over_bdry, cellfound)) {
      case -1:
        /* The point is outside the mesh, or across a boundary. */
        u = 0.5; /* ?!? */
        return -1;

      case 0:
        /* Vertex: we actually hit the vertex exactly, so don't
         * bother searching the quadratic mesh. */
        return 0;

      case 1:
      case 2:
        /* All is good; time to search the quadratic mesh. */
        break;

      default:
        /* No other case in 2-d */
    assert(0);
        return 0;
    }

    /*  
     * by tp517: Forget about exact point location
     * if the point is not in the linear triangle it is supposed to be,
     * then just return the midpoint of the edge in the proper direction
     * rather than a proper bfs to find the real quadratic triangle
     *
     */

#ifdef DEBUG_LOCATOR
        cout << "[BezierMesh]: considering triangle "<< *cellfound.f;
#endif
        /* Look to see if the point is in this triangle */
        int rval =  locate_point_in_quadratic_triangle(pt, cellfound.f, cellfound, u, v);
#ifdef DEBUG_LOCATOR
        cout << "[BezierMesh]: CD("<<pt<<") = " << rval << endl;
#endif
    return rval;
}


int BezierMesh::locate_point_in_quadratic_triangle(const Point2D &pt,
                                                   BezierTriangle *t,
                                                   BezierTuple& cellfound,
                                                   double &u,
                                                   double &v)
{
    double alpha, beta;
    int niter;
    int edge_num;

    /*zero the return values */
    u=0.0;
    v=0.0;

    bool found = t->newton_find(pt, alpha, beta, niter, edge_num);
    assert(edge_num >= -1); assert(edge_num < 3);

    if (!found) {
      u = 0.5;
      if (edge_num >= 0) {
        assert(niter >= 0); // check we converged
        cellfound = get_tuple(t->get_edge(edge_num));
      } else {
        assert(niter < 0); // should only happen if we diverged
        cellfound = get_tuple(t);
      }
      return -1;
    } else {
      assert(niter >= 0); // check we converged

      double w[3]; /* these are in CCW order, whereas alpha/beta are CW */
      w[0] = 1.0 - (beta + alpha);
      w[1] = beta;
      w[2] = alpha;
      bool is_zero[3];
      int num_zero = 0;
      for(int i = 0; i < 3; i++) {
        if (fabs(w[i]) < 1e-6) { // TODO: newton_zero_tolerance
          is_zero[i] = true;
          num_zero++;
        } else {
          is_zero[i] = false;
        }
      }
      assert(num_zero >= 0); assert(num_zero < 3);

      switch(num_zero) {
        case 2:
          /* Two zeroes => we found a vertex. */
          {
            /* w[0] is for cp[0], w[1] for cp[2], w[2] for cp[5];
               find the vertex to go along with it.  There is no
               parameter to set. */
            static const int cp_idx[3] = { 0, 2, 5 };
            for(int i = 0; i < 3; i++) {
              if (!is_zero[i]) {
                ControlPoint cpi = t->get_control_point(cp_idx[i]);

                BezierTuple tup = get_tuple(t);
                cellfound = tup;
                do {
                  if (cellfound.v->get_control_point() == cpi) {
                    return 0;
                  }
                  cellfound = Switch(1, Switch(0, cellfound));
                } while(cellfound != tup);
                assert(0); // not found?!?
              }
            }
            assert(0); // no non-zero?!?
            return 0;
          }

        case 1:
          /* One zero => we found an edge. */
          assert(edge_num >= 0);
          cellfound = get_tuple(t->get_edge(edge_num), t);
          tri_param_to_edge_param(cellfound.e, cellfound.f, u, alpha, beta);
          v = 0.0;

#ifdef DEBUG_LOCATOR
          /* Make sure we really got u right. */
          {
            double a, b;
            edge_param_to_tri_param(cellfound.e, cellfound.f, u, a, b);
            /*if (!are_equal(a, alpha) || !are_equal(b, beta))*/ {
              cout << "u = " << u << "; edge_num = " << edge_num
                << " => " << *cellfound.e << endl;
              cout << *cellfound.f << endl;
              cout << "alpha = " << alpha << "; beta = " << beta << endl;
              cout << "a     = " << a     << "; b    = " << b    << endl;
              cout << "pt = " << pt
                << "; (alpha,beta) = " << cellfound.f->eval(alpha,beta)
                << "; (a,b) = " << cellfound.f->eval(a,b)
                << "; (u) = " << cellfound.e->eval(u) << endl;
              cout << "t = " << cellfound.f << "; e = " << cellfound.e << endl;
              cout << "t = " << *cellfound.f;
              cout << "e = " << *cellfound.e;
            }
            //assert(are_equal(a, alpha));
            //assert(are_equal(b, beta));
            Point2D p2 = cellfound.f->eval(alpha, beta);
            //assert(are_equal(pt.x(), p2.x()));
            //assert(are_equal(pt.y(), p2.y()));
            p2 = cellfound.e->eval(u);
            //assert(are_equal(pt.x(), p2.x()));
            //assert(are_equal(pt.y(), p2.y()));
          }
#endif
          return 1;

        case 0:
          /* No zeroes => Inside.  Set both parameters. */
          u = alpha;
          v = beta;
          cellfound = get_tuple(t);
          return 2;

        default:
          /* This is dead code because of the asserts before the switch */
          assert(0);
          return 0;
      }
    }
}


int BezierMesh::locate_point_in_linear_mesh(const Point2D &pt,
                                            BezierTriangle *start,
                                            bool over_bdry,
                                            BezierTuple& cellfound)
{
    BezierTuple tup;
    BezierEdge *first_e;
    int hits = 0;

    assert(start);
#ifdef DEBUG_LOCATOR
    cout << "[BezierMesh]: looking for " << pt << " starting at " << *start
      << endl;
#endif

    /* For each triangle, loop around the edges and find one for
     * which the point is on the other side.  If there is none,
     * then the point is inside the triangle, or on it.
     * We'll either have found the edge or vertex that matches the
     * point, or we'll have established that the point is inside the
     * triangle. */
    tup = get_tuple( start );
    first_e=tup.e;
    while(1) {
      hits++;
      assert(hits <= 3 * get_num_faces()); // each edge of every face
      BezierVertex *v0 = tup.v;
      BezierVertex *v1 = Switch(0,tup).v;
#ifdef DEBUG_LOCATOR
      cout << "[BezierMesh]: considering " << *v0 << ", " << *v1 << endl;
#endif

      if (v0->get_cp().machine_equal(pt)) {
        cellfound = get_tuple(v0);
        return 0;
      } else if (v1->get_cp().machine_equal(pt)) {
        cellfound = get_tuple(v1);
        return 0;
      }

      double test = pt.line_side_test(v0->get_cp(), v1->get_cp());

      if(test == 0.0) {
        /* The point is *on* the line of the edge.  So we know exists alpha:
         *   alpha a + (1 - alpha) b = c
         *   alpha = (c - b) / (a - b)
         * (alpha is a scalar; the relation holds for both x and y)
         * We're on the edge if 0<alpha<1.
         */
        Point2D b = v1->get_cp();
        Point2D ab = v0->get_cp() - b;
        Point2D cb = pt - b;
        double alpha;
        if (is_zero(ab.x())) { /* avoid divide-by-zero */
          assert(!is_zero(ab.y())); /* that would mean a == b */
          alpha = (cb.y() / ab.y());
        } else {
          alpha = (cb.x() / ab.x());
        }
        if (alpha >= 0.0 && alpha <= 1.0) {
          cellfound = get_tuple(tup.e);
#ifdef DEBUG_LOCATOR
          cout << "[BezierMesh]: on edge\n";
#endif
          return 1;
        }
        /* otherwise, fall through to the else of the next if (yuck) */
      }
      if(test < 0.0) {
        /* Then our point is to the right of this edge, hence must be
           outside the triangle. */
        BezierEdge *e = tup.e;
#ifdef DEBUG_LOCATOR
        cout << "[BezierMesh]: outside triangle\n";
#endif

        if (e->num_faces() == 1) {
          /* We're about to exit the mesh. */
          cellfound = get_tuple(e);
          return -1;
        }

        if (e->is_boundary() && !over_bdry) {
          /* We're about to cross a boundary, but we're not allowed. */
          cellfound = get_tuple(e);
          return -1;
        }

        /* Otherwise we move to the next triangle, keeping track
         * of this edge so we don't consider it again. */
        tup = Switch(1, Switch(2, tup));
        first_e = e;
      } else {
#ifdef DEBUG_LOCATOR
        cout << "[BezierMesh]: maybe inside triangle\n";
#endif
        /* We are inside the triangle, move to next edge */
        tup = Switch(1, Switch(0, tup));

        /* If we have verified each edge around the triangle then the point
         * is inside it */
        if(tup.e == first_e){
#ifdef DEBUG_LOCATOR
          cout << "[BezierMesh]: definitely inside triangle\n";
#endif
          cellfound = get_tuple(tup.f);
          return 2;
        }
      }
    }
    /*Point was not located */
    assert(0);
    return 0;
}


#ifdef false
/* Set all the internal degrees of freedom in new_triangles
 * in order to best interpolate the function defined on old_triangles
 * old_triangles must geometrically contain new_triangles
 * size of new_triangles and old_triangles are assumed to be relatively small
 * Should run in roughly O(|new_triangles| * |old_triangles|)
 */
void reinterpolate(list<BezierTriangle*> new_triangles,list<GhostTriangle*> old_triangles)
{
    int i,j;
    list<LinearData*> new_data;

    list<BezierEdge*> free_edges;
    /* Determine which edges of the new_triangles are free */
    for (list<BezierTriangle*>::iterator it = new_triangles.begin();
     it != new_triangles.end(); ++it)
      {
    BezierTriangle *bt = *it;
    for (int j = 0; j < 3; j++)
      {
        BezierEdge *be = bt->get_edge(i);
        BezierTuple *t = get_tuple(be,bt);
        t = Switch(2,t);
        BezierTriangle *ot = t.f;
        list<BezierTriangle*>::iterator result = find(new_triangles.begin(),new_triangles.end(),ot);
        if (result != new_triangles.end())
          {
        free_edges.push_back(be);
          }
      }
      }
    free_edges.unique();

    // This section can be ignored if there are no internal vertices in the cavity

    list<BezierVertex*> free_vertices;
// Determine which vertices of the new_triangles are free
    for (list<BezierTriangle*>::iterator it = new_triangles.begin();
     it != new_triangles.end(); ++it)
      {
    BezierTriangle *bt = *it;
    for (int j = 0; j < 3; j++)
      {
        BezierVertex *be = bt->get_vertex(i);
        BezierTuple *t = get_tuple(be,bv);

        bool free = true;
        //foreach triangle ot around vertex
        //BezierTriangle *ot = t.f;
        {
          list<BezierTriangle*>::iterator result = find(new_triangles.begin(),new_triangles.end(),ot);
          if (result == new_triangles.end())
        {
          free = false;
          break;
        }
        }
      }
      }
    free_vertices.unique();

// zero out all the data points that we plan to set
    for (list<BezierEdge*>::iterator it = free_edges.begin();
     it != free_edges.end(); it++)
      {
    BezierEdge *fe = *it;
    LinearData data = &fe->access_dp(1);
    data->zero();
      }
    for (list<BezierVertex*>::iterator it = free_vertices.begin();
     it != free_vertices.end(); it++)
      {
    BezierEdge *fv = *it;
    LinearData data = &fv->access_dp();
    data->zero();
      }

    /* Now find the triangle's orientation with respect to the edge, e
     * the tri_orientation vector is booleans used by BezierMesh::phi_evaluate to
     * choose two of the factors (alpha, beta, gamma) in order to form the correct
     * basis function, phi, on that edge, given the orientation
     */
    bool tri_orientation[2][3];
    double v0alpha,v0beta, v1alpha,v1beta;
    for(i=0; i< 2; i++){
        edge_param_to_tri_param(e,newt[i],0,v0alpha,v0beta);
        edge_param_to_tri_param(e,newt[i],1,v1alpha,v1beta);
        tri_orientation[i][0]= (v0alpha != v1alpha);
        tri_orientation[i][1]= (v0beta  != v1beta);
        tri_orientation[i][2]= !(tri_orientation[i][0] && tri_orientation[i][1]);
    }

    /* Integrate using 7 point Gaussian quadrature, ALPHA, BETA and WEIGHT are
     * stored as global constants
     */
    double jacobian_det;
    double alpha, beta;
    BezierTriangle *t;

    double final_weight = 1.0;
    LinearData interp_err(data->length); /* temporary */
    bool successful_interpolation;

    LinearData ErrInt(data->length);
    double PhiInt=0.0;

    LinearData TotalErrInt(data->length);
    double TotalPhiInt=0.0;

    for(i=0; i < 2; i++){
        t = newt[i];
        for(j=0; j < 7; j++){
            alpha = ALPHA[j];
            beta  = BETA[j];
            jacobian_det = fabs(t->jacobian(alpha, beta)) * WEIGHT[j];

            /* Only use contribution of Gauss point if 't' is not inverted at this point */
            /* Sometimes an inverted triangle 't' is created by a flip in order to do a vertex
             * removal, so we ignore the contribution at this point
             */
            successful_interpolation = interpolant_error(t, oldt, alpha, beta, interp_err);
            if(successful_interpolation){
                ErrInt += interp_err * jacobian_det;
                PhiInt += phi_evaluate(tri_orientation[i], alpha, beta) * jacobian_det;
            }
            else {
                final_weight -= WEIGHT[j];
            }
        }

        /* Incase one of the Gauss points was ignored, correct the weighting to reflect the total
         * weight of all used points */
        TotalErrInt += ErrInt * final_weight;
        TotalPhiInt += PhiInt * final_weight;
    }

    /* Set edge data point */
    *data = TotalErrInt * (1.0 / TotalPhiInt);
}
#endif


void BezierMesh::reinterpolate(BezierEdge* e, BezierTriangle **newt, GhostTriangle **oldt){
    int i,j;
    LinearData *data;
    /* retrieve edges data point and zero it out */
    data = &e->access_dp(1);
    data->zero();

    /* if no data, then no need to integrate */
    if(data->length==0) return;


    /* Now find the triangle's orientation with respect to the edge, e
     * the tri_orientation vector is booleans used by BezierMesh::phi_evaluate to
     * choose two of the factors (alpha, beta, gamma) in order to form the correct
     * basis function, phi, on that edge, given the orientation
     */
    bool tri_orientation[2][3];
    double v0alpha,v0beta, v1alpha,v1beta;
    for(i=0; i< 2; i++){
        edge_param_to_tri_param(e,newt[i],0,v0alpha,v0beta);
        edge_param_to_tri_param(e,newt[i],1,v1alpha,v1beta);
        tri_orientation[i][0]= (v0alpha != v1alpha);
        tri_orientation[i][1]= (v0beta  != v1beta);
        tri_orientation[i][2]= !(tri_orientation[i][0] && tri_orientation[i][1]);
    }

    /* Integrate using 7 point Gaussian quadrature, ALPHA, BETA and WEIGHT are
     * stored as global constants
     */
    double jacobian_det;
    double alpha, beta;
    BezierTriangle *t;

    double final_weight = 1.0;
    LinearData interp_err(data->length); /* temporary */
    bool successful_interpolation;

    LinearData ErrInt(data->length);
    double PhiInt=0.0;

    LinearData TotalErrInt(data->length);
    double TotalPhiInt=0.0;

    for(i=0; i < 2; i++){
        t = newt[i];
        for(j=0; j < 7; j++){
            alpha = ALPHA[j];
            beta  = BETA[j];
            jacobian_det = fabs(t->jacobian(alpha, beta)) * WEIGHT[j];

            /* Only use contribution of Gauss point if 't' is not inverted at this point */
            /* Sometimes an inverted triangle 't' is created by a flip in order to do a vertex
             * removal, so we ignore the contribution at this point
             */
            successful_interpolation = interpolant_error(t, oldt, alpha, beta, interp_err);
            if(successful_interpolation){
                ErrInt += interp_err * jacobian_det;
                PhiInt += phi_evaluate(tri_orientation[i], alpha, beta) * jacobian_det;
            }
            else {
                final_weight -= WEIGHT[j];
            }
        }

        /* Incase one of the Gauss points was ignored, correct the weighting to reflect the total
         * weight of all used points */
        TotalErrInt += ErrInt * final_weight;
        TotalPhiInt += PhiInt * final_weight;
    }

    /* Set edge data point */
    *data = TotalErrInt * (1.0 / TotalPhiInt);
}

double BezierMesh::phi_evaluate(bool *tri_orientation, double alpha, double beta){
    double val;
    val=2.0;
    if(tri_orientation[0]) val *= alpha;
    if(tri_orientation[1]) val *= beta;
    if(tri_orientation[2]) val *= 1-alpha-beta;
    return val;
}

bool BezierMesh::interpolant_error(BezierTriangle *t, GhostTriangle **oldt, double alpha, double beta, LinearData &interp_err){
    Point2D pt;
    int ghost_triangle_index;
    double oldt_alpha, oldt_beta;
    LinearData oldt_data;

    pt = t->eval(alpha,beta);
    ghost_triangle_index = 0;

    if( !oldt[ ghost_triangle_index ]->newton_find(pt, oldt_alpha, oldt_beta) ){
        ghost_triangle_index++;
        if( !oldt[ghost_triangle_index]->newton_find(pt,oldt_alpha,oldt_beta)){
            return false;
        }
    }
    /* Return olddata - newdata */
    interp_err = oldt[ghost_triangle_index]->dataeval(oldt_alpha, oldt_beta) - t->dataeval(alpha,beta);
    return true;
}

double BezierMesh::function_angle(BezierEdge *e, double u, int func_idx)
{
    if (e->num_faces() < 2) {
      /* no function angles defined on boundaries */
      return 0.0;
    }

    double v = 1.0 - u;
    BezierTriangle *t0 = *e->begin_faces();
    BezierTriangle *t1 = *++e->begin_faces();
    if( !(t0 && t1) ) return  0.0;


    ControlPoint dummy;
    ControlPoint t0_cp3, t0_cp4;
    ControlPoint t1_cp3, t1_cp4;
    tri_cps_off_edge(e, t0, t0_cp3, t0_cp4, dummy);
    tri_cps_off_edge(e, t1, t1_cp3, t1_cp4, dummy);

    /* plane0 is normal plane on t0 at parameter u along e
     * it is defined by p0, p1, p2
     *
     * plane1 is normal plane on t1 at parameter u along e
     * it is defined by p1, p2, p3
     *
     * p1 and p2 are on edge
     */
    double data;
    data = (*datastore->get_data(t0_cp3))[func_idx]*u
         + (*datastore->get_data(t0_cp4))[func_idx]*v;
    Vec3D p0( ((*t0_cp3)*u + (*t0_cp4)*v), data);

    data = e->get_dp(0)[func_idx] * u
         + e->get_dp(1)[func_idx] * v;
    Vec3D p1( (e->get_cp(0)*u + e->get_cp(1)*v), data);

    data = e->get_dp(1)[func_idx] * u
         + e->get_dp(2)[func_idx] * v;
    Vec3D p2( (e->get_cp(1)*u + e->get_cp(2)*v), data);

    data = (*datastore->get_data(t1_cp3))[func_idx]*u
         + (*datastore->get_data(t1_cp4))[func_idx]*v;
    Vec3D p3( ((*t1_cp3)*u + (*t1_cp4)*v), data);

    Vec3D v0 = p0-p1;
    Vec3D v1 = p2-p1;
    Vec3D v2 = p3-p1;
    return (v0.cross(v1)).angle(v1.cross(v2));
}

int BezierMesh::smooth_mesh(double jacobian_bound, int passes)
{
    int num_smoothed = 0;
    BezierMesh::Bezier_Face_Hash_T::iterator i;
    BezierTriangle *t;
    BezierEdge *e;
    int j;
    double min_jacobian, max_jacobian, area;
    double jacobian_diff1, jacobian_diff2;

    for(int p=0; p<passes; p++){
        for(i = get_faces_begin(); i != get_faces_end(); ++i){
            t = *i;
            t->bound_jacobian(min_jacobian, max_jacobian, area);
            jacobian_diff1 = 1 - min_jacobian/area;
            jacobian_diff2 = max_jacobian/area - 1;
            if((min_jacobian < 0) || (max_jacobian < 0) ||
                (jacobian_diff1 >= jacobian_bound) || (jacobian_diff2 >= jacobian_bound)) {

                /* smooth all of t's edges (except boundaries) */
                for(j=0; j<3; ++j){
                    e = t->get_edge(j);
                    if(!e->is_boundary()) {
                        smooth_edge( e );
                        num_smoothed++;
                    }
                }
            }
        }
    }
    return num_smoothed;
}


int BezierMesh::make_delaunay()
{
    return make_delaunay(0.0,0.0);
}

int BezierMesh::make_delaunay(deque<BezierEdge*> flip_list)
{
  return make_delaunay(flip_list, 0.0, 0.0);
}


int BezierMesh::make_delaunay(double angle_const, double min_area_const)
{
  deque<BezierEdge*> flip_list;
  /* insert all edges into dequeue */
  flip_list.insert(flip_list.begin(), get_edges_begin(), get_edges_end());

  return  make_delaunay(flip_list, angle_const, min_area_const);
}

/* !\brief Make the mesh delaunay, assuming that the only non-delaunay edges
 * are those in \a flip_list
 */
int BezierMesh::make_delaunay(deque<BezierEdge*> flip_list, double angle_const, double min_area_const)
{
    int num_flips=0;

    BezierEdge *e;
    BezierTuple tup;
    bool success;
    //    unsigned num_failures=0;

    /* variables for a new delaunay alg */
    list<BezierEdge*> local_edges;
    vector<BezierEdge*> new_edges;
    list<BezierEdge*>::iterator i;
    vector<BezierEdge*>::iterator j;
    //    unsigned num_splits=0;
    //    bool check_func_angles = (angle_const > 0.0);

    keep_trash();
    while( !flip_list.empty() ){
        e = flip_list.front();
        flip_list.pop_front();
        if( !is_member(e) ) continue;
        if( e->is_boundary() ) continue;
        if( !should_flip(e) ) continue;

#ifdef NEW_DELAUNAY_ALG
        /* don't flip edges with a big function angle, as we will loose data */
        if(check_func_angles && should_refine_func_angle(e, angle_const, min_area_const)){
            num_splits++;
            split_edge_near_unflippable(e, new_edges);
            continue;
        }
#endif

        success = flip(e, tup);

#ifdef NEW_DELAUNAY_ALG
    /* If we couldn't flip this edge for geometric reasons */
        if(!success){
      num_failures++;
      num_splits++;
      split_edge_near_unflippable(e, new_edges);
    } else { // successful flip
            num_flips++;
            find_adjacent_edges(tup.e, flip_list);
        }
#else
            num_flips++;
            find_adjacent_edges(tup.e, flip_list);
#endif
    }
    empty_trash();
    return num_flips;
}

int BezierMesh::protect_boundarys()
{
    int num_refines=0;
    deque<BezierEdge*> edges_list;
    BezierEdge *e;
    BezierVertex *v;
    BezierTuple tup;
    bool double_sided;
    Point2D p0, p1;

    keep_trash();
    copy(get_edges_begin(), get_edges_end(), back_inserter(edges_list));
    while( !edges_list.empty() ){
        e = edges_list.front();
        edges_list.pop_front();
        if( !is_member(e) || !e->is_boundary() ) continue;

        /* Check opposite vertex of first tri for encroachment */
        tup = Switch(0, Switch(1, get_tuple(e)));
        if( tup.e->is_boundary() || Switch(1, tup).e->is_boundary())
          continue; /* no encroachment on neighboring boundaries */
        p0 = tup.v->get_cp();

        /* Check opposite vertex of second tri for encroachment */
        double_sided = e->num_faces() == 2;
        if(double_sided) {
            tup = Switch(0, Switch(1, Switch(2, get_tuple(e))));
            if(tup.e->is_boundary() || Switch(1, tup).e->is_boundary())
              continue; /* no encroachment on neighboring boundaries*/
            p1 = tup.v->get_cp();
        }

        if(e->is_encroached(p0) || (double_sided && e->is_encroached(p1)) ){
            v = clean_insert_edge_midpoint(e, 0.5);
            num_refines++;
            enqueue_edges(v, edges_list);
        }
    }
    empty_trash();
    return num_refines;
}

int BezierMesh::remove_small_angles()
{
    int num_refines=0;
    deque<BezierTriangle*> refine_queue;
    BezierTriangle *t;
    BezierVertex *vert;

    /* Initialize refine list with all triangles and randomize it */
    copy(get_faces_begin(), get_faces_end(), back_inserter( refine_queue ));

    //    random_shuffle(refine_queue.begin(), refine_queue.end());

    keep_trash();
    while( !refine_queue.empty() ){
        t = refine_queue.front();
        refine_queue.pop_front();
        if( !is_member(t) ) continue;
        if( !t->small_angle() ) continue;

        /*insert cirucmcenter */
        num_refines++;
        vert = clean_insert_circumcenter(t);
    assert(vert != NULL);
        /* Add faces surrounding new vertex into the queue */
        enqueue_faces(vert, refine_queue);
    }
    empty_trash();
    return num_refines;
}

int BezierMesh::remove_large_triangles(double size_const)
{
    int num_refines=0;
    deque<BezierTriangle*> refine_queue;
    BezierVertex *vert;

    /* Initialize refine list with all triangles and randomize it */
    copy(get_faces_begin(), get_faces_end(), back_inserter( refine_queue ));
    //    random_shuffle(refine_queue.begin(), refine_queue.end());

    keep_trash();
    while( !refine_queue.empty() ){
        BezierTriangle *t = refine_queue.front();
        refine_queue.pop_front();
        if( !is_member(t) ) continue;

        /* check for large triangle, correct by factor of 2 * PI to keep this from
         * thrashing with a constant sizing cleaner which interprets the size constant
         * as a ball packing
         */
        if( t->area() < 2.0 * PI * size_const ) continue;

        /*insert cirucmcenter */
        num_refines++;
        vert = clean_insert_circumcenter(t);
        assert(vert != NULL);
    if (vert != NULL) enqueue_faces(vert, refine_queue);
    }
    empty_trash();
    return num_refines;
}






int BezierMesh::coarsen_const_size(double size_const, double lip_const, double nn_const, double dp_epsilon)
{
    EdgeLengthMapT edge_length;
    BallRadiusMapT radius;
    VertexSetT keep;
    CoarsenQueueT queue(get_num_vertices());
    CoarsenKillListT tokill;
    int num_coarsens;

#ifdef CHECK_COARSEN
    cout<<"Called Coarsen_Const_Size"<<endl;
#endif

    coarsen_calculate_edge_lengths(edge_length);
    coarsen_keep_boundary(dp_epsilon, keep);
    coarsen_approximate_sizing_func(size_const, nn_const, queue, keep, edge_length);
    coarsen_make_lipshitz(lip_const, queue, edge_length, radius);
#ifdef CHECK_COARSEN
    cout<<"Finished Coarsening Function Determination"<<endl;
    coarsen_draw_debug(keep, radius, tokill);
#endif
    coarsen_find_verts_to_remove(keep, edge_length, radius, tokill);
#ifdef CHECK_COARSEN
    cout<<"Selected a Delete Set"<<endl;
    coarsen_draw_debug(keep, radius, tokill);
#endif
    num_coarsens = coarsen_remove_verts(tokill);
    return num_coarsens;
}

int BezierMesh::coarsen_func_angle(double size_const, double lip_const, double nn_const, double dp_epsilon,
                                   double angle_const, double min_area_const)
{
    EdgeLengthMapT edge_length;
    BallRadiusMapT radius;
    VertexSetT keep;
    CoarsenQueueT queue(get_num_vertices());
    CoarsenKillListT tokill;
    int num_coarsens;

    coarsen_calculate_edge_lengths(edge_length);
    coarsen_keep_boundary(dp_epsilon, keep);
    coarsen_keep_large_function_angles(angle_const, min_area_const, keep);
    coarsen_approximate_sizing_func(size_const, nn_const, queue, keep, edge_length);
    coarsen_make_lipshitz(lip_const, queue, edge_length, radius);
#ifdef CHECK_COARSEN
    coarsen_draw_debug(keep, radius, tokill);
#endif
    coarsen_find_verts_to_remove(keep, edge_length, radius, tokill);
#ifdef CHECK_COARSEN
    coarsen_draw_debug(keep, radius, tokill);
#endif
    num_coarsens = coarsen_remove_verts(tokill);
    return num_coarsens;
}

int BezierMesh::coarsen_func_angle_debug(double size_const, double lip_const, double nn_const, double dp_epsilon,
                                   double angle_const, double min_area_const)
{
    EdgeLengthMapT edge_length;
    BallRadiusMapT radius;
    VertexSetT keep;
    CoarsenQueueT queue(get_num_vertices());
    CoarsenKillListT tokill;
    int num_coarsens;

    coarsen_calculate_edge_lengths(edge_length);
    coarsen_keep_boundary(dp_epsilon, keep);
    coarsen_keep_large_function_angles(angle_const, min_area_const, keep);
    coarsen_approximate_sizing_func(size_const, nn_const, queue, keep, edge_length);
    coarsen_make_lipshitz(lip_const, queue, edge_length, radius);
    coarsen_draw_debug(keep, radius, tokill);
    coarsen_find_verts_to_remove(keep, edge_length, radius, tokill);
    coarsen_draw_debug(keep, radius, tokill);
    num_coarsens = coarsen_remove_verts(tokill);
    return num_coarsens;
}

void BezierMesh::coarsen_calculate_edge_lengths(EdgeLengthMapT &edge_length)
{
    /* calculate all edge lengths */
    /* This is O( |E| ) = O( |V| ) */
    for(Edge_Hash_T::iterator edge_iter = get_edges_begin();
        edge_iter != get_edges_end(); ++edge_iter) {
        BezierEdge *e = *edge_iter;
        BezierVertex *v0, *v1;
        v0 = e->get_vertex(0);
        v1 = e->get_vertex(1);
        edge_length[e] = (v0->get_cp() - v1->get_cp()).mag();
    }
}

void BezierMesh::coarsen_keep_boundary(double dp_epsilon, VertexSetT &keep)
{
    BoundaryMesh::Boundary_Vertex_Hash_T::iterator bdry_vert_iter;
    BoundaryMesh::Boundary_Edge_Hash_T::iterator bdry_edge_iter;

    /* keep all d0 vertexs (boundary vertexs) */
    for(bdry_vert_iter = boundarymesh->get_vertexs_begin();
        bdry_vert_iter != boundarymesh->get_vertexs_end(); ++bdry_vert_iter) {
        keep.insert( (*bdry_vert_iter)->get_bezier() );
    }

    /* keep all d1 vertexs that douglas puecker says are needed */
    for(bdry_edge_iter = boundarymesh->get_edges_begin();
        bdry_edge_iter != boundarymesh->get_edges_end(); ++bdry_edge_iter) {
        (*bdry_edge_iter)->get_spline()->douglas_peucker(dp_epsilon, keep);
    }
}




void BezierMesh::coarsen_keep_large_function_angles(double angle_const,
                                                    double min_area_const,
                                                    VertexSetT &keep)
{
    /* this will keep points adjacent to edges with steep function angles */

    /* This is a multiplier for the angle_const used by the algorithm
     * I lower the angle const so as to keep edges with a moderate amount of
     * curvature around, as they are likley carying important data
     */
    double angle_adj=0.50;

    /* Area needs slack for exactly the same reason
     */
    double area_adj=0.80;

    angle_const *= angle_adj;
    min_area_const *= area_adj;

    for(Edge_Hash_T::iterator edge_iter = get_edges_begin();
        edge_iter != get_edges_end(); ++edge_iter) {
        BezierEdge *e = *edge_iter;
        if( !should_refine_func_angle(e, angle_const, min_area_const) ) {
          continue;
        }

        /* If this edge would normally be refined, then keep each vertex of
         * this edge */
        for(int i = 0; i < 2 ; ++i) {
          keep.insert(e->get_vertex(i));
        }
    }
}

void BezierMesh::coarsen_approximate_sizing_func(double size_const, double nn_const, CoarsenQueueT &queue,
                                                 VertexSetT &keep, EdgeLengthMapT &edge_length)
{
    double radius_const = sqrt(size_const);

    /* Create initial approximation to radius function, as the minimum of the coarsening function,
     * and the nearest neighbor function, also ensure that keep nodes have small enough radii, so that
     * they will always be in the final ball packing
     *
     * This is O( |V| )
     */
    for(Vertex_Hash_T::iterator vert_iter = get_vertexs_begin();
        vert_iter != get_vertexs_end(); ++vert_iter) {
        BezierVertex *v = *vert_iter;

        /* if this is a keeper then make ball half of the dist to nearest neighbor */
        if(keep.find(v) != keep.end()){
            /* find min dist to neighbor */
            double min = std::numeric_limits<double>::infinity();
            for(BezierVertex::edge_iterator it = v->begin_edges();
                it != v->end_edges(); ++it) {
                BezierEdge *e = *it;
                double dist = edge_length[e] * 0.49;
                if(dist < min) min = dist;
            }
            queue.decrease(v, min);
        }

        /* Otherwise this is not a keep node*/
        /* Let N(v) be the set of neighbors of v
         * Set the function to min{ c(v), min{ |v-v0|*nnc : v0 in N(v) } }
         * in this case c(v) = radius_const
         */
        else {
            double min = radius_const; /* replace radius const with coarsening function if not using constant coarsening */
            for(BezierVertex::edge_iterator it = v->begin_edges();
                it != v->end_edges(); ++it) {
                BezierEdge *e = *it;
                double dist = edge_length[e] * nn_const;
                if( dist < min) min = dist;
            }
            queue.decrease(v, min);
        }
    }
}


void BezierMesh::coarsen_make_lipshitz(double lip_const, CoarsenQueueT &queue,
                                       EdgeLengthMapT &edge_length, BallRadiusMapT &radius)
{
    BezierEdge *e;
    BezierVertex *v= NULL;
    BezierVertex *v0;
    double r = 0.0;

    while( !queue.empty() ){
        queue.rem_min(v, r);
        for(BezierVertex::edge_iterator it = v->begin_edges();
            it != v->end_edges(); ++it) {
            e = *it;
            v0 = get_opposite_vertex(e, v);
            queue.decrease(v0, r + lip_const * edge_length[e]);
        }
        radius[v] = r;
    }
}

void BezierMesh::coarsen_find_verts_to_remove(VertexSetT &keep, EdgeLengthMapT &edge_length,
                                              BallRadiusMapT &radius, CoarsenKillListT &tokill)
{
    /* find maximal non-overlapping set of vertexs, including all original keep points
    * This is O( |V| )
    */
    for(Vertex_Hash_T::iterator vert_iter = get_vertexs_begin();
        vert_iter != get_vertexs_end(); ++vert_iter) {
        BezierVertex *v = *vert_iter;
        if(keep.find(v) != keep.end()) continue;

        bool should_keep = true;
        double r = radius[v];
        for(BezierVertex::edge_iterator it = v->begin_edges();
                it != v->end_edges(); ++it) {
            BezierEdge *e = *it;
            BezierVertex *v0 = get_opposite_vertex(e, v);
            if(keep.find(v0) != keep.end()){
                if(r + radius[v0] > edge_length[e]){
                    should_keep=false;
                    break;
                }
            }
        }
        if(should_keep) keep.insert(v);
        else tokill.push_back(v);
    }
}


int BezierMesh::coarsen_remove_verts(CoarsenKillListT &tokill)
{
    int num_coarsens=0;
    BezierVertex *v;

    /* remove all vertexs to kill */
    while(!tokill.empty()){
        v = tokill.front();
        tokill.pop_front();
        num_coarsens++;
        remove_vertex(v);
    }
    return num_coarsens;
}

void BezierMesh::coarsen_draw_debug(VertexSetT &keep, BallRadiusMapT &radius, CoarsenKillListT &tokill)
{
#ifdef VISUAL_DEBUG
    VertexSetT killset;
    CoarsenKillListT::iterator i;

    Visualization vis(this, this->boundarymesh, false);
    vis.start_draw();
    vis.draw(GREEN,BLUE);
    for(Vertex_Hash_T::iterator vert_iter = get_vertexs_begin();
        vert_iter != get_vertexs_end(); ++vert_iter) {
        BezierVertex *v = *vert_iter;

        /* find if we are in the tokill list*/
        for(i=tokill.begin(); i!=tokill.end(); ++i){
            if(*i == v){
                killset.insert(v);
                break;
            }
        }
    }

    /* Draw yellow for the keepers and grey for the kills, otherwise don;t draw point */
    /* draw radii for non-kills */
    for(Vertex_Hash_T::iterator vert_iter = get_vertexs_begin();
        vert_iter != get_vertexs_end(); ++vert_iter) {
        BezierVertex *v = *vert_iter;
        if(killset.find(v) != killset.end()) vis.draw_point(v->get_cp(), GRAY4, 3.0);
        else vis.draw_circle(RED, v->get_cp(), radius[v]);

        if(keep.find(v) != keep.end()) vis.draw_point(v->get_cp(), RED, 3.0);
    }
    vis.finish_draw();


    /* redraw */
    vis.start_draw();
    vis.draw(GREEN, BLUE);
    vis.finish_draw();
#endif
}

int BezierMesh::function_angle_refine(double angle_const, double min_area_const)
{
    list<BezierTriangle*> todo;
    int num_refines=0;

    /* Find triangles that need refined */
    for(Edge_Hash_T::iterator edge_iter = get_edges_begin();
        edge_iter != get_edges_end(); ++edge_iter) {
      BezierEdge *e = *edge_iter;
      if( !should_refine_func_angle(e, angle_const, min_area_const) ) {
        continue;
      }

      /* If we should refine then add the faces to the todo queue */
      todo.insert(todo.end(), e->begin_faces(), e->end_faces());
    }

    /* Refine triangles by adding thier circumcenter.
     * Check to make sure the triangles are still around before working with
     * them as they my have been removed by earlier inserts
     */
    list<BezierTriangle*>::iterator i;
    keep_trash();
    for(i=todo.begin(); i!= todo.end(); ++i){
        BezierTriangle *t = *i;
        if(!is_member(t)) continue;
        clean_insert_circumcenter(t);
        num_refines++;
    }
    empty_trash();
    return num_refines;
}

bool BezierMesh::should_refine_func_angle(BezierEdge *e, double angle_const, double min_area_const)
{
    if(e->is_boundary()) return false; /* don't worry about boundaries */
    if(e->num_faces() != 2) return false; /* must have two faces to have an angle */

    if(func_angle_x_cuttoff > 0.0 && (e->get_cp(0).x() > func_angle_x_cuttoff || e->get_cp(2).x() > func_angle_x_cuttoff)) return false;

    double xangle = (function_angle(e,0.0,0) + function_angle(e,1.0,0) + function_angle(e,0.5,0)) / 3.0;
    double yangle = (function_angle(e,0.0,1) + function_angle(e,1.0,1) + function_angle(e,0.5,1)) / 3.0;

    /* if function angle is less than the cut-off, then these triangles are ok to coarsen, so skip them and move on */
    if((xangle < angle_const) && (yangle < angle_const)) return false;

    /* if these triangles are very small, then they are OK to coarsen, so skip them and move on */
    for(BezierEdge::face_iterator it = e->begin_faces();
        it != e->end_faces(); ++it) {
      if((*it)->area() < min_area_const) {
        return false;
      }
    }

    /* At this point we have an edge with a large angle, who is not part of small triangles*/
    return true;
}

int BezierMesh::find_inverted_triangles(vector<BezierTriangle*> &inverted)
{
    int num_inverted=0;
    BezierTriangle *t;

    for(Face_Hash_T::iterator it = get_faces_begin();
        it != get_faces_end(); ++it) {
        t = *it;
    // make conservative since new adaptive time: tp517 4/26/10
    // (t->is_linear_inverted() || t->is_definitely_inverted())
      if (! (t->is_definitely_not_inverted()) )    
      {
            num_inverted++;
            inverted.push_back(t);
        }
    }
    return num_inverted;
}

int BezierMesh::classify_inverted_triangles(vector<BezierTriangle*> &normal, vector<BezierTriangle*> &inverted, vector<BezierTriangle*> &unknown)
{
    int num_inverted=0;
    int num_unknown=0;
    int num_normal=0;
    int num_errors=0;
    BezierTriangle *t;
    int status;

    for(Face_Hash_T::iterator it = get_faces_begin();
        it != get_faces_end(); ++it) {
        t = *it;
        status = t->is_inverted();
        switch(status){
            case 1:
                num_inverted++;
                inverted.push_back(t);
                break;
            case 0:
                num_normal++;
                normal.push_back(t);
                break;
            case -1:
                num_unknown++;
                unknown.push_back(t);
                break;
            case -2:
                num_errors++;
        }
    }
    return num_inverted;
}


bool BezierMesh::check_consistency() const
{

    /* assert the cp and dp pointers all match up */
    bool is_good = true;
    for(Face_Hash_T::const_iterator it = get_faces_begin();
        it != get_faces_end(); ++it) {
        const BezierTriangle *t = *it;

        for(int i=0; i<2; i++){
            const BezierEdge *e = t->get_edge(i);
            int orient = orient_edge_triangle(e,t);

            static const int orderings[6][3] = {
              { 0, 1, 2 },
              { 2, 4, 5 },
              { 5, 3, 0 },
              { 2, 1, 0 },
              { 5, 4, 2 },
              { 0, 3, 5 }
            };
            assert(orient >= 0); assert(orient < 6);
            if (!consistency_check_helper(t,e,orderings[orient])) {
              cerr << "Orientation was: " << orient << endl;
              is_good = false;
            }
        }
    }

    /* assert the sampling array is accurate */
    int mismatch = 0;
    for(vector<BezierTriangle*>::const_iterator it = sampler.begin();
        it != sampler.end(); ++it) {
      if (!is_member(*it)) {
        mismatch++;
        cerr << "bad triangle in sampler: " << **it;
      }
    }
    if (mismatch != 0) {
      is_good = false;
      cerr << mismatch << " bad triangles in sampler\n";
    }
    if (sampler.size() != (unsigned)get_num_faces()) {
      is_good = false;
      cerr << "size mismatch in sampler: " << sampler.size() << " should be "
        << get_num_faces() << endl;
    }

    /* assert switch sizes make sense */
    int switch_sizes[3] = {
      get_num_edges() * 2,
      get_num_faces() * 6,
      get_num_faces() * 3
    };
    for(int i = 0; i < 3; i++) {
      if (switch_size(i) != switch_sizes[i]) {
        is_good = false;
        cerr << "switch"<<i<<" has size " << switch_size(i) << " should be "
          << switch_sizes[i] << endl;
      }
    }

    /* bfs through the mesh; assert that we find all the elements */
    {
      hash_set<BezierVertex*> vertices;
      hash_set<BezierEdge*> edges;
      hash_set<BezierTriangle*> triangles;
      stack<BezierTriangle*> todo;
      todo.push(get_tuple().f);
      while(!todo.empty()) {
        BezierTriangle *t = todo.top();
        todo.pop();
        if (triangles.count(t) != 0) {
          continue;
        }
        triangles.insert(t);

        BezierTuple start = get_tuple(t);
        BezierTuple current = start;
        do {
          BezierTriangle *opposite = Switch(2, current).f;

          if (!is_member(current.v)) {
            is_good = false;
            cerr << current.v << " not a member\n";
          }
          if (!is_member(current.e)) {
            is_good = false;
            cerr << current.e << " not a member\n";
          }
          if (!is_member(current.f)) {
            is_good = false;
            cerr << current.f << " not a member\n";
          }
          if (!is_member(opposite)) {
            is_good = false;
            cerr << opposite << " not a member\n";
          }

          vertices.insert(current.v);
          edges.insert(current.e);
          todo.push(opposite);
          current = Switch(1, Switch(0, current));
        } while(current != start);
      }

      if (vertices.size() != (unsigned)get_num_vertices()) {
        is_good = false;
        cerr << "only " << vertices.size() << " vertices reached out of "
          << get_num_vertices() << endl;
      }
      if (edges.size() != (unsigned)get_num_edges()) {
        is_good = false;
        cerr << "only " << edges.size() << " edges reached out of "
          << get_num_edges() << endl;
      }
      if (triangles.size() != (unsigned)get_num_faces()) {
        is_good = false;
        cerr << "only " << triangles.size() << " triangles reached out of "
          << get_num_faces() << endl;
      }
    }

#ifdef DEBUG
    if (!is_good) {
      static int writes = 0;
      char name[4096];
      snprintf(name, sizeof(name), "check_consistency%03d.geo", writes++);
      MeshBinaryOutput::write(name, const_cast<BezierMesh*>(this),
          const_cast<BoundaryMesh*>(boundarymesh));
    }
#endif

    return is_good;
}

bool BezierMesh::consistency_check_helper(const BezierTriangle *t,
                                          const BezierEdge *e,
                                          const int p[3]) const
{
    const BezierVertex *v0, *v1;
    v0 = e->get_vertex(0);
    v1 = e->get_vertex(1);
    ControlPoint v0cp = v0->get_control_point();
    ControlPoint v1cp = v1->get_control_point();
    DataPoint v0dp = v0->get_data_point();
    DataPoint v1dp = v1->get_data_point();

    /* check cp's */
    for(int i = 0; i < 3; ++i) {
      if (e->get_control_point(i) != t->get_control_point(p[i])) {
        cout<<"Edge control points don't coorespond to triangle's"<<endl
            <<"T: "<<*t<<endl
            <<"E: "<<*e<<endl<<endl;
        return false;
      }
    }
    if( (v0cp != e->get_control_point(0)) || (v1cp != e->get_control_point(2))){
        cout<<"Vertex control points don't coorespond to edges's"<<endl
            <<"E: "<<*e<<endl
            <<"V0: "<<*v0<<endl
            <<"V1: "<<*v1<<endl;
        return false;
    }
    if((v0cp != t->get_control_point(p[0])) || (v1cp != t->get_control_point(p[2]))){
        cout<<"Vertex control points don't coorespond to triangle's"<<endl
            <<"T: "<<*t<<endl
            <<"E: "<<*e<<endl
            <<"V0: "<<*v0<<endl
            <<"V1: "<<*v1<<endl;
        return false;
    }


    /* check dp's */
    for(int i = 0; i < 3; ++i) {
      if (e->get_data_point(i) != t->get_data_point(p[i])) {
        cout<<"Edge data points don't coorespond to triangle's"<<endl
            <<"T: "<<*t<<endl
            <<"E: "<<*e<<endl<<endl;
        return false;
      }
    }
    if( (v0dp != e->get_data_point(0)) || (v1dp != e->get_data_point(2))){
        cout<<"Vertex data points don't coorespond to edges's"<<endl
            <<"E: "<<*e<<endl
            <<"V0: "<<*v0<<endl
            <<"V1: "<<*v1<<endl;
        return false;
    }
    if((v0dp != t->get_data_point(p[0])) || (v1dp != t->get_data_point(p[2]))){
        cout<<"Vertex data points don't coorespond to triangle's"<<endl
            <<"T: "<<*t<<endl
            <<"E: "<<*e<<endl
            <<"V0: "<<*v0<<endl
            <<"V1: "<<*v1<<endl;
        return false;
    }
    return true;
}

bool BezierMesh::print_check_inverted(const char *message, int expected)
{
#ifdef DEBUG
    static int writes = 0;
    vector<BezierTriangle*> inverts;
    find_inverted_triangles(inverts);
    int inverted = inverts.size();
    if (!inverts.empty()) {
    cout<< " INVERTED TRIANGLES CREATED\n";
        cout << " -> " << message << endl;
    for (vector<BezierTriangle*>::iterator i = inverts.begin();
         i != inverts.end(); ++i) {
          BezierTriangle *t = *i;
      draw_insert_error("InvertedTriangle",t);
      cout << "Inverted Triangle (" << t->is_inverted() << "): "
            <<(*t)<<endl;
          if(t->is_inverted() == -1) {
            inverted--;
          }
        }
        char name[4096];
        sprintf(name, "print_check_inverted%03d.geo", writes++);
    MeshBinaryOutput::write(name, this, boundarymesh);
    }
    return check_consistency() && inverted == expected;
#endif
    return true;
}

ostream& operator<<( ostream &stream, const BezierMesh &bm)
{
    stream<<"BezierMesh"<<endl;
    stream<<"  boundarymesh:"<<bm.boundarymesh<<endl;
    stream<<"  datastore:"<<bm.datastore<<endl;
    stream<<"  CellComplex:"<<endl;
    stream<<*((BezierComplex*)(&bm))<<endl;
    return stream;
}

int BezierMesh::data_length() const {
  return datastore->data_length();
}

---