// point_in_poly.c count number of edge crossing horizontal line from test
// odd, c==1, is inside  pnpoly
//             pvpoly uses vertical line from  testx, testy
//             pitri tests 3D point crosses 3D triangle
//             ptdat tests 3d point in .dat structure
// include point_in_poly.h after datread.h
// use ptdat() after datread()

#include <stdio.h>
#include <stdlib.h>
#include "point_in_poly.h"
#include "simeq.h"
#define abs(x) ((x)<0.0?(-(x)):(x))
#define max(a,b) ((a)>(b)?(a):(b))
#define min(a,b) ((a)<(b)?(a):(b))

static double determinant(int n, double A[n][n]);

int pnpoly(int nvert, float vertx[], float verty[], float testx, float testy)
{
  int i, j, c = 0;
  // for(i=0, j=nvert-1; i<nvert; j=i++) fancy
  for(i=0; i<nvert; i++)
  {
    j = i-1;
    if(j<0) j=nvert-1;
    // printf("i=%d, j=%d \n", i, j); // observe subscripts
    if(((verty[i]>testy) != (verty[j]>testy)) &&
	(testx < (vertx[j]-vertx[i]) * 
        (testy-verty[i]) / (verty[j]-verty[i]) + vertx[i]) )
      // c = !c;  fancy
      c = 1-c;
  }
  return c;
} // end pnpoly

int pvpoly(int nvert, float vertx[], float verty[], float testx, float testy)
{
  int i, j, c = 0;
  // for(i=0, j=nvert-1; i<nvert; j=i++) fancy
  for(i=0; i<nvert; i++)
  {
    j = i-1;
    if(j<0) j=nvert-1;
    // printf("i=%d, j=%d \n", i, j);
    if(((vertx[i]>testx) != (vertx[j]>testx)) &&
	(testy < (verty[j]-verty[i]) * 
        (testx-vertx[i]) / (vertx[j]-vertx[i]) + verty[i]) )
      // c = !c;  fancy
      c = 1-c;
  }
  return c;
} // end pvpoly

// line from point intersects triangle
int pitri(int nvert, float vertx[], float verty[], float vertz[],
	             float testx, float testy, float testz)
{
  int c = 0;  // nvert should be 3, ignored
  double xa, ya, za; // given point 
  double xb, yb, zb; // some direction from point
  double x0, y0, z0, x1, y1, z1, x2, y2, z2; // triangle points
  double t, u, v, d;
  double A[3][3];
  double Y[3];
  double X[3];

  xa = testx;
  ya = testy;
  za = testz;
  xb = xa + 100.0; // random direction
  yb = ya - 0.7;
  zb = za + 0.9;
  x0 = vertx[0];
  y0 = verty[0];
  z0 = vertz[0];
  x1 = vertx[1];
  y1 = verty[1];
  z1 = vertz[1];
  x2 = vertx[2];
  y2 = verty[2];
  z2 = vertz[2];
  A[0][0] = xa-xb;
  A[1][0] = ya-yb;
  A[2][0] = za-zb;
  A[0][1] = x1-x0;
  A[1][1] = y1-y0;
  A[2][1] = z1-z0;
  A[0][2] = x2-x0;
  A[1][2] = y2-y0;
  A[2][2] = z2-z0;
  Y[0] = xa-x0;
  Y[1] = ya-y0;
  Y[2] = za-z0;

  d = determinant(3, A);
  if(abs(d)<1.0e-6) return 0;
  simeq2(3, A, Y, X);
  t = X[0];
  u = X[1];
  v = X[2];
  if(t<=0.0 || t>=1.0) return 0;
  // if(u>-1.0 && u<1.0 && v>-1.0 && v<1.0 && (u+v)<1.0) return 1; 
  if(u>0.0 && u<1.0 && v>0.0 && v<1.0 && (u+v)<1.0) return 1; 
  return 0;
} // end pitri

int ptdat(int num_vert, int num_poly, dpts verts[], int polys[],
	  float testx, float testy, float testz)
{
  int c = 0;
  float vertx[10], verty[10], vertz[10]; // only triangles
  float x, y, z;
  double xmin, xmax, ymin, ymax, zmin, zmax, hx, hy, hz;
  int i, j, k, pts, pt, ct;

  c = 0;
  k = 0;
  for(i=0; i<num_poly; i++)
  {
    pts = polys[k++];
    for(j=0; j<pts; j++)
    {
      pt = polys[k++];
      vertx[j] = verts[pt-1].x;
      verty[j] = verts[pt-1].y;
      vertz[j] = verts[pt-1].z;
    }
    ct = pitri(3, vertx, verty, vertz, testx, testy, testz);
    c = c + ct;
  } // end i  poly
  return c;
} // end ptdat

static double determinant(int n, double A[n][n])
{
  double D = 1.0;  /* DETERMINANT */
  double *B;  /* WORKING MATRIX */
  int *ROW;  /* ROW INTERCHANGE INDICIES */
  int HOLD , I_PIVOT;  /* PIVOT INDICIES */
  double PIVOT;  /* PIVOT ELEMENT VALUE */
  double ABS_PIVOT;
  int i, j, k;

  B = (double *)calloc(n*n, sizeof(double));
  for(i=0; i<n; i++){
    for(j=0; j<n; j++){
      B[i*n+j] = A[i][j];
    }
  }
  ROW = (int *)calloc(n, sizeof(int));
  for(k=0; k<n; k++){
    ROW[k]= k;
  }
  for(k=0; k<n-1; k++){
    PIVOT = B[ROW[k]*n+k];
    ABS_PIVOT = abs ( PIVOT );
    I_PIVOT = k;
    for(i=k; i<n; i++){
      if(abs(B[ROW[i]*n+k]) > ABS_PIVOT ){
        I_PIVOT = i;
        PIVOT = B[ROW[i]*n+k];
        ABS_PIVOT = abs(PIVOT);
      }
    }
    if(I_PIVOT != k){
      HOLD = ROW[k];
      ROW[k] = ROW[I_PIVOT];
      ROW[I_PIVOT] = HOLD;
      D = - D;
    }
    if(ABS_PIVOT < 1.0E-10){
      free(B);
      free(ROW);
      return 0.0;
    }
    else
    {
      D = D * PIVOT;
      for(j=k+1; j<n; j++){
        B[ROW[k]*n+j] = B[ROW[k]*n+j] / B[ROW[k]*n+k];
      }
      for(i=0; i<n; i++){
        if(i != k){
          for(j=k+1; j<n; j++){
            B[ROW[i]*n+j] = B[ROW[i]*n+j] - B[ROW[i]*n+k]* B[ROW[k]*n+j];
          } /* end j */
        } /* end if */
      } /* end i */
    } /* end else */
  } /* end k */
  D = D * B[ROW[n-1]*n+n-1];
  free(B);
  free(ROW);
  return D;
} /* end determinant */