/**
 * gts_edge_is_contact:
 * @e: a #GtsEdge.
 *
 * Returns: the number of sets of connected triangles sharing @e as a
 * contact edge.
 */
guint gts_edge_is_contact (GtsEdge * e)
{
    GSList * i, * triangles;
    guint ncomponent = 0;

    g_return_val_if_fail (e != NULL, 0);

    triangles = gts_vertex_triangles (GTS_SEGMENT (e)->v1, NULL);
    i = triangles = gts_vertex_triangles (GTS_SEGMENT (e)->v2, triangles);
    while (i) {
        GTS_OBJECT (i->data)->reserved = i;
        i = i->next;
    }

    i = e->triangles;
    while (i) {
        GtsTriangle * t = i->data;
        if (GTS_OBJECT (t)->reserved) {
            GtsEdge * e1;
            GTS_OBJECT (t)->reserved = NULL;
            e1 = next_edge (t, NULL, e);
            triangle_next (e1, e);
            triangle_next (next_edge (t, e1, e), e);
            ncomponent++;
        }
        i = i->next;
    }

    g_slist_foreach (triangles, (GFunc) gts_object_reset_reserved, NULL);
    g_slist_free (triangles);

    return ncomponent;
}

PygtsSegment * 
pygts_segment_new(GtsSegment *s)
{
  PyObject *args, *kwds;
  PygtsObject *segment;

  /* Check for Segment in the object table */
  if( (segment=PYGTS_OBJECT(g_hash_table_lookup(obj_table,GTS_OBJECT(s))))
      != NULL ) {
    Py_INCREF(segment);
    return PYGTS_FACE(segment);
  }

  /* Build a new Segment */
  args = Py_BuildValue("OO",Py_None,Py_None);
  kwds = Py_BuildValue("{s:O}","alloc_gtsobj",Py_False);
  segment = PYGTS_SEGMENT(PygtsSegmentType.tp_new(&PygtsSegmentType, 
						  args, kwds));
  Py_DECREF(args);
  Py_DECREF(kwds);
  if( segment == NULL ) {
    PyErr_SetString(PyExc_MemoryError,"could not create Segment");
    return NULL;
  }
  segment->gtsobj = GTS_OBJECT(s);

  /* Register and return */
  pygts_object_register(segment);
  return PYGTS_SEGMENT(segment);
}

static void gfs_init_wave_destroy (GtsObject * object)
{
  gts_object_destroy (GTS_OBJECT (GFS_INIT_WAVE (object)->d));
  gts_object_destroy (GTS_OBJECT (GFS_INIT_WAVE (object)->hs));

  (* GTS_OBJECT_CLASS (gfs_init_wave_class ())->parent_class->destroy) (object);
}

PygtsVertex *
pygts_vertex_new(GtsVertex *v)
{
  PyObject *args, *kwds;
  PygtsObject *vertex;

  /* Check for Vertex in the object table */
  if( (vertex = PYGTS_OBJECT(g_hash_table_lookup(obj_table,GTS_OBJECT(v)))) 
      !=NULL ) {
    Py_INCREF(vertex);
    return PYGTS_VERTEX(vertex);
  }

  /* Build a new Vertex */
  args = Py_BuildValue("ddd",0,0,0);
  kwds = Py_BuildValue("{s:O}","alloc_gtsobj",Py_False);
  vertex = PYGTS_VERTEX(PygtsVertexType.tp_new(&PygtsVertexType, args, kwds));
  Py_DECREF(args);
  Py_DECREF(kwds);
  if( vertex == NULL ) {
    PyErr_SetString(PyExc_MemoryError,"could not create Vertex");
    return NULL;
  }
  vertex->gtsobj = GTS_OBJECT(v);

  /* Attach the parent */
  if( (vertex->gtsobj_parent=parent(v)) == NULL ) {
	Py_DECREF(vertex);
	return NULL;
  }

  /* Register and return */
  pygts_object_register(vertex);
  return PYGTS_VERTEX(vertex);
}

static void add_unused (GtsGNode * n, GtsGraph * g2)
{
  if (GTS_OBJECT (n)->reserved)
    GTS_OBJECT (n)->reserved = NULL;
  else
    gts_container_add (GTS_CONTAINER (g2), GTS_CONTAINEE (n));
}

