#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <stdio.h>

void binary_search(double * key, int * index, int N, double * arr, int len);
void blossom(double * x, int nx, int * i, int * j, double * B, int N, int order_stop, int order, int nj);
void blossom_grid(double * t, int nt, double * x, int nx, int * i, int * j, double * B, int N, int order_stop, int order, int nj);

/*
 * blossoming function: compute the spline values, at the requested
 * order for the x's passed in argument. Return triplets (i,j,B) which
 * are used to construct a sparse jacobian matrix: B_j(x_i)
 * 
 * args
 * ----
 *  - x     : the x values, rescaled on the integer grid: [order-1, jmax]
 *  - nx    : size of the x array 
 *  - i, j  : line and column indices of the jacobian matrix
 *  - B     : values of the jacobian mastrix (B_j(x_i))
 *  - N     : size of the i,j and B vectors ( = nx*order)
 *  - order_stop : stop blossoming at order_stop (<order)
 *  - order : spline order 
 *  - nj  : size of the spline basis
 * 
 * returns
 * -------
 *   void 
 *
 * Note: for the x's which are outside the basis range, we return 0.
 */
void blossom(double * x, int nx, int * i, int * j, double * B, int N, int order_stop, int order, int nj)
{
  int p,q,k;
  int deg = order-1;
  assert(N == nx*order);
  
  /* -------------------------------------------------------------- */
  /* initialize i, j, B                                             */
  /*                                                                */
  /*   i    ... | k | k | k | ... | k       | ...                   */
  /*   j    ... |   |   |   | ... | E(x_i)  | ...                   */
  /*   B    ... | 0 | 0 | 0 | ... | 1       | ...                   */
  /*                                                                */
  /* -------------------------------------------------------------- */  
  for(k=0; k<nx; k++)
    {
      p = order*k;
      B[p+deg] = 1.;
      i[p+deg] = k;
      j[p+deg] = (int)floor(x[k]);
      for(q=deg-1; q>=0; q--)
	{
	  i[p+q] = k;
	  j[p+q] = j[p+q+1]-1;
	}
      if(x[k]<deg || x[k]>=nj)
	{
	  B[p+deg] = 0.;
	  for(q=deg; q>=0; q--)
	      j[p+q] = 0;
	}
    }
  
  /* blossom */
  int oo;
  double xx, scale, o1, o2;
  register double l1, l2;
  /* loop on the spline orders */
  for(oo=2; oo<=order_stop; oo++)
    {
      scale = 1. / (double)(oo-1);
      /* loop on the xi's */
      for(k=0; k<nx; k++)
  	{
  	  p = order*k;
  	  xx = x[k];
	  o1 = (xx-j[p+deg]) * scale;
  	  l1 = o1 * B[p+deg];
	  /* loop on the splines that have non-zero values */
  	  for(q=deg-1; q>=(order-oo); q--)
  	    {
  	      o1 = (xx-j[p+q]) * scale;
  	      o2 = (xx-j[p+q+1]) * scale;
  	      l2 = l1;
  	      l1 = o1 * B[p+q] + (1.-o2) * B[p+q+1];
  	      B[p+q+1] = l2;
  	    }
	  B[p+q+1] = l1;
  	}
    }
  //  for(k=0; k<N; k++)
  //    {
  //      //      j[k] += deg;
  //      if((j[k]<0.) || (j[k]>=nj)) 
  //	j[k] = B[k] = 0.;
  //    }
}

/* 
 * same as above, but the grid can be irregular. 
 * 
 * 
 * 
 */
void blossom_grid(double * t, int nt, double * x, int nx, int * i, int * j, double * B, int N, int order_stop, int order, int nj)
{
  int p,q,k;
  int deg = order-1;
  assert(N == nx*order);
  
  int * jj = (int*)malloc(nx * sizeof(int));
  double * scales = (double*)malloc(nt * sizeof(double));

  /* we cannot escape a binary search to locate the x's in the grid */
  binary_search(x, jj, nx, t, nt);
  
  /* initialize i,j,B */
  for(k=0;k<nx;k++)
    {
      p = order*k;
      B[p+deg] = 1.;
      i[p+deg] = k;
      j[p+deg] = jj[k];
      for(q=deg-1;q>=0;q--)
	{
	  i[p+q] = k;
	  j[p+q] = j[p+q+1]-1;
	}
      if(jj[k]<deg || jj[k]>=nj)
	{
	  B[p+deg] = 0;
	  for(q=deg; q>=0; q--)
	      j[p+q] = 0;
	}
    }
  
  /* blossom */
  int oo, jx;
  double xx, o1, o2;
  register double l1, l2, s;
  for(oo=2; oo<=order_stop; oo++)
    {
      /* compute the scales */
      for(p=0; p<nt-1; p++)
	{
	  s = 0.;
	  jx = p + oo - 1;
	  if(jx<nt)
	    s = t[jx] - t[p];
	  if(s>0.)
	    scales[p] = 1. / s; // (t[jxo] - t[jx]);
	  else
	    scales[p] = 0.;
	}
      /* B-Spline values, from the preceding order */
      for(k=0; k<nx; k++)
	{
	  p = order * k;
	  xx = x[k];
	  jx = j[p+deg];
	  o1 = (xx-t[jx]) * scales[jx];
	  l1 = o1 * B[p+deg];
	  for(q=deg-1; q>=(order-oo); q--)
	    {
	      jx = j[p+q];
	      o1 = (xx - t[jx]) * scales[jx];
	      o2 = (xx - t[jx+1]) * scales[jx+1];
	      l2 = l1;
	      l1 = o1 * B[p+q] + (1.-o2) * B[p+q+1];
	      B[p+q+1] = l2;
	    }
	  B[p+q+1] = l1;
	}
    }
  
  free(jj);
  free(scales);
}

