/**
 * Sample an arbitrary quadrilateral mesh with an equally spaced regular grid.
 */

#include<stdio.h>
#include <math.h>
#include "ruby.h"
#include "narray.h"

#ifndef NARRAY_BIGMEM
typedef int na_shape_t;
#endif

static double Eps = 1e-10;

// sort three pairs of x&y by x
void sort3pairs(double ax, double bx, double cx,   // in
               double ay, double by, double cy,   // in
               double *x3, double *y3)   // out (len=3, alreadt allocated)
{
  if ( ax <= bx && ax <= cx ) {
    x3[0]=ax;  y3[0]=ay;
    if ( bx <= cx ) {
      x3[1]=bx; y3[1]=by; x3[2]=cx; y3[2]=cy;
    } else {
      x3[1]=cx; y3[1]=cy; x3[2]=bx; y3[2]=by;
    }
  } else if ( bx <= cx ) {
    x3[0]=bx; y3[0]=by;
    if ( ax <= cx ) {
      x3[1]=ax; y3[1]=ay; x3[2]=cx; y3[2]=cy;
    } else {
      x3[1]=cx; y3[1]=cy; x3[2]=ax; y3[2]=ay;
    }
  } else {
    x3[0]=cx; y3[0]=cy;
    if ( bx <= ax ) {
      x3[1]=bx; y3[1]=by; x3[2]=ax; y3[2]=ay;
    } else {
      x3[1]=ax; y3[1]=ay; x3[2]=bx; y3[2]=by;
    }
  }
}

static int idxceil(double x, int limit)
{
  int idx;
  idx = ceil(x-Eps);
  idx = ( idx >= limit ? idx : limit );
  return idx;
}

static int idxfloor(double x, int limit)
{
  int idx;
  idx = floor(x+Eps);
  idx = ( idx <= limit ? idx : limit );
  return idx;
}

static double ycrossing(double x0, double x1, double y0, double y1, double x)
{
  double yc = -999;   // negative value considering the positive array indexing
  if (x1!=x0) yc = y0 + (y1-y0)/(x1-x0)*(x-x0);
  return yc;
}

// solve quadratic equation to return a solution which is likely between 0 and 1
// (if both solutions are between 0 and 1, arbitrary one of them is returned).
static double solv_quad_0to1(double a, double b, double c){
  double x;
  if ( a == 0 ) {
    if (b == 0){
      return 0.5;   // no solution. return a moderate val
    } else {
      return -c/b;
    }
  }
  if ( b*b - 4*a*c < 0) {
    return 0.5;   // no solution. return a moderate val
  } else if ( b*b < Eps*fabs(4*a*c) ){  // b ~ 0
    if ( c/a < 0) {
      return 0.5; // likely due to a round-off error
    } else {
      return sqrt(c/a);   // must be positive to be between 0 and 1
    }
  } else if ( fabs(4*a*c) < Eps*b*b ) { 
    // a ~ 0 or c ~ 0 : determinant is vulnerable to round-off error
    // Taylor expansion -> solutions are -c/b or -b/a+c/b (usu big)
    x = -b/a+c/b;
    if ( x >= -Eps && x <= 1+Eps ) {   // rare case
      return x;
    } else {
      return -c/b;
    }
  } else {
    x = ( -b + sqrt(b*b - 4*a*c) ) / (2*a) ;
    if ( x >= -Eps && x <= 1+Eps ) {
      return x;
    } else {
      x = ( -b - sqrt(b*b - 4*a*c) ) / (2*a) ;
      return x;
    }
  }
}

/*
 * Initialize a quadrilateral mesh sampling with an equally-spaced regular grid
 * 
 * Prepare for the call of quad_mesh_sample.
 * Using a transformed bilinear interpolation.
 * 
 * (IN)
 * * x, y: one-dimensionalized 2D array of ns(minor) * nr (major).
 *   Coordinate values of the field value to sample
 * * nx, dx0, d_x: describes the regular grid of x: d0, d0+d_x,..,d0+(nx-1)*d_x.
 * * ny, dy0, d_y: describes the regular grid of y: d0, d0+d_y,..,d0+(ny-1)*d_y.
 * * handle_miss: if true (non zero), data missing in x and y are handled:
 *   not to sample if any one vertex of a surrounding quadrilateral is missing.
 * (OUT)
 * * si, ri: specifies the surrounding quadrilateral for the regular grid points
 * * p, q: bilinear interpolation parameter, obtained from quadratic equations
 *   to support arbitrary quadrilaterals.
 */