/**
 * gts_bb_tree_point_closest:
 * @tree: a bounding box tree.
 * @p: a #GtsPoint.
 * @closest: a #GtsBBoxClosestFunc.
 * @distance: if not %NULL is set to the distance between @p and the 
 * new #GtsPoint.
 *
 * Returns: a new #GtsPoint, closest point to @p and belonging to an object of
 * @tree.
 */
GtsPoint * gts_bb_tree_point_closest (GNode * tree, 
				      GtsPoint * p,
				      GtsBBoxClosestFunc closest,
				      gdouble * distance)
{
  GSList * list, * i;
  gdouble dmin = G_MAXDOUBLE;
  GtsPoint * np = NULL;

  g_return_val_if_fail (tree != NULL, NULL);
  g_return_val_if_fail (p != NULL, NULL);
  g_return_val_if_fail (closest != NULL, NULL);

  i = list = gts_bb_tree_point_closest_bboxes (tree, p);
  while (i) {
    GtsPoint * tp = (*closest) (p, GTS_BBOX (i->data)->bounded);
    gdouble d = gts_point_distance2 (tp, p);

    if (d < dmin) {
      if (np)
	gts_object_destroy (GTS_OBJECT (np));
      np = tp;
      dmin = d;
    }
    else
      gts_object_destroy (GTS_OBJECT (tp));
    i = i->next;
  }
  g_slist_free (list);

  if (distance)
    *distance = dmin;

  return np;  
}

PygtsFace *
pygts_face_new(GtsFace *f)
{
  PyObject *args, *kwds;
  PygtsObject *face;

  /* Check for Face in the object table */
  if( (face=PYGTS_OBJECT(g_hash_table_lookup(obj_table,GTS_OBJECT(f))))
      != NULL ) {
    Py_INCREF(face);
    return PYGTS_FACE(face);
  }

  /* Build a new Face */
  args = Py_BuildValue("OOO",Py_None,Py_None,Py_None);
  kwds = Py_BuildValue("{s:O}","alloc_gtsobj",Py_False);
  face = PYGTS_OBJECT(PygtsFaceType.tp_new(&PygtsFaceType, args, kwds));
  Py_DECREF(args);
  Py_DECREF(kwds);
  if( face == NULL ) {
    PyErr_SetString(PyExc_MemoryError, "could not create Face");
    return NULL;
  }
  face->gtsobj = GTS_OBJECT(f);

  /* Attach the parent */
  if( (face->gtsobj_parent = parent(f)) == NULL ) {
    Py_DECREF(face);
    return NULL;
  }
  
  /* Register and return */
  pygts_object_register(face);
  return PYGTS_FACE(face);
}

void 
pygts_face_cleanup(GtsSurface * s)
{
  GSList *triangles = NULL;
  GSList * i;

  g_return_if_fail(s != NULL);

  /* build list of triangles */
  gts_surface_foreach_face(s, (GtsFunc) build_list, &triangles);
  
  /* remove duplicate and degenerate triangles */
  i = triangles;
  while(i) {
    GtsTriangle * t = (GtsTriangle*)i->data;
    if (!gts_triangle_is_ok(t)) {
      /* destroy t, its edges (if not used by any other triangle)
	 and its corners (if not used by any other edge) */
      if( g_hash_table_lookup(obj_table,GTS_OBJECT(t))==NULL ) {
	gts_object_destroy(GTS_OBJECT(t));
      }
      else {
	gts_surface_remove_face(PYGTS_SURFACE_AS_GTS_SURFACE(s),GTS_FACE(t));
      }
    }
    i = g_slist_next(i);
  }
  
  /* free list of triangles */
  g_slist_free(triangles);
}

static void add_neighbor (GtsGNode * n, GtsEHeap * heap)
{
  if (GTS_OBJECT (n)->reserved == n)
    return;
  if (GTS_OBJECT (n)->reserved)
    gts_eheap_remove (heap, GTS_OBJECT (n)->reserved);
  GTS_OBJECT (n)->reserved = gts_eheap_insert (heap, n);
}

static void write_edge (GtsSegment * s, guint * ne)
{
  printf ("  GtsEdge * e%u = gts_edge_new (gts_edge_class (), v%u, v%u);\n",
	  *ne, 
	  GPOINTER_TO_UINT (GTS_OBJECT (s->v1)->reserved),
	  GPOINTER_TO_UINT (GTS_OBJECT (s->v2)->reserved));
  GTS_OBJECT (s)->reserved = GUINT_TO_POINTER ((*ne)++);
}

