/*
 * Part of triangulate.c, Copyright (c) 1989, Paul Bourke
 *
 * From "Efficient Triangulation Algorithm Suitable for Terrain Modelling"
 * http://paulbourke.net/papers/triangulate/
 *
 * "Any source code found here may be freely used provided credits are given
 *  to the author." - http://paulbourke.net/papers/
 *
 * Additional tuning & optimisation by Jeffrey Lee
 */
#include <stdlib.h>
#include <math.h>

#include "triangulate.h"

#define FALSE 0
#define TRUE 1
#define EPSILON 0.00001f

typedef struct {
   int8_t p1,p2;
} IEDGE;

/*
   Return TRUE if a point (xp,yp) is inside the circumcircle made up
   of the points (x1,y1), (x2,y2), (x3,y3)
   The circumcircle centre is returned in (xc,yc) and the radius r
   NOTE: A point on the edge is inside the circumcircle
*/
static int CircumCircle(float xp,float yp,
   float x1,float y1,float x2,float y2,float x3,float y3,
   float *xc,float *yc,float *rsqr)
{
   float m1,m2,mx1,mx2,my1,my2;
   float dx,dy,drsqr;
   float fabsy1y2 = fabsf(y1-y2);
   float fabsy2y3 = fabsf(y2-y3);

   /* Check for coincident points */
   if (fabsy1y2 < EPSILON && fabsy2y3 < EPSILON)
       return -1;

   mx1 = (x1 + x2) * 0.5f;
   mx2 = (x2 + x3) * 0.5f;
   my1 = (y1 + y2) * 0.5f;
   my2 = (y2 + y3) * 0.5f;

   if (fabsy1y2 < EPSILON) {
      m2 = (x3-x2) / (y2-y3);
      *xc = mx1;
      *yc = m2 * (mx1 - mx2) + my2;
   } else if (fabsy2y3 < EPSILON) {
      m1 = (x2-x1) / (y1-y2);
      *xc = mx2;
      *yc = m1 * (mx2 - mx1) + my1;
   } else {
      m1 = (x2-x1) / (y1-y2);
      m2 = (x3-x2) / (y2-y3);
      *xc = (m1 * mx1 - m2 * mx2 + my2 - my1) / (m1 - m2);
      if (fabsy1y2 > fabsy2y3) {
         *yc = m1 * (*xc - mx1) + my1;
      } else {
         *yc = m2 * (*xc - mx2) + my2;
      }
   }

   dx = x2 - *xc;
   dy = y2 - *yc;
   *rsqr = dx*dx + dy*dy;

   dx = xp - *xc;
   dy = yp - *yc;
   drsqr = dx*dx + dy*dy;

   // Original
   //return((drsqr <= *rsqr) ? TRUE : FALSE);
   // Proposed by Chuck Morris
   return((drsqr - *rsqr) <= EPSILON ? TRUE : FALSE);
}


