/*
This file is part of EZRide.
Copyright 2007 Dwight Barkley, 
Copyright 2010 Vadim Biktashev & Andrew Foulkes.

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published
by the Free Software Foundation, either version 3 of the License,
or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "ezride.h"

/*---------------------------------------------------------------------------*
 * These subroutines compute the location of the spiral tips. The tip (x,y) is
 * defined implicitly by f1(x,y) = f2(x,y) = 0. The functions f1 and f2 are
 * defined at the end of this file and can in principle be almost any
 * functions. Currently they are such as to define the tip as the intersection
 * of a pair of u,v contours. Newton iterations are used to find the root
 * (x,y). Derivatives are determined by finite differencing.  The u and v
 * fields are approximated (interpolated) with polynomials of order
 * N_ORDER. **THE CODE ASSUMES N_ORDER IS ODD.** N_ORDER=3 gives accurate
 * results.
 *
 * Note that each time Find_tips () is called, the entire grid is searched for
 * possible tips. This is a bit inefficient if there is only one tip. However
 * timing tests show that it takes little time and allows for finding multiple
 * tips.
 *---------------------------------------------------------------------------*/


#define NORDER 3         /* interpolation order for fields (MUST BE ODD)     */
#define DX     0.05      /* finite diff used for computing derivatives       */
#define DY     0.05      /* finite diff used for computing derivatives       */
#define TOL2   1.e-10    /* tol (squared) for newton convergence             */
#define TOOBIG 10.       /* max size of eps2 in newton (to prevent blowup)   */
#define MXNWT  8         /* max number of newton iterations                  */

#define PRNT_WARN(verbose,level,message) if(verbose>=level) printf(message);

/* 
 * Private functions 
 * ----------------- */

static int   Newton      (Real *px, Real *py, Real *pth);
static Real  Eval_func   (Real (*f)(int i, int j), Real x, Real y, int new);
static Real  hh          (Real x_local, int i); 
static Real  f1          (int i, int j);
static Real  f2          (int i, int j);
static inline double sq(double x) {return x*x;}


/* ========================================================================= */

Real x_tip,y_tip;

/* ========================================================================= */

void Tip_ini (void)
{
  int max; 

  max=nsteps+NX+NY;

  tip_x =(Real *) malloc((unsigned)(max)*sizeof(Real));
  tip_y =(Real *) malloc((unsigned)(max)*sizeof(Real));
  tip_x[0] = 0.;
  tip_y[0] = 0.;
  ntips = 0;
}
/* ========================================================================= */

void Find_tips (void) 
{
  Real x,y,theta=0;
  Real R, R2;
  Real f100, f101, f110, f111, f200, f201, f210, f211;
  int i,j,count=0,imin,imax,jmin,jmax;/*,fact;*/
  unsigned int uindex, vindex;
  Real D2;

  imin=2;
  imax=NX-1;
  jmin=2;
  jmax=NY-1;
  R=NX2-2;
  if ((NY2-2)<R) R=NY2-2; /* inscribed disk: smaller of the two radii */

  if (!NBC) {
    imin+=TIPRIM;
    imax-=TIPRIM;
    jmin+=TIPRIM;
    jmax-=TIPRIM;
    R-=TIPRIM;
  }
  R2=R*R;

  /* Search the grid (except close to edges) */
  for(i=imin;i<imax;i++) {
    for(j=jmin;j<jmax;j++) {

      /* in the ride mode, restrict attention to the inscribed disk */
      if (ride_on) {
	D2 = sq(i-NX2) + sq(j-NY2);
	if (D2  > R2) continue;
      }
      
      f100=f1(i,j); f101=f1(i+1,j); f110=f1(i,j+1); f111=f1(i+1,j+1);
      uindex=(f100>0.)|((f101>0.)<<1)|((f110>0.)<<2)|((f111>0.)<<3);
      
      if ((uindex != 0) && (uindex != 15)) {        /* non-trivial index for u */


        f200=f2(i,j); f201=f2(i+1,j); f210=f2(i,j+1); f211=f2(i+1,j+1);
        vindex=(f200>0.)|((f201>0.)<<1)|((f210>0.)<<2)|((f211>0.)<<3);
      
        if ((vindex != 0) && (vindex != 15)) {        /* non-trivial index for v */

	  /* Both u and v have non-trivial index. Possible tip. Take initial
             guess to be center of this square */

	  x = (Real)i + 0.5;    
	  y = (Real)j + 0.5;

	  /* Use Newton iteration to find tip.  Do not do this if above guess
             is too close to the last computed tip at this time step */
	  if( (ntips==0) || (count==0) || (sq(x-x_tip)+sq(y-y_tip)>4) ) {
	    if( Newton(&x, &y, &theta) ) {
	      x_tip=x;
	      y_tip=y;                  /* real grid coords of the tip, for plotting */
	      XX=i;
	      YY=j;                     /* integer grid coordinates of the tip, for the Mover */
              X0=grid_h*(x-NX2);
	      Y0=grid_h*(y-NY2);
	      TH0 = theta;               /* model coords of the tip, for output */
	      count++;                  /* number of tips at this time moment */
	    } /* newton successful */
	  } /* new tip is not too close */
        } /* nontrivial index for v */
      } /* nontrivial index for u */
    } /* for j */
  } /* for i */

  if(verbose>=2) printf("number of tips in one field= %d\n", count);
  if (count) {
    tip_x[ntips]=x_tip;
    tip_y[ntips]=y_tip;                 /* real grid coords of the tip, for plotting */
    ntips++;				/* number of tips to be plotted */
    if (write_tip)
      Write_tip_data (X0,Y0,TH0);          /* model coords of the tip, for output */
  }
}
/* ========================================================================= */

