;;
;; Procedures for rotating, scaling, translating surfaces.
;; Based on the Python GTS library.
;;

#>

void triangle_destroy (GtsTriangle *t)
{
   if (t != NULL)
   {
      gts_object_destroy ((GtsObject *)t);
   }
}

void vertex_destroy (GtsVertex *v)
{
   if (v != NULL)
   {
      gts_object_destroy ((GtsObject *)v);
   }
}

void surface_destroy (GtsSurface *s)
{
   if (s != NULL)
   {
      gts_object_destroy ((GtsObject *)s);
   }
}

int surface_point_translate_fn (GtsPoint* p, C_word data)
{
  GtsMatrix *m;
  GtsVector *v;

  v = (GtsVector *)C_c_f64vector(data);

  if( (m = gts_matrix_translate(NULL,*v)) == NULL ) 
  {
      return -1;
  }  
  gts_point_transform(p,m);
  gts_matrix_destroy(m);
  return 0;
}

int surface_point_scale_fn (GtsPoint* p, C_word data)
{
  GtsMatrix *m;
  GtsVector *v;

  v = (GtsVector *)C_c_f64vector(data);

  if( (m = gts_matrix_scale(NULL,*v)) == NULL ) 
  {
      return -1;
  }  
  gts_point_transform(p,m);
  gts_matrix_destroy(m);
  return 0;
}

int surface_point_rotate_fn (GtsPoint* p, C_word data)
{
  GtsMatrix *m;
  GtsVector *v;
  double angle;

  v = (GtsVector *)C_c_f64vector(data);
  angle = C_ub_i_f64vector_ref(data,3);

  if( (m = gts_matrix_rotate(NULL,*v,angle)) == NULL ) 
  {
      return -1;
  }  
  gts_point_transform(p,m);
  gts_matrix_destroy(m);
  return 0;
}
<#