/*
   Triangulation subroutine
   Takes as input NV vertices in array pxyz
   Returned is a list of ntri triangular faces in the array v
   These triangles are arranged in a consistent clockwise order.
   The triangle array 'v' should be malloced to 3 * nv
   The vertex array pxyz must be big enough to hold 3 more points
   The vertex array must be sorted in increasing x values say

   qsort(p,nv,sizeof(XYZ),XYZCompare);
      :
   int XYZCompare(void *v1,void *v2)
   {
      XYZ *p1,*p2;
      p1 = v1;
      p2 = v2;
      if (p1->x < p2->x)
         return(-1);
      else if (p1->x > p2->x)
         return(1);
      else
         return(0);
   }
*/
void Triangulate(int nv,XYZ *pxyz,ITRIANGLE *v,int *ntri)
{
   uint8_t complete[4*NUM_VORONOI];
   IEDGE edges[9*NUM_VORONOI+3]; /* 3 edges per triangle, with max 3*nv triangles, so 9*NUM_VORONOI */
   int nedge = 0;

   int inside;
   int i,j,k;
   float xp,yp,x1,y1,x2,y2,x3,y3,xc,yc,r;
   float xmid,ymid,dmax;

   /*
      Find the maximum and minimum vertex bounds.
      This is to allow calculation of the bounding triangle
   */
   xmid = 0.5f;
   ymid = 0.5f;
   dmax = 4.0f;

   /*
      Set up the supertriangle
      This is a triangle which encompasses all the sample points.
      The supertriangle coordinates are added to the end of the
      vertex list. The supertriangle is the first triangle in
      the triangle list.
   */
   pxyz[nv+0].x = xmid - 20 * dmax;
   pxyz[nv+0].y = ymid - dmax;
   pxyz[nv+1].x = xmid;
   pxyz[nv+1].y = ymid + 20 * dmax;
   pxyz[nv+2].x = xmid + 20 * dmax;
   pxyz[nv+2].y = ymid - dmax;
   v[0].p1 = nv;
   v[0].p2 = nv+1;
   v[0].p3 = nv+2;
   complete[0] = FALSE;
   *ntri = 1;

   /*
      Include each point one at a time into the existing mesh
   */
   for (i=0;i<nv;i++) {

      xp = pxyz[i].x;
      yp = pxyz[i].y;
      nedge = 0;

      /*
         Set up the edge buffer.
         If the point (xp,yp) lies inside the circumcircle then the
         three edges of that triangle are added to the edge buffer
         and that triangle is removed.
      */
      for (j=0;j<(*ntri);j++) {
         if (complete[j])
            continue;
         x1 = pxyz[v[j].p1].x;
         y1 = pxyz[v[j].p1].y;
         x2 = pxyz[v[j].p2].x;
         y2 = pxyz[v[j].p2].y;
         x3 = pxyz[v[j].p3].x;
         y3 = pxyz[v[j].p3].y;
         inside = CircumCircle(xp,yp,x1,y1,x2,y2,x3,y3,&xc,&yc,&r);
         if ((inside > -1) && (xc < xp) && ((xp-xc)*(xp-xc)) > r)
            complete[j] = TRUE;
         if (inside > 0) {
            edges[nedge+0].p1 = v[j].p1;
            edges[nedge+0].p2 = v[j].p2;
            edges[nedge+1].p1 = v[j].p2;
            edges[nedge+1].p2 = v[j].p3;
            edges[nedge+2].p1 = v[j].p3;
            edges[nedge+2].p2 = v[j].p1;
            nedge += 3;
            v[j] = v[(*ntri)-1];
            complete[j] = complete[(*ntri)-1];
            (*ntri)--;
            j--;
         }
      }

      /*
         Tag multiple edges
         Note: if all triangles are specified anticlockwise then all
               interior edges are opposite pointing in direction.
      */
      for (j=0;j<nedge-1;j++) {
         for (k=j+1;k<nedge;k++) {
            if ((edges[j].p1 == edges[k].p2) && (edges[j].p2 == edges[k].p1)) {
               edges[j].p1 = -1;
               edges[k].p1 = -1;
            }
         }
      }

      /*
         Form new triangles for the current point
         Skipping over any tagged edges.
         All edges are arranged in clockwise order.
      */
      for (j=0;j<nedge;j++) {
         if (edges[j].p1 < 0)
            continue;
         v[*ntri].p1 = edges[j].p1;
         v[*ntri].p2 = edges[j].p2;
         v[*ntri].p3 = i;
         complete[*ntri] = FALSE;
         (*ntri)++;
      }
   }

   /*
      Remove triangles with supertriangle vertices
      These are triangles which have a vertex number greater than nv
   */
   for (i=(*ntri)-1;i>=0;i--) {
      if (v[i].p1 >= nv || v[i].p2 >= nv || v[i].p3 >= nv) {
         v[i] = v[--(*ntri)];
      }
   }
}