static int Newton (Real *px, Real *py, Real *pth)
{
  Real x=*px, y=*py, theta=*pth, f1_val, f2_val, df1dx, df1dy, df2dx, df2dy, jac, nrm2;
  int nwt=0, new=1, old=0;    /* new & old determine whether new
			       * interpolating polynomials are
			       * generated */

  while(nwt<MXNWT) {

    f1_val = Eval_func(f1,x,y,new);           /* compute f1 at current (x,y) */
    f2_val = Eval_func(f2,x,y,old);           /* compute f2 at current (x,y) */
    nrm2 = f1_val*f1_val + f2_val*f2_val;               /* norm of functions */

    if(nrm2>TOOBIG) return(0);

    if(nrm2<TOL2) {                                              /* Success! */
      *px  = x;  
      *py  = y;
      *pth = theta;
      return(1);
    }

    /* Compute new x and y by taking one Newton step */
    df1dx = (Eval_func(f1,x+DX,y,new) - f1_val)/DX;
    df2dx = (Eval_func(f2,x+DX,y,old) - f2_val)/DX;
    df1dy = (Eval_func(f1,x,y+DY,new) - f1_val)/DY;
    df2dy = (Eval_func(f2,x,y+DY,old) - f2_val)/DY;
    jac   = df1dx*df2dy - df1dy*df2dx;
    x -= ( df2dy*f1_val - df1dy*f2_val)/jac;
    y -= (-df2dx*f1_val + df1dx*f2_val)/jac;
    theta = atan2(df1dy,df1dx);
    nwt++;
  }

  /* Too many Newton iterations */
  PRNT_WARN(verbose,2,"Warning: nwt > MXNWT in Newton()\n");
  return(0);
}

/* ========================================================================= */

void tips(Real x_tip, Real y_tip)
{
	/* Purpose of this function is so that the allocation *
         * of the tips is not wasted, by allocating the tips  *
         * to the arrays tip_x and tip_y more than once for   *
         * each time step. tips(c_tip,y_tip) can be called    *
         * upon only once during a loop.                      */

	tip_x[ntips] = x_tip;
	tip_y[ntips] = y_tip;
 
	ntips++;
}

/* ========================================================================= */

Real Eval_func (Real (*f)(int i, int j), Real x, Real y, int new)
{
  /* Uses Lagrangian interpolation to find value of function f at point
   * (x,y).  If new=1, then this is a new (x,y) point and new interpolating
   * polynomials are computed.  */

  static Real x_local, y_local, hx[NORDER+1], hy[NORDER+1]; 
  static int ix_0, iy_0;
  Real sum=0.;
  int i,j;

  if(new) { 
    /* Generate new polynomials */
    ix_0 = (int)x - ((NORDER+1)/2 -1);
    iy_0 = (int)y - ((NORDER+1)/2 -1);
    x_local = x - ix_0;
    y_local = y - iy_0;
    for(i=0;i<(NORDER+1);i++) {
      hx[i] = hh(x_local,i);
      hy[i] = hh(y_local,i);
    }
  }

  /* Test for out of range */
  if(ix_0<1||ix_0>NX-NORDER||iy_0<1||iy_0>NY-NORDER) {
    PRNT_WARN(verbose,2,"Warning: (x,y) out of range in Eval_func() \n");
    return(TOOBIG+1.);
  }

  /* Interpolate function */
  for(i=0;i<(NORDER+1);i++) {
    for(j=0;j<(NORDER+1);j++) {
      sum += hx[i]*hy[j]*(*f)(ix_0+i,iy_0+j);
    }  
  }
  return(sum);
}
/* ========================================================================= */

Real hh (Real x_local, int i)
{
  /* Computes the ith Lagrange interpolating polynomial at point x_local.  */

  Real product=1.;
  int j;

  for(j=0;j<(NORDER+1);j++) 
    if(j!=i) product *= (x_local-(Real)j)/(Real)(i-j);

  return(product);
} 
/* ========================================================================= */


/* Define Functions f1 and f2 
 *
 * Here f1 and f2 are such as to define the spiral 
 * tip as the intersection of the contours: 
 * u=Utip and v=Vtip (defined in ezride.c) 
 * ----------------------------------------------- */

Real f1 (int i, int j) 
{
  return(U(i,j)-Utip);
}
/* ========================================================================= */

Real f2 (int i, int j) 
{
  return(V(i,j)-Vtip);
}
/* ========================================================================= */