static void quad_mesh_sample_init(int ns, int nr, double *x, double *y,  // (in)
                        int nx, double x0, double d_x,           // (in)
                        int ny, double y0, double d_y,           // (in)
                        int handle_miss, double misval, // (in)
                        int *si, int *ri, double *p, double *q)  // (out)
{
  int i, is, ir, ix, iy, j;
  int iy0, iy1;
  int imis=-1;  // negative value as an invalid (missing) index
  double ax, bx, cx, dx;   //  c  d    quadrilateral points
  double ay, by, cy, dy;   //  a  b    normalized by x0, d_x, y0, d_y 
  double x3[3], y3[3];     // normalized triangle points sorted by x
  double a0, a1, a2, a3, b0, b1 ,b2, b3, a, b, c, xi, yi;

  //< array initialization >

  for ( i=0 ; i<nx*ny ; i++ ){
    si[i] = ri[i] = imis;
    p[i] = q[i] = 0.0;  // just to avoid non-initialization
  }

  //< find where each of x-y grid point is in the original mesh >

  for ( ir=0 ; ir<nr-1 ; ir++ ){
    for ( is=0 ; is<ns-1 ; is++ ){
      if (handle_miss && ( x[is+ns*ir]==misval || x[is+1+ns*ir]==misval || 
          x[is+ns*(ir+1)]==misval || x[is+1+ns*(ir+1)]==misval ||
          y[is+ns*ir]==misval || y[is+1+ns*ir]==misval || 
          y[is+ns*(ir+1)]==misval || y[is+1+ns*(ir+1)]==misval ) ) {
        continue;
      }

      ax = (x[is   + ns*ir]     - x0)/d_x;
      bx = (x[is+1 + ns*ir]     - x0)/d_x;
      cx = (x[is   + ns*(ir+1)] - x0)/d_x;
      dx = (x[is+1 + ns*(ir+1)] - x0)/d_x;
      ay = (y[is   + ns*ir]     - y0)/d_y;
      by = (y[is+1 + ns*ir]     - y0)/d_y;
      cy = (y[is   + ns*(ir+1)] - y0)/d_y;
      dy = (y[is+1 + ns*(ir+1)] - y0)/d_y;

      for(j=0; j<2; j++) {
        if (j==0) {
          sort3pairs(ax, bx, cx, ay, by, cy,  x3, y3);   // first time
        } else {
          sort3pairs(dx, bx, cx, dy, by, cy,  x3, y3);  // second time
        }
        for(ix=idxceil(x3[0],0); ix<=idxfloor(x3[2],nx-1); ix++){
          xi = (double) ix;  // normalized x grid
          if ( (x3[0]!=x3[1] && ix<=x3[1]) || x3[1]==x3[2]  ){
            // consider angle at x3[0],y3[0]
            if ((x3[2]-x3[0])*(y3[1]-y3[0]) - (x3[1]-x3[0])*(y3[2]-y3[0]) < 0) {
              iy0 = idxceil(ycrossing(x3[0],x3[1],y3[0],y3[1],xi), 0);
              iy1 = idxfloor(ycrossing(x3[0],x3[2],y3[0],y3[2],xi), ny-1);
            } else {
              iy0 = idxceil(ycrossing(x3[0],x3[2],y3[0],y3[2],xi), 0);
              iy1 = idxfloor(ycrossing(x3[0],x3[1],y3[0],y3[1],xi), ny-1);
            }
          } else {
            // consider angle at x3[2],y3[2]
            if ((x3[0]-x3[2])*(y3[1]-y3[2]) - (x3[1]-x3[2])*(y3[0]-y3[2]) > 0) {
              iy0 = idxceil(ycrossing(x3[2],x3[1],y3[2],y3[1],xi), 0);
              iy1 = idxfloor(ycrossing(x3[2],x3[0],y3[2],y3[0],xi), ny-1);
            } else {
              iy0 = idxceil(ycrossing(x3[2],x3[0],y3[2],y3[0],xi), 0);
              iy1 = idxfloor(ycrossing(x3[2],x3[1],y3[2],y3[1],xi), ny-1);
            }
          }
          for(iy=iy0; iy<=iy1; iy++){
            si[ix + nx*iy] = is;
            ri[ix + nx*iy] = ir;
          }
        }
      }
    }
  }

  //< interpolation point >

  for (i=0; i<nx*ny; i++){
    if (si[i] >= 0) {  // then ri[i] >= 0
      xi = (double) (i % nx);    // normalized grid x val
      yi = (double) (i / nx);    // normalized grid y val

      is = si[i];
      ir = ri[i];
      ax = (x[is   + ns*ir]     - x0)/d_x;
      bx = (x[is+1 + ns*ir]     - x0)/d_x;
      cx = (x[is   + ns*(ir+1)] - x0)/d_x;
      dx = (x[is+1 + ns*(ir+1)] - x0)/d_x;
      ay = (y[is   + ns*ir]     - y0)/d_y;
      by = (y[is+1 + ns*ir]     - y0)/d_y;
      cy = (y[is   + ns*(ir+1)] - y0)/d_y;
      dy = (y[is+1 + ns*(ir+1)] - y0)/d_y;

      // mapping   xi = a0 + a1*p + a2*q + a3*p*q   where p=0..1 & q=0..1
      //           yi = b0 + b1*p + b2*q + b3*p*q
      a0 = ax;                 // x = ax when p=q=0
      a1 = bx - ax;            // x = bx when p=1 & q=0
      a2 = cx - ax;            // x = cx when p=0 & q=1
      a3 = dx - bx -cx + ax;   // x = dx when p=q=1
      b0 = ay;
      b1 = by - ay;
      b2 = cy - ay;
      b3 = dy - by -cy + ay;

      // quadratic equation
      a = a3*b2 - a2*b3;
      b = -a3*yi + b3*xi +a3*b0 - a0*b3 + a1*b2 - a2*b1;
      c = -a1*yi + b1*xi +a1*b0 - a0*b1;
      q[i] = solv_quad_0to1(a, b, c);
      if ( a1 + a3*q[i] != 0 ) {
        p[i] = (xi - a2*q[i] - a0) / (a1 + a3*q[i]);
        if (p[i] > 1.0) p[i]=1.0;  // abnormal, likely due to round-off error
        if (p[i] < 0.0) p[i]=0.0;  // abnormal, likely due to round-off error
      } else if ( b1 + b3*q[i] != 0 ) {
        p[i] = (yi - b2*q[i] - b0) / (b1 + b3*q[i]);
        if (p[i] > 1.0) p[i]=1.0;  // abnormal, likely due to round-off error
        if (p[i] < 0.0) p[i]=0.0;  // abnormal, likely due to round-off error
      } else {
        p[i] = 0.5;  //not uniq.
      }
    }
  }

}

