/*	$Id: dist_rrupt.c 145 2009-12-09 17:18:39Z cbworden $	USGS	*/

/* 
 * Computes directitivty parameter of Rowshandel (2006, 2010)
 */

/*####################################################################
#
# Notes on finite fault :
# In the finite fault file *_fault.txt in the input directory should have
# three columns (lat,lon,depth).
# Each fault is defined by a set of 4-point planar segments (quadrilaterals)
# joined by common sides.
# The points should be arranged in clockwise- or counterclockwise order, e.g.
#
#      3------4
#      |\      \ 
# 1(9)-2 6------5
#   \   \|    
#    8-- 7
#
# The last point is a repeat of the first point. 
#
# Each quadrilateral segment (1278, 2367, 3465) must have 4 corner points 
# which non-collinear. If the four points are not co-planar, the fourth
# point will be mapped onto the plane defined by the other three. Multiple 
# fault segments must connect in linear fashion as shown above; more 
# degenerate configurations are not supported. One planar segment (4 
# points + the first point repeated) or two connected planar segments 
# should be sufficient for most cases.
#
# More than one fault file, representing separate fault segments,
# may be used as long as the first and last points of each fault file 
# are identical.
#
# The faults so specified must be converted to ECEF coordinates (in meters)
# and written to an input file for this program, as described below.
#
# If the hypocenter does not lie exactly on one of the planar segments, it
# will be mapped onto the nearest plane. For the purposes of calculating 
# the directivity parameter, the "hypocenter" for the segments that do
# not contain the actual hypocenter will be assumed lie along the edge 
# connecting the segment to the segment closest to the hypocenter at 
# a location that maximizes the angle between the vector leading from
# the hypocenter to the edge and from the edge to the center of the patch
# in question.
#
#  The arguments to this program are:
#   faultfile npts patch_size hypo_x hypo_y hypo_z < surface_pts 
#  
#  faultfile: contains fault specs; described below
#  npts: the number of points (coming via stdin) at which to calculate the 
#      directivity parameter
#  patch_size: size (in meters) of the fault sub-elements
#  hypo_(x,y,z): The hypocenter in ECEF coordinates (meters)
#  surface_pts: the (x,y,z) coordinates at which to calculate the directivity
#      parameter; in ECEF coordinates (meters)
#  
#  The input file should be structured thusly:
#   
#    poly1_np
#    poly1_p1_x poly1_p1_y poly1_p1_z
#    poly1_p2_x poly1_p2_y poly1_p2_z
#    ...
#    poly1_np-1_x poly1_np-1_y poly1_np-1_z
#    poly1_p1_x poly1_p1_y poly1_p1_z
#    poly2_np
#    ...
#   
#  The coordinates must be specified in ECEF coordinates (meters). The last 
#  point in each polygon should be the same as the first
#
####################################################################*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <float.h>
#include <string.h>

#include <vector.h>

#define MAX_QUADS 50000	/* Maximum number of segments in a fault */

Vector3 find_hypo(Vector3 start, Vector3 end, Vector3 p1, Vector3 p2);
double find_angle(double dist);

/* Numerical Recipes in C programs to find minima */
void mnbrak(double *ax, double *bx, double *cx, double *fa, double *fb,
  double *fc, double (*func)(double));
double brent(double ax, double bx, double cx, double (*f)(double), double tol,
  double *xmin);

