/*

   [xx, yy] = arc(x, y, narc, eps)

   x, y are vectors or matrices; if the latter, they are
   simply treated as a set of column vectors.
   xx, yy are corresponding vectors or matrices that include
   x, y and points interpolated between them so as to make a
   smooth curve.  The number of points in xx for each pair
   in x is narc; e.g., narc=3 interpolates a single point
   between each pair.
   If the distance between any successive (x,y) pair is less
   than eps, then no points are interpolated for that pair.



   arc.c

   The arc subroutine is taken from the consub1.f module of
   Roger Lukas' "contour" program.  It was translated to C
   using f2c, and then modified to reduce "fortranisms".  A
   mex wrapper was then added.  It could be coded much more
   efficiently by putting the loop through the input points
   into the arc routine itself, so that first differences
   would be calculated only once per pair of points; but the
   present version was the quickest way to get a usable mex
   file from the Fortran. Optimization can be done later.

   The interpolated curve is smooth so long as the changes
   in angle from one line segment to the next, as defined by
   the input points, are not too large.  If the input curve
   is jerky, the output will have discrete bends but will
   still have interpolated points.  A better way to handle
   this might be to simply not interpolate whenever the
   input angles are too sharp.  This can be enabled by
   uncommenting the "goto dont_interpolate" statements.  One
   can also fiddle with angmax to adjust the freedom of the
   algorithm to make fanciful loops from random inputs!

   Eric Firing
   Sun  97/04/20

*/


#include "mex.h"
#include "matrix.h"
#include <stdio.h>
#include <math.h>

#ifndef M_PI
#define M_PI        3.14159265358979323846
#endif

int arc(double *x, double *y, int nask, double eps, double *xx, double *yy);


void mexFunction(
   int nlhs, mxArray *plhs[],
   int nrhs, const mxArray* prhs[])
{
   double *x, *y;
   double *xx, *yy;
   double eps;
   int nask;

   double NaN;

   double xc[4], yc[4], *xxc, *yyc;

   int nrx, ncx, nry, ncy;
   int nrxx, ncxx;
   int ii, jj, nret;

   NaN = mxGetNaN();

   if (nrhs != 4)
   {
      mexErrMsgTxt("Need 4 inputs: x,y,nask,eps.");
   }
   x = mxGetPr(prhs[0]);
   y = mxGetPr(prhs[1]);
   ncx = mxGetN(prhs[0]);
   nrx = mxGetM(prhs[0]);
   ncy = mxGetN(prhs[1]);
   nry = mxGetM(prhs[1]);
   if (ncx != ncy || nrx != nry || ncx*nrx < 2)
   {
      mexErrMsgTxt("Input arrays don't match or are scalars.");
   }
   if (nrx = 1)  /* Access a row vector as if it were a column. */
   {
      nrx = ncx;
      ncx = 1;
   }
   /* No input checking yet for these. */
   nask = (int) mxGetScalar(prhs[2]);
   eps = mxGetScalar(prhs[3]);

   ncxx = ncx;
   nrxx = (nrx - 1) * (nask - 1) + 1;

   if (nrx == 2) /* Only two points: just return them. */
   {
      plhs[0] = prhs[0];
      plhs[1] = prhs[1];
      return;
   }

   plhs[0] = mxCreateDoubleMatrix(nrxx, ncxx, mxREAL);
   plhs[1] = mxCreateDoubleMatrix(nrxx, ncxx, mxREAL);

   xx = mxGetPr(plhs[0]);
   yy = mxGetPr(plhs[1]);

   for (jj = 0; jj < ncx; jj++)
   {
      xxc = xx + jj*nrxx;
      yyc = yy + jj*nrxx;
      for (ii = -1; ii < (nrx-2); ii++)
      {
         int i0, i1, i2, i3;
         if (ii == -1)
         {
            i0 = i1 = jj*nrx;
         }
         else
         {
            i0 = jj*nrx + ii;
            i1 = i0 + 1;
         }
         if (ii == nrx-3)
         {
            i2 = i3 = i1 + 1;
         }
         else
         {
            i2 = i1 + 1;
            i3 = i2 + 1;
         }
         xc[0] = x[i0];
         xc[1] = x[i1];
         xc[2] = x[i2];
         xc[3] = x[i3];
         yc[0] = y[i0];
         yc[1] = y[i1];
         yc[2] = y[i2];
         yc[3] = y[i3];
         nret = arc(xc, yc, nask, eps, xxc, yyc);
         xxc += (nret - 1);
         yyc += (nret - 1);
      }
      xxc++;
      yyc++;
      /* If nret was less than nask for any intervals, the
      number of points in the curve will be fewer than
      anticipated; NaN them out.
      */
      while (xxc < (xx + (jj+1) * nrxx))
      {
         *xxc = NaN;
         *yyc = NaN;
         xxc++;
         yyc++;
      }

   }
}