static GtsSurface * happrox_list (GSList * points,
				  gboolean keep_enclosing,
				  gboolean closed,
				  CostFunc cost_func,
				  gpointer cost_data,
				  GtsStopFunc stop_func,
				  gpointer stop_data)
{
  GtsSurface * s = gts_surface_new (gts_surface_class (),
				    GTS_FACE_CLASS (list_face_class ()),
				    gts_edge_class (),
				    gts_vertex_class ());
  GtsTriangle * t;
  GtsVertex * w1, * w2, * w3;
  GtsListFace * f;

  /* creates enclosing triangle */
  t = gts_triangle_enclosing (gts_triangle_class (), points, 10.);
  gts_triangle_vertices (t, &w1, &w2, &w3);
  GTS_POINT (w1)->z = GTS_POINT (w2)->z = GTS_POINT (w3)->z = 
    keep_enclosing ? -10. : -1e30;

  f = GTS_LIST_FACE (gts_face_new (s->face_class, t->e1, t->e2, t->e3));
  gts_surface_add_face (s, GTS_FACE (f));
  f->points = points;

  /* refine surface */
  surface_hf_refine (s, cost_func, cost_data, stop_func, stop_data);

  /* destroy unused vertices */
  gts_surface_foreach_face (s, (GtsFunc) destroy_unused, NULL);
  
  /* destroy enclosing triangle */
  if (!keep_enclosing) {
    gts_allow_floating_vertices = TRUE;
    gts_object_destroy (GTS_OBJECT (w1));
    gts_object_destroy (GTS_OBJECT (w2));
    gts_object_destroy (GTS_OBJECT (w3));
    gts_allow_floating_vertices = FALSE;
  }
  else if (closed) {
    GSList * l = gts_surface_boundary (s);
    GtsFace * f;

    g_assert (g_slist_length (l) == 3);
    f = gts_face_new (s->face_class, l->data, l->next->data, l->next->next->data);
    gts_surface_add_face (s, f);
    if (!gts_face_is_compatible (f, s))
      gts_triangle_revert (GTS_TRIANGLE (f));
    g_slist_free (l);
    gts_object_destroy (GTS_OBJECT (t));
  }
  else
    gts_object_destroy (GTS_OBJECT (t));

  return s;
}

static void write_face (GtsTriangle * t, guint * nf)
{
  printf ("  GtsFace * f%u = gts_face_new (gts_face_class (),\n"
	  "                                e%u, e%u, e%u);\n",
	  (*nf)++, 
	  GPOINTER_TO_UINT (GTS_OBJECT (t->e1)->reserved),
	  GPOINTER_TO_UINT (GTS_OBJECT (t->e2)->reserved),
	  GPOINTER_TO_UINT (GTS_OBJECT (t->e3)->reserved));
}

int main (int argc, char * argv[])
{
  GtsSurface * s;
  GtsBBox * bbox;
  gdouble delta;
  GtsPoint * p1, * p2, * p3;
  guint nt;
  GtsRange cluster_stats;
  GtsClusterGrid * cluster_grid;
  
  if (argc != 2) {
    fprintf (stderr, "usage: oocs DELTA < infile > outfile\n");
    return 1;
  }

  s = gts_surface_new (gts_surface_class (),
		       gts_face_class (),
		       gts_edge_class (),
		       gts_vertex_class ());

  bbox = gts_bbox_new (gts_bbox_class (), s, 0., 0., 0., 0., 0., 0.);
  scanf ("%u", &nt);
  scanf ("%lf %lf %lf", &bbox->x1, &bbox->y1, &bbox->z1);
  scanf ("%lf %lf %lf", &bbox->x2, &bbox->y2, &bbox->z2);
  delta = atof (argv[1])*sqrt (gts_bbox_diagonal2 (bbox));

  cluster_grid = gts_cluster_grid_new (gts_cluster_grid_class (), 
				       gts_cluster_class (), 
				       s, bbox, delta);

  p1 = gts_point_new (gts_point_class (), 0., 0., 0.);
  p2 = gts_point_new (gts_point_class (), 0., 0., 0.);
  p3 = gts_point_new (gts_point_class (), 0., 0., 0.);

  while (scanf ("%lf %lf %lf", &p1->x, &p1->y, &p1->z) == 3 &&
	 scanf ("%lf %lf %lf", &p2->x, &p2->y, &p2->z) == 3 &&
	 scanf ("%lf %lf %lf", &p3->x, &p3->y, &p3->z) == 3)
    gts_cluster_grid_add_triangle (cluster_grid, p1, p2, p3, NULL);
  cluster_stats = gts_cluster_grid_update (cluster_grid);

  gts_object_destroy (GTS_OBJECT (p1));
  gts_object_destroy (GTS_OBJECT (p2));
  gts_object_destroy (GTS_OBJECT (p3));
  gts_object_destroy (GTS_OBJECT (cluster_grid));

  fprintf (stderr, "Initial number of triangles: %u\n", nt);
  fprintf (stderr, "%d clusters of size: min: %g avg: %.1f|%.1f max: %g\n",
	   cluster_stats.n,
	   cluster_stats.min, 
	   cluster_stats.mean, cluster_stats.stddev, 
	   cluster_stats.max);
  gts_surface_print_stats (s, stderr);
  gts_surface_write (s, stdout);

  return 0;
}