/*
 * Conduct a quadrilateral mesh sampling with an equally-spaced regular grid
 *
 * Need to call quad_mesh_sample_init to set up.
 * 
 * (IN)
 * * z: The field value. One-dimensionalized 2D array of ns(minor) * nr (major).
 * *si, ri, p, q: obtained from quad_mesh_sample_init
 * (OUT)
 * * zg: The interpolated field value. 
 *   One-dimensionalized 2D array of nx (minor) * ny (major).
 */

static void quad_mesh_sample(int ns, int nr, double *z, int nx, int ny,   // (in)
                   int *si, int *ri, double *p, double *q, double misval, //(in)
                   double *zg)                           // (out)
{
  int i, is, ir;
  for (i=0; i<nx*ny; i++){
    is = si[i];
    ir = ri[i];
    if (is >= 0 && ir >= 0) {
      zg[i] = (1-p[i])*(1-q[i])*z[is+ns*ir]  
            + p[i]*(1-q[i])*z[is+1+ns*ir]
            + (1-p[i])*q[i]*z[is+ns*(ir+1)]
            + p[i]*q[i]*z[is+1+ns*(ir+1)];
    } else {
      zg[i] = misval;
    }
  }
}


///////////////////////////////////////////////////////////////////////
// ruby interface
///////////////////////////////////////////////////////////////////////
/*
 * misval: nil or Numeric(Float)
 */