(define (surface-translate s #!key (dx 0.0) (dy 0.0) (dz 0.0))
  (define surface_point_translate
    (foreign-value "&surface_point_translate_fn" c-pointer))
  (let ((data (f64vector dx dy dz)))
    (surface-foreach-vertex s surface_point_translate data)
    s))
 

(define (surface-scale s #!key (dx 1.0) (dy 1.0) (dz 1.0))
  (define surface_point_scale
    (foreign-value "&surface_point_scale_fn" c-pointer))
  (let ((data (f64vector dx dy dz)))
    (surface-foreach-vertex s surface_point_scale data)
    s))
 

(define (surface-rotate s #!key (dx 0.0) (dy 0.0) (dz 0.0) (angle 0.0))
  (define surface_point_rotate
    (foreign-value "&surface_point_rotate_fn" c-pointer))
  (let ((data (f64vector dx dy dz angle)))
    (surface-foreach-vertex s surface_point_rotate data)
    s))
 

#>

/* Helper function for inter() */
void get_largest_coord_fn(GtsVertex *v,C_word val) 
{
  double v0, v1;
 
  v0 = C_ub_i_f64vector_ref(val, 0);

 if( fabs(GTS_POINT(v)->x) > v0 ) 
   v1 = fabs(GTS_POINT(v)->x);
 if( fabs(GTS_POINT(v)->y) > v0 ) 
   v1 = fabs(GTS_POINT(v)->y);
 if( fabs(GTS_POINT(v)->z) > v0 ) 
   v1 = fabs(GTS_POINT(v)->z);

 C_ub_i_f64vector_set(val, 0, v1);
}



<#


#>
/* Cleanup functions */

static void
build_list1(gpointer data, GList ** list)
{
  *list = g_list_prepend(*list, data);
}

static void 
build_list(gpointer data, GSList ** list)
{
  *list = g_slist_prepend(*list, data);
}

void vertex_cleanup(GtsSurface *s, gdouble threshold)
{
  GList * vertices = NULL;

  /* merge vertices which are close enough */
  /* build list of vertices */
  gts_surface_foreach_vertex(s, (GtsFunc) build_list1, &vertices);

  /* merge vertices: we MUST update the variable vertices because this function
     modifies the list (i.e. removes the merged vertices). */
  vertices = gts_vertices_merge(vertices, threshold, NULL);



  /* free the list */
  g_list_free(vertices);
}


void edge_cleanup(GtsSurface *s)
{
  GList *edges = NULL, *i = NULL;
  GtsEdge *e;

  g_return_if_fail(s != NULL);

  /* build list of edges */
  gts_surface_foreach_edge(s, (GtsFunc)build_list1, &edges);

  /* merge vertices: we MUST update the variable vertices because this function
     modifies the list (i.e. removes the merged vertices). */
  edges = gts_edges_merge(edges);

  i = edges;
  while(i) 
  {
     e = i->data;
     if(GTS_SEGMENT(e)->v1 == GTS_SEGMENT(e)->v2) 
     {
      /* edge is degenerate */
	gts_object_destroy(GTS_OBJECT(e));
      }
     i = g_list_next(i);
   }

  /* free the list */
  g_list_free(edges);
}


void 
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 = i->data;
    if (!gts_triangle_is_ok(t)) 
       {
	gts_surface_remove_face(s,GTS_FACE(t));
      /* destroy t, its edges (if not used by any other triangle)
	 and its corners (if not used by any other edge) */
	gts_object_destroy(GTS_OBJECT(t));
       }
    i = g_slist_next(i);
  }
  
  /* free list of triangles */
  g_slist_free(triangles);
}

<#

(define vertex-cleanup
  (foreign-safe-lambda void "vertex_cleanup" (nonnull-c-pointer GtsSurface) double ))

(define edge-cleanup
  (foreign-safe-lambda void "edge_cleanup" (nonnull-c-pointer GtsSurface) ))

(define face-cleanup
  (foreign-safe-lambda void "face_cleanup" (nonnull-c-pointer GtsSurface) ))



;; Helper function for intersection operations 
(define (inter  s1 s2 op1 op2)
  (define get_largest_coord
    (foreign-value "&get_largest_coord_fn" c-pointer))

  ;; Make sure that we don't have two pointers to the same surface 
  (if (equal? s1 s2) 
      (error 'inter "can't determine intersection between surfaces" s1 s2))

  ;; *** ATTENTION *** 
  ;; Check for self-intersections in either surface 
;;  (if (surface-is-self-intersecting s1)
;      (error 'inter "surface s1 is self-intersecting" s1))
;;  (if (surface-is-self-intersecting s2)
;;      (error 'inter "surface s2 is self-intersecting" s2))

  ;; Avoid complete self-intersection of two surfaces
  (if (and (= (surface-face-number s1) (surface-face-number s2))
	   (= (surface-edge-number s1) (surface-edge-number s2))
	   (= (surface-vertex-number s1) (surface-vertex-number s2))
	   (= (surface-area s1) (surface-area s2)))
      
      (let ((area1 (surface-center-of-area s1))
	    (area2 (surface-center-of-area s2)))
	
	(if (and (= (car area1) (car area2))
		 (let ((v1 (cadr area1)) (v2 (cadr area2)))
		   (and 
		    (= (f64vector-ref v1 0) (f64vector-ref v2 0))
		    (= (f64vector-ref v1 1) (f64vector-ref v2 1))
		    (= (f64vector-ref v1 2) (f64vector-ref v2 2)))))
	    (error 'inter "surfaces are mutually intersecting" s1 s2))
	))

  ;; Get bounding boxes 
  (let ((tree1 (bb-tree-surface s1))
	(tree2 (bb-tree-surface s2))
	(is-open1 (not (surface-is-closed s1)))
	(is-open2 (not (surface-is-closed s2))))

    ;; Creates the surface intersection object 
    (let ((si (surface-inter-new (surface-inter-class) s1 s2
				 tree1 tree2 is-open1 is-open2)))
      (bb-tree-destroy tree1 #t)
      (bb-tree-destroy tree2 #t)

      ;; Check that the surface intersection object is closed  
      (if (not (surface-inter-check si))
	  (begin (object-destroy si)
		 (error 'inter "intersection result is not closed" si)))

      ;; Create the surface 
      (let ((surface (surface-new)))

	;; Calculate the new surface 
	(surface-inter-boolean si surface op1)
	(surface-inter-boolean si surface op2)
	(object-destroy si)

	;; Clean up the result 
	(let ((eps (f64vector 0.0)))
	  (surface-foreach-vertex surface get_largest_coord eps)
	  (let ((eps (* (f64vector-ref eps 0) (expt 2 -50))))
	    (vertex-cleanup surface eps)
	    (edge-cleanup surface)
	    (face-cleanup surface)
	    eps
	    ))

	;; Check for self-intersection 
;;	(if (surface-is-self-intersecting surface)
;;	    (begin
;;	      (object-destroy surface)
;;	      (error 'inter "result is self-intersection surface")))

	surface)
      )))

(define-foreign-variable GTS_1_OUT_2  int)
(define-foreign-variable GTS_1_IN_2   int)
(define-foreign-variable GTS_2_OUT_1  int)
(define-foreign-variable GTS_2_IN_1   int)

(define (surface-intersection a b ) (inter a b GTS_1_OUT_2 GTS_2_OUT_1))

(define (surface-union a b ) (inter a b GTS_1_IN_2 GTS_2_IN_1))

(define (surface-difference a b ) (inter a b GTS_1_OUT_2 GTS_2_IN_1))