/**
 * gts_edge_swap:
 * @e: a #GtsEdge.
 * @s: a #GtsSurface.
 *
 * Performs an "edge swap" on the two triangles sharing @e and
 * belonging to @s.
 */
void gts_edge_swap (GtsEdge * e, GtsSurface * s)
{
    GtsTriangle * t1 = NULL, * t2 = NULL, * t;
    GtsFace * f;
    GSList * i;
    GtsVertex * v1, * v2, * v3, * v4, * v5, * v6;
    GtsEdge * e1, * e2, * e3, * e4;
    GtsSegment * v3v6;

    g_return_if_fail (e != NULL);
    g_return_if_fail (s != NULL);

    i = e->triangles;
    while (i) {
        if (GTS_IS_FACE (i->data) && gts_face_has_parent_surface (i->data, s)) {
            if (!t1)
                t1 = i->data;
            else if (!t2)
                t2 = i->data;
            else
                g_return_if_fail (gts_edge_face_number (e, s) == 2);
        }
        i = i->next;
    }
    g_assert (t1 && t2);

    gts_triangle_vertices_edges (t1, e, &v1, &v2, &v3, &e, &e1, &e2);
    gts_triangle_vertices_edges (t2, e, &v4, &v5, &v6, &e, &e3, &e4);
    g_assert (v2 == v4 && v1 == v5);

    v3v6 = gts_vertices_are_connected (v3, v6);
    if (!GTS_IS_EDGE (v3v6))
        v3v6 = GTS_SEGMENT (gts_edge_new (s->edge_class, v3, v6));
    f = gts_face_new (s->face_class, e1, GTS_EDGE (v3v6), e4);
    if ((t = gts_triangle_is_duplicate (GTS_TRIANGLE (f))) &&
            GTS_IS_FACE (t)) {
        gts_object_destroy (GTS_OBJECT (f));
        f = GTS_FACE (t);
    }
    gts_surface_add_face (s, f);

    f = gts_face_new (s->face_class, GTS_EDGE (v3v6), e2, e3);
    if ((t = gts_triangle_is_duplicate (GTS_TRIANGLE (f))) &&
            GTS_IS_FACE (t)) {
        gts_object_destroy (GTS_OBJECT (f));
        f = GTS_FACE (t);
    }
    gts_surface_add_face (s, f);

    gts_surface_remove_face (s, GTS_FACE (t1));
    gts_surface_remove_face (s, GTS_FACE (t2));
}

static void triangle_next (GtsEdge * e1, GtsEdge * e)
{
    GSList * i;

    i = e1->triangles;
    while (i) {
        GtsTriangle * t = i->data;
        if (GTS_OBJECT (t)->reserved) {
            GTS_OBJECT (t)->reserved = NULL;
            triangle_next (next_edge (t, e1, e), e);
        }
        i = i->next;
    }
}

ofxGtsSurface::~ofxGtsSurface() {
	if(loaded) {
		/* destroy surfaces and intersection */
		gts_object_destroy(GTS_OBJECT(surface));		
		
		if(boolPerformed) {
			gts_object_destroy(GTS_OBJECT(si));
			
			/* destroy bounding box trees (including bounding boxes) */
			gts_bb_tree_destroy(tree1, TRUE);
			gts_bb_tree_destroy(tree2, TRUE);  
		}
	}
		
}