VALUE rb_quad_mesh_sample_init(VALUE obj, VALUE x_na, VALUE y_na, // (in)
                        VALUE nx_i, VALUE x0_f, VALUE dx_f,     // (in)
                        VALUE ny_i, VALUE y0_f, VALUE dy_f,     // (in)
                        VALUE misval_f )     // (in)
{
  VALUE si_na, ri_na, p_na, q_na;  // (out)
  int ns, nr, nx, ny, rank;
  double *x, *y;
  double x0, dx, y0, dy, misval;
  int handle_miss;
  int *si, *ri;
  double *p, *q;
  struct NARRAY *na;
  na_shape_t sh[2];

  if (!IsNArray(x_na)) rb_raise(rb_eTypeError, "1st arg (x) must be an NArray");
  if (!IsNArray(y_na)) rb_raise(rb_eTypeError, "2nd arg (y) must be an NArray");

  rank = NA_RANK(x_na);
  if (rank != 2) rb_raise(rb_eArgError, "rank of x is not 2");
  rank = NA_RANK(y_na);
  if (rank != 2) rb_raise(rb_eArgError, "rank of x is not 2");

  GetNArray( na_cast_object(x_na, NA_DFLOAT), na);
  ns = na->shape[0];  
  nr = na->shape[1];  
  x = (double *) NA_PTR(na, 0);

  GetNArray( na_cast_object(y_na, NA_DFLOAT), na);
  if ( ns != na->shape[0] || nr != na->shape[1])
    rb_raise(rb_eArgError, "shapes of x and y do not agree");
  y = (double *) NA_PTR(na, 0);

  nx = NUM2INT( nx_i ); 
  ny = NUM2INT( ny_i ); 
  x0 = NUM2DBL( x0_f ); 
  dx = NUM2DBL( dx_f ); 
  y0 = NUM2DBL( y0_f ); 
  dy = NUM2DBL( dy_f ); 

  handle_miss = misval_f != Qnil;
  if (handle_miss) {
    misval = NUM2DBL( misval_f ); 
  } else {
    misval_f = 0.0; // just to avoid non-nitialization
  } 

  sh[0] = (na_shape_t) nx;
  sh[1] = (na_shape_t) ny;
  si_na = na_make_object(NA_LINT, 2, sh, cNArray);
  si = NA_PTR_TYPE(si_na, int *);
  ri_na = na_make_object(NA_LINT, 2, sh, cNArray);
  ri = NA_PTR_TYPE(ri_na, int *);
  p_na = na_make_object(NA_DFLOAT, 2, sh, cNArray);
  p = NA_PTR_TYPE(p_na, double *);
  q_na = na_make_object(NA_DFLOAT, 2, sh, cNArray);
  q = NA_PTR_TYPE(q_na, double *);

  quad_mesh_sample_init(ns, nr, x, y, nx, x0, dx, ny, y0, dy, // (in)
                     handle_miss, misval, // (in)
                     si, ri, p, q);

  return rb_ary_new3(4, si_na, ri_na, p_na, q_na);
}

VALUE rb_quad_mesh_sample(VALUE obj, VALUE z_na, VALUE si_na, VALUE ri_na,
                       VALUE p_na, VALUE q_na, VALUE misval_f)
{
  VALUE zg_na;
  int ns, nr, nx, ny;
  int *si, *ri; 
  double *p, *q, misval, *z, *zg;
  struct NARRAY *na;
  na_shape_t sh[2];

  GetNArray( na_cast_object(z_na, NA_DFLOAT), na);
  ns = na->shape[0];  
  nr = na->shape[1];  
  z = (double *) NA_PTR(na, 0);

  GetNArray( na_cast_object(si_na, NA_DFLOAT), na);
  nx = na->shape[0];
  ny = na->shape[1];
  si = NA_PTR_TYPE(si_na, int *);
  ri = NA_PTR_TYPE(ri_na, int *);
  p = NA_PTR_TYPE(p_na, double *);
  q = NA_PTR_TYPE(q_na, double *);
  misval = NUM2DBL( misval_f ); 

  sh[0] = (na_shape_t) nx;
  sh[1] = (na_shape_t) ny;
  zg_na = na_make_object(NA_DFLOAT, 2, sh, cNArray);
  zg = NA_PTR_TYPE(zg_na, double *);

  quad_mesh_sample(ns, nr, z, nx, ny, si, ri, p, q, misval, zg);

  return zg_na;
}