int main(int ac, char **av) {

  int i, j, k, m, pt_c;
  int nq = 0, np;
  Quad2 *q, *quads[MAX_QUADS];
  Vector3 *fpts;
  Vector3 *pts;
  double d;
  int sgn0, sgn1, sgn2, sgn3;
  double s0, s1, s2, s3;
  Vector3 n0, n1, n2, n3;
  Vector3 p0, p1, p2, p3;
  Vector3 hypop, phypo;
  Vector3 sx0, sx1, sj0, sj1;
  Vector3 jx0, jx1, jx2, jx3;
  Vector3 pp0, pp1, pp2, pp3;
  Vector3 mp0, mp1, cp, vac, vbd, r1, r2;
  double sx0len, sx1len, sy0len, sy1len, dx0, dx1, dy0, dy1, area;
  int nx, ny; 
  double Atotal = 0;
  ssize_t nr = 0;
  double *dp;
  FILE *fd;

  char *faultfile = av[1]; /* file containing the fault specifications */
  int npts = atoi(av[2]);  /* # of points that will be coming to stdin */
  float patch_dim = atof(av[3]); /* desired patch dimensions in meters */
  Vector3 hypo;		   /* hypocenter in ECEF coordinates */
  hypo.x = atof(av[4]);
  hypo.y = atof(av[5]);
  hypo.z = atof(av[6]); 

  /* Read the fault file */
  if( (fd = fopen(faultfile, "r")) == NULL ) {
    fprintf( stderr, "Unable to open %s\n", faultfile);
    exit(1);
  }
  /* Set up fault array of Quad2's */
  while( fscanf(fd, "%d\n", &np) != EOF ) {
    if( (fpts = (Vector3 *)malloc(np * sizeof(Vector3))) == NULL) {
      fprintf(stderr,"Out of memory reading polygons\n");
      exit(1);
    }
    for(i = 0; i < np; i++) {
      if( fscanf(fd, "%lf %lf %lf\n", &(fpts[i].x), &(fpts[i].y), &(fpts[i].z)) 
								== EOF ) {
        fprintf( stderr, "Badly structured fault file %s\n", faultfile);
        exit(1);
      }
    }
    /* Loop through each planar quadrilateral */
    for (pt_c = 1; pt_c<(np-1)/2; pt_c++) {
      if( (q = (Quad2 *)malloc(sizeof(Quad2))) == NULL) {
        fprintf(stderr,"Out of memory while making Quads\n");
        exit(1);
      }

      /* Find the vertices of the quadrilateral */
      q->p0 = fpts[pt_c-1];
      q->p1 = fpts[pt_c];
      q->p2 = fpts[np-pt_c-2];
      q->p3 = fpts[np-pt_c-1];

/*
fprintf(stderr,"Fault point 1: %lf, %lf, %lf\n", q->p0.x, q->p0.y, q->p0.z);
fprintf(stderr,"Fault point 2: %lf, %lf, %lf\n", q->p1.x, q->p1.y, q->p1.z);
fprintf(stderr,"Fault point 3: %lf, %lf, %lf\n", q->p2.x, q->p2.y, q->p2.z);
fprintf(stderr,"Fault point 4: %lf, %lf, %lf\n", q->p3.x, q->p3.y, q->p3.z);
*/

      /* Make the Hessian representation */
      q->hp = makeHessian(*q);

      /* Fix p3 so it lies on the plane defined by p0, p1, and p2 */
      q->p3 = mapPoint2Plane(q->hp, q->p3);

      /* Add this quad to the list */
      quads[nq++] = q;
      if (nq >= MAX_QUADS) {
        fprintf(stderr,"Too many fault segments in 'directivity'\n");
        exit(1);
      }
    }
    free(fpts);
  }
  fclose(fd);

  /* 
   * find the quad closest to the hypocenter 
   */
  double hdmin = DBL_MAX;
  int hqi = -1;
  for(i = 0; i < nq; i++) {
    q = quads[i];
    /* Create 4 planes with normals pointing outside rectangle */
    n0 = vcross(vsubtr(q->p1,q->p0),q->hp.np);
    n1 = vcross(vsubtr(q->p2,q->p1),q->hp.np);
    n2 = vcross(vsubtr(q->p3,q->p2),q->hp.np);
    n3 = vcross(vsubtr(q->p0,q->p3),q->hp.np);

    /* 
     * Is the hypocenter inside the projected rectangle?
     * Dot products show which side the origin is on.
     * If origin is on same side of all the planes, then it is 'inside'
     */
    sgn0 = signbit(vdot(n0, vsubtr(q->p0,hypo)));
    sgn1 = signbit(vdot(n1, vsubtr(q->p1,hypo)));
    sgn2 = signbit(vdot(n2, vsubtr(q->p2,hypo)));
    sgn3 = signbit(vdot(n3, vsubtr(q->p3,hypo)));
/*
fprintf(stderr,"Signbits: %d %d %d %d\n", sgn0, sgn1, sgn2, sgn3);
*/
      
    if (sgn0 == sgn1 && sgn1==sgn2 && sgn2==sgn3) {
      /*
       * Origin is inside. Use distance-to-plane formula.
       */
      d = vdot(q->hp.np, hypo) + q->hp.p;
      d *= d;
    } else {
      /* Origin is outside. Find distance to edges */
      p0 = vsubtr(q->p0,hypo);
      p1 = vsubtr(q->p1,hypo);
      p2 = vsubtr(q->p2,hypo);
      p3 = vsubtr(q->p3,hypo);
      s0 = dist2_to_segment_3d(p0,p1);
      s1 = dist2_to_segment_3d(p1,p2);
      s2 = dist2_to_segment_3d(p2,p3);
      s3 = dist2_to_segment_3d(p3,p0);
      d = fmin(fmin(s0,s1),fmin(s2,s3));
/*
fprintf(stderr,"Dist to edges: %lf %lf %lf %lf\n", sqrt(s0), sqrt(s1), sqrt(s2), sqrt(s3));
*/
    }
    if (d < hdmin) {
      hdmin = d;
      hqi   = i;
    }
  }
  if (hdmin == DBL_MAX) {
    fprintf(stderr,"Unable to find segment nearest hypocenter\n");
    exit(1);
  }

  /*
   * Project the hypocenter onto the plane of the nearest quad.
   * Note: the hypocenter may lie outside the nearest quad; this is
   * a pathological situation, but is likely to happen in practice. We
   * just map the hypocenter to the plane of the nearest quad, but don't
   * attempt to relocate it to fall within (or on the edge of the quad).
   */
  hypop = mapPoint2Plane(quads[hqi]->hp, hypo);

/*
fprintf(stderr,"Nearest quad: %d dist: %lf\n", hqi, sqrt(hdmin));
fprintf(stderr,"Hypocenter: %lf, %lf, %lf\n", hypop.x, hypop.y, hypop.z);
*/
    
  /* read the input points */
  if( (pts = (Vector3 *)malloc(npts * sizeof(Vector3))) == NULL) {
    fprintf(stderr,"Out of memory for surface points\n");
    exit(1);
  }
  if((nr = fread(pts, sizeof(Vector3), npts, stdin)) != npts) {
    fprintf(stderr,"Error reading input: %d read, %d expected\n", nr, npts);
    exit(1);
  }
    
  /* make a corresponding array of directivity parameter, zero it */
  if( (dp = (double *)malloc(npts * sizeof(double))) == NULL) {
    fprintf(stderr,"Out of memory for directivity parameter\n");
    exit(1);
  }
  memset(dp, 0, npts * sizeof(double));

  /* for each quad */
  for(i = 0; i < nq; i++) {
    q = quads[i];
    /* divide the quad into rectangular sub-faults */
    /* call the sides p0->p1 and p3->p2 "horizontal", i.e., the x dimension; */
    /* call p0->p1 the "top" and p3->p2 the "bottom" of the quad */
    /* find the number of sub-faults in the x dimension and set up the */
    /* unit vectors and quad increment along each side */
    sx0 = vsubtr(q->p1, q->p0);
    sx1 = vsubtr(q->p2, q->p3);
    sx0len = vlen(sx0);
    sx1len = vlen(sx1);
    if (sx0len > sx1len) {
      nx = (int)ceil(sx0len / patch_dim);
    } else {
      nx = (int)ceil(sx1len / patch_dim);
    }
    dx0 = sx0len / nx;
    dx1 = sx1len / nx;
    sx0 = vnorm(sx0);
    sx1 = vnorm(sx1);
    /* need ny in the loop below */
    sy0len = vlen(vsubtr(q->p3, q->p0));
    sy1len = vlen(vsubtr(q->p2, q->p1));
    if (sx0len > sx1len) {
      ny = (int)ceil(sy0len / patch_dim);
    } else {
      ny = (int)ceil(sy1len / patch_dim);
    }
    /* loop in the x dimension */
/*
fprintf(stderr, "nx %d ny %d dx0 %lf dx1 %lf\n", nx, ny, dx0, dx1);
*/
    for(j = 0; j < nx; j++) {
      /* jx0 and jx1 are the points along the top edge that bound this set
       * of sub-faults, jx2 and jx3 bound the bottom; note that we've flipped
       * the ordering convention here -- jx2 is directly below jx0, and
       * jx3 is below jx1 */
      jx0 = vadd(q->p0, vmult(sx0, j*dx0));
      jx1 = vadd(q->p0, vmult(sx0, (j+1)*dx0));
      jx2 = vadd(q->p3, vmult(sx1, j*dx1));
      jx3 = vadd(q->p3, vmult(sx1, (j+1)*dx1));
      /* sj0 points from jx0 to jx1, sj1 points from jx1 to jx3 */
      sj0 = vsubtr(jx2, jx0);
      sj1 = vsubtr(jx3, jx1);
      /* find the grid increment along each vector in the y dimension, and
       * the unit vectors along each side */
      dy0 = vlen(sj0) / ny;
      dy1 = vlen(sj1) / ny;
/*
fprintf(stderr, "dy0 %lf dy1 %lf\n", dy0, dy1);
*/
      sj0 = vnorm(sj0);
      sj1 = vnorm(sj1);
      /* loop in the y dimension */
      for(k = 0; k < ny; k++) {
        /* find the corners and center of the sub-fault */
        /* note that the point ordering is back to the "normal" ordering 
         * (i.e., p0->p1->p2->p3(->p0) form a quadrilateral) */
        pp0 = vadd(jx0, vmult(sj0, k*dy0));
        pp1 = vadd(jx1, vmult(sj1, k*dy1));
        pp2 = vadd(jx1, vmult(sj1, (k+1)*dy1));
        pp3 = vadd(jx0, vmult(sj0, (k+1)*dy0));

        /* find the center of the sub-fault */
        /* find the midpoints between pp0->pp1, and pp3->pp2 */
        mp0 = vadd(pp0, vmult(vsubtr(pp1, pp0), 0.5));
        mp1 = vadd(pp3, vmult(vsubtr(pp2, pp3), 0.5));
        /* find the midpoint between mp0 and mp1 -- that's the center */
        cp = vadd(mp0, vmult(vsubtr(mp1, mp0), 0.5));

        /* the area of a convex quadrilateral is 1/2 * | AC x BD | */
        vac = vsubtr(pp2, pp0);
        vbd = vsubtr(pp3, pp1);
        area = 0.5 * vlen(vcross(vac, vbd));
        /* sum A into total area */
        Atotal += area;
/*
fprintf(stderr,"Center point: %lf %lf %lf area: %lf\n", cp.x, cp.y, cp.z, area);
*/

        /* if quad is pre-hypocenter set the hypocenter mid side p1-p2 */
        /* if quad is post-hypocenter set the hypocenter mid side p0-p3 */
        /* otherwise use actual hypocenter */
        if (i < hqi) {
/*
fprintf(stderr,"Using pre hypo\n");
*/
          phypo = find_hypo(hypop, cp, q->p1, q->p2);
        } else if (i > hqi) {
/*
fprintf(stderr,"Using post hypo\n");
*/
          phypo = find_hypo(hypop, cp, q->p0, q->p3);
        } else {
/*
fprintf(stderr,"Using actual hypo\n");
*/
          phypo = hypop;
        }
/*
fprintf(stderr,"hypo: %lf %lf %lf\n", phypo.x, phypo.y, phypo.z);
*/

        /* make vector r1 */
        r1 = vnorm(vsubtr(cp, hypo));

        /* loop over the output points */
        for(m = 0; m < npts; m++) {
          /* make vector r2 */
          r2 = vnorm(vsubtr(pts[m], cp));
          /* compute r1.r2*A, sum into directivity array */
          dp[m] += area * vdot(r1, r2);
/*
if( j==0 && k==0 ) {
fprintf(stderr, "pts: %lf %lf %lf\n", pts[m].x, pts[m].y, pts[m].z);
}
*/
        }
      }
    }
  }

  /* divide directivity array by total area */
  for(m = 0; m < npts; m++) {
    dp[m] /= Atotal;
  }

  /* output directivity for each point */
  if (fwrite(dp, sizeof(double), npts, stdout) != npts) {
    fprintf(stderr,"Error writing directivity array\n");
    exit(1);
  }
  return 0;
}