int arc(double *x, double *y, int nask, double eps, double *xx, double *yy)
{
    /* Initialized data */

    static double pi =  M_PI; /* 3.141592654f; */
    static double hafpi = M_PI/2; /* 1.570796327f; */
    static double angmax = M_PI/4; /*.785398164f; */

    /* System generated locals */
    int i__1;

    /* Builtin functions */
/*    double sqrt(double), atan2(double,
double), tan(double); */

    /* Local variables */
    static double c, d;
    static int k, nret;
    static double xcent, slope[3], ycent, ds2haf, ds[3], dx, dy, xt, yt, ds2,
	    dx2, dy2, ang, ang2, cos2, sin2;


/*     Given four successive points X(K),Y(K),K=1,4 defining 3 segments, 
*/
/*     the points XX(K),YY(K),K=1,NRET are generated describing a curve */

/*     between points 2 and 3 trying to match slope with similar curves */

/*     from 1 to 2 and from 3 to 4. If the segment (2,3) is less than EPS 
*/
/*     in length,NRET is set to 2 and points 2 and 3 only are returned in
*/
/*     arrays XX,YY.  Otherwise NRET = the input parameter NASK. */
/*     the curve XX,YY Is a cubic over segment (2,3). */
/*     If segment (1,2) is less than EPS the second derivative at */
/*     point 2 is set to zero.   If segment (3,4) is less than EPS */
/*     the second derivative at point 3 is set to zero.  These two */
/*     features may be used for starting and ending an open curve. */
/*     OCEANOGRAPHY EMR  DECEMBER 1969 */

    /* Parameter adjustments so that indexing starts at 1,
    not 0*/
    --y;
    --x;

    /* Function Body */


    dx2 = x[3] - x[2];
    dy2 = y[3] - y[2];
    ds2 = sqrt(dx2 * dx2 + dy2 * dy2);

/*      IF THE DISTANCE BETWEEN THE TWO POINTS IS ALREADY */
/*      SMALL, NO NEED TO PLOT A CUBIC ARC */

    if (ds2 < eps || nask <= 2)
    {
dont_interpolate:
       nret = 2;
       xx[0] = x[2];
       yy[0] = y[2];
       xx[1] = x[3];
       yy[1] = y[3];
       return nret;
    }


    xcent = (x[2] + x[3]) * 0.5;
    ycent = (y[2] + y[3]) * 0.5;
    ang2 = atan2(dy2, dx2);
    cos2 = dx2 / ds2;
    sin2 = dy2 / ds2;

/*      COMPUTE LENGTH AND DIRECTION OF THE LINE SEGMENTS */
/*      CONNECTING THE TWO POINTS TO THE ADJACENT POINTS */

    for (k = 1; k <= 3; k += 2)
    {
       dx = x[k + 1] - x[k];
       dy = y[k + 1] - y[k];
       ds[k - 1] = sqrt(dx * dx + dy * dy);
       slope[k - 1] = 0.0;
       if (ds[k - 1] >= eps)
       {
          ang = (atan2(dy, dx) - ang2) * 0.5;
          if (ang > hafpi) {
              ang -= pi;
          }
          if (ang < -hafpi) {
              ang += pi;
          }
          if (ang > angmax) {
              ang = angmax;
           /*   goto dont_interpolate; */
          }
          if (ang < -angmax) {
              ang = -angmax;
           /*    goto dont_interpolate;   */
          }
          slope[k - 1] = tan(ang);
       }
    }


    if (ds[2] < eps) {
      slope[2] = -slope[0] * 0.5;
    }
    if (ds[0] < eps) {
      slope[0] = -slope[2] * 0.5;
    }
    c = (slope[2] - slope[0]) * 0.25;
    d = (slope[2] + slope[0]) * 0.25;

/*      INTERPOLATE THE REQUIRED # OF POINTS BETWEEN THE TWO */
/*      CENTER POINTS. */

    nret = nask;
    dx = 2.0 / (nret - 1);
    xt = -1.0 - dx;
    ds2haf = ds2 * 0.5;
    for (k = 0; k < nret; ++k)
    {
       xt += dx;
       yt = (c + d * xt) * (xt * xt - 1.0);
       xx[k] = (xt * cos2 - yt * sin2) * ds2haf + xcent;
       yy[k] = (xt * sin2 + yt * cos2) * ds2haf + ycent;
    }
    return nret;
} /* arc */