/* 
 *  deriv
 * 
 *  implement the formula:
 *    dB_{jk}/dx = (k-1) * (-B_{j+1,k-1}/(t_{j+k}-t_{j+1}) + B_{j,j-1}/(t_{j+k-1}-t_{j}))
 */
void deriv_grid(double * t, int nt, int * i, int * j, double * B, int N, int nx, int order)
{
  int p,q,k,jx;
  int deg = order - 1;
  double km1 = order-1.;
  register double s, l1, l2;
  double * scale = (double*)malloc(nt * sizeof(double));
  for(p=0; p<nt; p++)
    {
      if(p+order>=nt)
	scale[p] = 0.;
      s = t[p+order-1] - t[p];
      scale[p] = s>0. ? 1./s : 0.;
    }
  
  for(k=0; k<nx;k++)
    {
      p = order*k;
      jx = j[p+deg];
      l1 = B[p+deg] * scale[jx] * km1;
      for(q=deg-1;q>=0;q--)
	{
	  jx = j[p+q];
	  l2 = l1;
	  l1 = (-B[p+q+1] * scale[jx+1] + B[p+q] * scale[jx]) * km1;
	  B[p+q+1] = l2;
	}      
      B[p+q+1] = l1;
    }
  free(scale);
}


/* 
 * binary search with guess (for general BSplines)
 * 
 * largely borrowed from numpy/core/src/multiarray/compile_base.c
 */
#define LIKELY_IN_CACHE_SIZE 8
int binary_search_with_guess(double key, double * arr, int len, int guess)
{
  int imin=0, imax=len;
  
  /* key outside grid boundaries */
  if(key < arr[0])
    return -1;
  if(key > arr[len-1])
    return len;

  /* if len<=4, perform a linear search */
  if(len <= 4)
    {
      int i;
      for(i=1; i<len && key>=arr[i]; ++i);
      return i-1;
    }
  
  /* try the values close to guess */
  if(guess > len - 3)
    guess = len - 3;
  if(guess < 1)
    guess = 1;

  /* check most likely values: guess-1, guess, guess+1 */
  if(key < arr[guess])
    {
      if(key < arr[guess-1])
	{
	  imax = guess-1;
	  if(guess > LIKELY_IN_CACHE_SIZE && 
	     key >= arr[guess - LIKELY_IN_CACHE_SIZE])
	    {
	      imin = guess - LIKELY_IN_CACHE_SIZE;
	    }
	}
      else
	{
	  return guess-1;
	}
    }
  else /* key >= arr[guess] */
    {
      if(key < arr[guess+1])
	{
	  return guess;
	}
      else
	{
	  if(key < arr[guess+2])
	    {
	      return guess + 1;
	    }
	  else
	    {
	      imin = guess + 2;
	      if(guess < len - LIKELY_IN_CACHE_SIZE - 1 && 
		 key < arr[guess + LIKELY_IN_CACHE_SIZE])
		{
		  imax = guess + LIKELY_IN_CACHE_SIZE;
		}
	    }
	}
    }
  
  /* still not found, but we now have a (imin, imax), so try a bisection */
  while(imin < imax)
    {
      const int imid = imin + ((imax-imin) >> 1);
      if(key >= arr[imid])
	imin = imid + 1;
      else
	imax = imid;	
    }
  return imin - 1;  
}
#undef LIKELY_IN_CACHE_SIZE

/* 
 * Seems to be faster than np.digitize
 */
void binary_search(double * key, int * index, int N, double * arr, int len)
{
  int i, guess = len>>1;
  for(i=0; i<N; i++)
    {
      index[i] = binary_search_with_guess(key[i], arr, len, guess);
      guess = index[i];
    }
}









void test(double * x, int nx, double * grid, int ngrid, int * j, double * B, int N, int order)
{
  int i;
  for(i=0; i<N; i++)
    {
      int r = i%order;
      B[i] = (r==(order-1));
      j[i] -= (order-r);
    }
  
}