/**
 * gts_graph_bisection_destroy:
 * @bg: a #GtsGraphBisection.
 * @destroy_graphs: controls graph destruction.
 *
 * Frees all the memory allocated for @bg. If @destroy_graphs is %TRUE
 * the graphs created by @bg are destroyed.  
 */
void gts_graph_bisection_destroy (GtsGraphBisection * bg,
				  gboolean destroy_graphs)
{
  g_return_if_fail (bg != NULL);

  g_hash_table_destroy (bg->bg1);
  g_hash_table_destroy (bg->bg2);

  if (destroy_graphs) {
    gts_object_destroy (GTS_OBJECT (bg->g1));
    gts_object_destroy (GTS_OBJECT (bg->g2));
  }

  g_free (bg);
}

static void list_face_update (ListFace * f, gpointer * data)
{
  if (IS_LIST_FACE (f)) {
    GSList * i = GTS_LIST_FACE (f)->points;
    GtsEHeap * heap = data[0];
    CostFunc cost = data[1];
    gpointer cost_data = data[2];
    
    if (i) {
      gdouble maxerr = 0.;
      ListFaceClass * klass = LIST_FACE_CLASS (GTS_OBJECT (f)->klass);

      (* klass->cost_init) (f);
      f->best = NULL;
      while (i) {
	GtsPoint * v = i->data;
	gdouble e = (* cost) (f, v, cost_data);
	
	if (e > maxerr) {
	  maxerr = e;
	  f->best = GTS_VERTEX (v); 
	}
	i = i->next;
      }
      if (f->heap) {
	gts_eheap_remove (f->heap, f->pair);
	f->heap = NULL;
      }
      if (maxerr > 0.) {
	f->pair = gts_eheap_insert_with_key (heap, f, - maxerr);
	f->heap = heap;
      }
    }
  }
}

void ofxGtsSurface::setup(string filename) {

	FILE * fptr;
	GtsFile * fp;
	
	string filePath = ofToDataPath(filename);
	
	/* open first file */
	if ((fptr = fopen (filePath.c_str(), "rt")) == NULL) {		
		ofLog(OF_LOG_ERROR, "Cannot open file: " + filePath);
		return;
	}
	/* reads in first surface file */
	surface = gts_surface_new(GTS_SURFACE_CLASS(gts_surface_class()), 
							  GTS_FACE_CLASS(gts_nface_class()), 
							  GTS_EDGE_CLASS(gts_nedge_class()), 
							  GTS_VERTEX_CLASS(gts_nvertex_class()));
	fp = gts_file_new(fptr);
	if (gts_surface_read (surface, fp)) {
		ofLog(OF_LOG_ERROR, filePath + " is not a valid GTS surface file");
		loaded = false;
		gts_object_destroy(GTS_OBJECT(surface));
	} else {
		ofLog(OF_LOG_NOTICE, "Gts surface file read: " + filePath);
		loaded = true;
	}
	gts_file_destroy (fp);
	fclose (fptr);	
}

static void gts_triangulate_test_stuff()
{
	gdouble v[4][3] = {
		{0.0, 0.0, 0.0},
		{0.0, 3.0, 0.0},
		{3.0, 3.0, 0.0},
		{1.0, 1.0, 0.0}
	};
	
	int i; 
	GtsVertex *vtx[4];
	GCList *polygon = NULL;
	GtsVector orient; 
	GCList *p; 
	
	for (i = 0; i < 4; ++i) {
		vtx[i] = gts_vertex_new(gts_vertex_class(), v[i][0],v[i][1],v[i][2]); 
		polygon = g_clist_append(polygon, vtx[i]); 
	}
	g_assert(g_clist_length(polygon));
	g_assert(gts_polygon_orientation(polygon, orient));	

	g_assert(orient[0] == 0 && orient[1] == 0 && orient[2] < 0); 

	p = g_clist_next(polygon); 
	g_assert(is_convex(GTS_POINT(p->prev->data), GTS_POINT(p->data),GTS_POINT(p->next->data), orient));
	g_assert(is_inside(GTS_POINT(vtx[0]), GTS_POINT(vtx[1]), GTS_POINT(vtx[2]), GTS_POINT(vtx[3])));
	g_assert(!is_inside(GTS_POINT(vtx[0]), GTS_POINT(vtx[1]), GTS_POINT(vtx[3]), GTS_POINT(vtx[2])));

	g_clist_free(p); 
	for (i = 0; i < 4; ++i) 
		gts_object_destroy(GTS_OBJECT(vtx[i])); 
		
}