/* "_start" conflicted with Sun Studio 12 runtime code, so I renamed it *
 * and the other variables here by prepending "v3". PNL 2011/03/04      */
Vector3 v3_p1, v3_uv1, v3_start, v3_end;

/*
 * find_hypo() finds a point on the segment running from p1 to p2 such
 * that the angle between the vector from the start (the hypocenter) to
 * the point, and the vector from the point to the end (the fault patch),
 * is maximized (i.e., the distance from the start to the end, passing
 * through the segment is minimized). This defines a pseudo-hypocenter for
 * for fault patches that lie within fault segments that don't contain
 * the hypocenter.
 */
Vector3 find_hypo(Vector3 start, Vector3 end, Vector3 p1, Vector3 p2) {

  double dist;
  double tol = 1;
  double ax, bx, cx, fa, fb, fc;

  /* need to set globals for find angle to use */
  /* We create a unit vector from p1 to p2. The point along the segment
   * p1->p2 is specified by how far along the vector one goes (this makes
   * for a simple, single-variable for the minimization function to work
   * with).
   */
  v3_p1 = p1;
  v3_uv1 = vsubtr(p2, p1);
  dist = vlen(v3_uv1);
  v3_uv1 = vmult(v3_uv1, 1 / dist);
  v3_start = start;
  v3_end   = end;

  /* the range of the search is from ax to bx */
  ax = 0;
  bx = dist;

  /*
   * mnbrak() is a numerical recipes function that sets the starting values
   * for brent() -- it is only necessary under some circumstances, but it's
   * cheap, so we use it.
   */
  mnbrak(&ax, &bx, &cx, &fa, &fb, &fc, find_angle);

  /*
   * brent() is another numerical recipes function. It uses parabolic 
   * interpolation to find a minimum.
   */
  (void) brent(ax, bx, cx, find_angle, tol, &dist);

  return vadd(p1, vmult(v3_uv1, dist));
}

/*
 * Given the vectors: 
 *
 * v1 defined by  v3_start to v3_p1 + (dist * v3_uv1)
 * v2 defined by v3_p1 + (dist * v3_uv1) to v3_end
 *
 * this function returns (v1/|v1|) (dot) (v2/|v2|)
 */
double find_angle(double dist) {

  Vector3 mid = vadd(v3_p1, vmult(v3_uv1, dist));

  /* 
   * return the negative dot product since brent() looks for a minimum 
   * and we want a maximum
   */
  return -vdot(vnorm(vsubtr(mid, v3_start)), vnorm(vsubtr(v3_end, mid)));
}