void
init_gphys_quad_mesh_sample()
{
    static VALUE mNumRu;
    static VALUE cGPhys;
    mNumRu = rb_define_module("NumRu");
    cGPhys = rb_define_class_under(mNumRu, "GPhys", rb_cObject);
    rb_define_singleton_method(cGPhys, "quad_mesh_sample_init",
                               rb_quad_mesh_sample_init, 9);
    rb_define_singleton_method(cGPhys, "quad_mesh_sample",
                               rb_quad_mesh_sample, 6);
}

/*
///////////////////////////////////////////////////////////////////////
// gcc -Wall -DTESTMAIN -lm -g quad_mesh_sample.c
///////////////////////////////////////////////////////////////////////
#ifdef TESTMAIN
#include <stdlib.h>

int main(int argc, char *argv[]) {
  int ns=21, nr=6; // sizes of the original quadrilateral mesh
                   // (ns:minor, nr:major)
  int nx=25, ny=25; // sizes of the regular grid to resample(nx:minor, ny:major)
  double x[ns*nr],  y[ns*nr];  // the mesh
  double z[ns*nr],  zg[nx*ny]; // orig & resampled values
  double x0=-nr, dx=0.5, y0=-nr, dy=0.5;
  int si[nx*ny], ri[nx*ny];
  double p[nx*ny], q[nx*ny];
  int is,ir,i;
  double r, d, theta, misval=99.99;
  
  // polar coordinate
  d = 2.0 * M_PI / (ns - 1.0);
  for ( ir=0 ; ir<nr ; ir++ ){
    r = (double) ir;
    for ( is=0 ; is<ns ; is++ ){
      theta = d*is;
      x[is + ns*ir] = r * cos(theta);
      y[is + ns*ir] = r * sin(theta);
      z[is + ns*ir] = 0.1 * sqrt(r) * sin(2*theta);  // wavenum-2 field
    }
  }

  // prep
  quad_mesh_sample_init(ns, nr, x, y, nx, x0, dx, ny, y0, dy, // (in)
                     0, 0.0,  // (in)
                     si, ri, p, q);

  printf("si\n");
  printf("    ");  for(i=0;i<nx;i++) printf(" %2d",i);
  printf("\n %2d:",0);
  for (i=0; i<nx*ny; i++){
    printf("%3d",si[i]);
    if ( (i+1) % nx == 0) printf("\n %2d:",(i+1)/nx);
  }
  printf("\nri\n %2d:",0);
  for (i=0; i<nx*ny; i++){
    printf("%3d",ri[i]);
    if ( (i+1) % nx == 0) printf("\n %2d:",(i+1)/nx);
  }
  printf("\n");

  printf("\nq\n %2d:",0);
  for (i=0; i<nx*ny; i++){
    printf("%5.2f",q[i]);
    if ( (i+1) % nx == 0) printf("\n %2d:",(i+1)/nx);
  }
  printf("\n");
  printf("\np\n %2d:",0);
  for (i=0; i<nx*ny; i++){
    printf("%5.2f",p[i]);
    if ( (i+1) % nx == 0) printf("\n %2d:",(i+1)/nx);
  }
  printf("\n");

  quad_mesh_sample(ns, nr, z, nx, ny, si, ri, p, q, misval, zg);

  printf("\nzg\n %2d:",0);
  for (i=0; i<nx*ny; i++){
    printf("%5.2f",zg[i]);
    if ( (i+1) % nx == 0) printf("\n %2d:",(i+1)/nx);
  }

  return 0;
}
#endif
*/