float noise::atPoint( float x, float y, float z )
{
	int ix, iy, iz;
	int i, j, k;
	float fx, fy, fz;
	float xknots[4], yknots[4], zknots[4];

	if ( !isInitialized ) {
		initTable( 23479015 );
	}

	ix = (int)floorf( x );
	fx = x - (float)ix; 

	iy = (int)floorf( y );
	fy = y - (float)iy;

	iz = (int)floorf( z );
	fz = z - (float)iz;

	for ( k = -1; k <= 2; k++ ) {
		for ( j = -1; j <= 2; j++ ) {
			for ( i = -1; i <= 2 ; i++ ) {
				xknots[i+1] = value( ix + i, iy + j, iz + k );
			}
			yknots[j+1] = spline( fx, xknots[0], xknots[1], xknots[2], xknots[3] );
		}
		zknots[k+1] = spline( fy, yknots[0], yknots[1], yknots[2], yknots[3] );
	}

	float val = spline( fz, zknots[0], zknots[1], zknots[2], zknots[3] ); 

	return val;
}

void testSine()
{
    typedef Eigen::Spline<double,1> Spline1d;

    // create data
    const size_t numData = 1000;
    Eigen::VectorXd x(numData);
    Eigen::VectorXd y(numData);
    const double a = 0;
    const double b = 4*M_PI;
    const double dx = (b-a)/double(numData);
    for(size_t i=0;i<numData;++i)
    {
        x(i) = a + i*dx;
        y(i) = std::sin(x(i));
    }

    // create a spline
    Spline1d spline(
        Eigen::SplineFitting<Spline1d>::Interpolate(y.transpose(),
            std::min<int>(x.rows() - 1, 3),scaledValues(x))
        );

    std::ofstream outFile;
    outFile.open("eigenSplinePlot.dat",std::ios::trunc);
    for(size_t i=0;i<numData;++i)
    {
        double xiSc = scaledValue(x.minCoeff(),x.maxCoeff())(x(i));
        double yiSpl = spline(xiSc)(0);
        outFile<<x(i)<<"\t"<<y(i)<<"\t"<<yiSpl<<std::endl;
    }
    outFile.close();

}

FunVals LogSplines::operator ()(double xmin, double xmax, int nBins) const
{
    if(!m_cubicSpline) return FunVals();
    const LogSplines& spline = *this;

    //error checking
    if(xmin == xmax)
    {
        FunVal splineVal;
        splineVal.first = xmin;
        splineVal.second= spline(xmin);
        return FunVals(1,splineVal);
    }
    if(xmax < xmin) std::swap(xmax,xmin);
    if(nBins <= 0) return FunVals();

    //calculate step
    double h = (xmax-xmin)/nBins;
    FunVals splineVals(nBins+1);
    std::for_each(splineVals.begin(),
                  splineVals.end(),
                  [h,&xmin,spline](FunVal& splineVal)->void
    {
       splineVal.first = xmin;
       splineVal.second= spline(xmin);
       xmin += h;
    });

    return splineVals;
}

            Real value(Real x, Real y) const {
                std::vector<Real> section(splines_.size());
                for (Size i=0; i<splines_.size(); i++)
                    section[i]=splines_[i](x,true);

                CubicInterpolation spline(this->yBegin_, this->yEnd_,
                                          section.begin(),
                                          CubicInterpolation::Spline, false,
                                          CubicInterpolation::SecondDerivative, 0.0,
                                          CubicInterpolation::SecondDerivative, 0.0);
                return spline(y,true);
            }

/*---integral-------------------------------------------------------------*/
void findq()
{
 float h=0.01,x,Q,a=0,b=1;
  x=0.01;
  Q=0;
  while(x<0.98){
    Q=(spline(x)*spline(x)*2)/3+(spline(x+h)*spline(x+h))/3;
    x=x+2*h;
  }
  Q=(spline(x)*spline(x)*2)/3+(spline(a)*spline(a)+spline(b)*spline(b))/6;
  Q=Q*2*h;
  printf("Q = %f\n\n",Q);
}

void
BicubicSplineInterpolation::constructRowSplineSecondDerivativeTable()
{
  auto m = _x1.size();
  _y2_rows.resize(m);

  if (_yx21.empty())
    for (decltype(m) j = 0; j < m; ++j)
      spline(_x2, _y[j], _y2_rows[j]);

  else
    for (decltype(m) j = 0; j < m; ++j)
      spline(_x2, _y[j], _y2_rows[j], _yx21[j], _yx2n[j]);
}

void graphik()
{
  int gd=0, gm;
  initgraph(&gd,&gm,"c:\\lang\\bgi");
  cleardevice();
  uuu[0]=y0;
  yyy[0]=y0;
  yyy[1]=kkk;
  line(kx(-0.1),ky(0),kx(1.5),ky(0));
  line(kx(0),ky(-0.1),kx(0),ky(1.5));
  outtextxy(kx(0.01),ky(-0.01),"0");
  circle(kx(0),ky(1),1);
  circle(kx(1),ky(0),1);
  outtextxy(kx(-0.02),ky(1),"1");
  outtextxy(kx(1.01),ky(-0.01),"1");
  float i=0;
  int j=1,ind=0;
  setcolor(11);
  circle(kx(0),ky(uuu[0]),2);
  moveto(kx(0),ky(yyy[0]));
  while(i<=1){
    rongekutt(i,H,yyy);
    i=i+H;
    ind++;
    lineto(kx(i),ky(yyy[0]));
    if(ind==int(0.2/H)){
      ind=0;
      uuu[j]=yyy[0];
      circle(kx(i),ky(uuu[j]),2);
      j=j+1;
    }
  }
  outtextxy(450,50,"- graphik");
  getch();
  znach();
  prhod(ccc,ddd);
  obrhod(ccc,ddd,mmm);
  setcolor(13);
  outtextxy(450,60,"- spline");
  i=0;
  moveto(kx(i),ky(spline(i)));
  while(i<=1){
    lineto(kx(i),ky(spline(i)));
    i=i+H;
  }
  getch();
  closegraph();
}

/* ===========================================================================
   get_spline   x , y - spline points
   xx, yy - output (allocated) spline interpolation 
   =========================================================================== */
void get_spline(int i_x[],int i_y[],int nwhisker_points, int min_x, int max_x, int **yy)
{

  int i, status, x_val;
  float *x, *y, *y2;
  float y_val;
  int start_ind, end_ind;

  x = allocate_vector( 1, nwhisker_points );
  y = allocate_vector( 1, nwhisker_points );
  y2 = allocate_vector( 1, nwhisker_points );

  for (i = 0; i<nwhisker_points; i++) {
    x[i+1] = i_x[i] + 0.;
    y[i+1] = i_y[i] + 0.;
  }

  *yy = allocate_ivector( min_x, max_x );
  
  spline( x,y, y2, nwhisker_points ); /* calculate the 2nd derivatives of y at spline points */
  
  for (x_val = min_x;x_val <= max_x; x_val++) {

    status = splint( x, y, y2, nwhisker_points, (float) x_val, &y_val );
    if (status != 1) 
      mexErrMsgTxt("bad spline");
    (*yy)[x_val] = round ( y_val );  
  }

  free_vector( x, 1,nwhisker_points);
  free_vector( y, 1,nwhisker_points);
  free_vector( y2, 1,nwhisker_points);

}

double xi_linear_interp(double r)
{
  static int flag=0,prev_cosmo=0;
  static double *x,*y,*y2;
  int n=100,i;
  double a,rlo=0.1,rhi=150,dlogr,klo;
  double xi_linear_int();

  if(!flag || RESET_COSMOLOGY!=prev_cosmo)
    {
      if(!flag)
	{
	  x=dvector(1,n);
	  y=dvector(1,n);
	  y2=dvector(1,n);
	}
      flag=1;

      dlogr = (log(rhi)-log(rlo))/(n-1);
      klo = 0;
      if(BOX_SIZE>0)klo = 1/BOX_SIZE;
      for(i=1;i<=n;++i)
	{
	  r_g4 = x[i] = exp((i-1)*dlogr)*rlo;
	  y[i] = qromo(xi_linear_int,klo,1.0/r_g4,midpnt)+
	    qromo(xi_linear_int,1.0/r_g4,1.0E+3,midpnt);
	}
      check_for_smoothness(x,y,n,0);
      spline(x,y,n,2.0E+30,2.0E+30,y2);
      prev_cosmo=RESET_COSMOLOGY;
    }

  splint(x,y,y2,n,r,&a);
  return(a);
}

STCalEnum::InterpolationType CalibrationManager::stringToInterpolationEnum(const string &s)
{
  String itype(s);
  itype.upcase();
  const Char *c = itype.c_str();
  String::size_type len = itype.size();
  Regex nearest("^NEAREST(NEIGHBOR)?$");
  Regex linear("^LINEAR$");
  Regex spline("^(C(UBIC)?)?SPLINE$");
  Regex poly("^POLY(NOMIAL)?$");
  if (nearest.match(c, len) != String::npos) {
    return STCalEnum::NearestInterpolation;
  }
  else if (linear.match(c, len) != String::npos) {
    return STCalEnum::LinearInterpolation;
  }
  else if (spline.match(c, len) != String::npos) {
    return STCalEnum::CubicSplineInterpolation;
  }
  else if (poly.match(c, len) != String::npos) {
    return STCalEnum::PolynomialInterpolation;
  }

  os_.origin(LogOrigin("CalibrationManager","stringToInterpolationEnum",WHERE));
  os_ << LogIO::WARN << "Interpolation type " << s << " is not available. Use default interpolation method." << LogIO::POST;
  return STCalEnum::DefaultInterpolation;
}

/**
 * parameter derivative of spline function with 5 nodes
 *
 * @param id argument index for differentiation
 * @param t point at which the spline should be evaluated
 * @param t1 location of node 1
 * @param p1 spline value at node 1
 * @param t2 location of node 2
 * @param p2 spline value at node 2
 * @param t3 location of node 3
 * @param p3 spline value at node 3
 * @param t4 location of node 4
 * @param p4 spline value at node 4
 * @param t5 location of node 5
 * @param p5 spline value at node 5
 * @param ss flag indicating whether slope at first node should be user defined
 * @param dudt user defined slope at first node
 *
 * @return dspline(t)dp(id)
 *
 */
double Dspline5(int id, double t, double t1, double p1, double t2, double p2, double t3, double p3, double t4, double p4, double t5, double p5, int ss, double dudt) {
    double uout;
    
    double ts[5];
    double us[5];
    
    double b[5];
    double c[5];
    double d[5];
    int did;
    
    ts[0] = t1;
    ts[1] = t2;
    ts[2] = t3;
    ts[3] = t4;
    ts[4] = t5;
    
    us[0] = 0.0;
    us[1] = 0.0;
    us[2] = 0.0;
    us[3] = 0.0;
    us[4] = 0.0;
    
    did = floor(id/2-1);
    us[did] = 1.0;
    
    spline(5, ss, 0, dudt, 0.0, ts, us, b, c, d);
    uout = seval(5, t, ts, us, b, c, d);
    
    return(uout);
}

double Dspline5(double t, double t1, double p1, double t2, double p2, double t3, double p3, double t4, double p4, double t5, double p5, int ss, double dudt, int id) {   
    double uout;
    
    double ts[5];
    double us[5];
    
    double b[5];
    double c[5];
    double d[5];
    
    ts[0] = t1;
    ts[1] = t2;
    ts[2] = t3;
    ts[3] = t4;
    ts[4] = t5;
    
    us[0] = 0.0;
    us[1] = 0.0;
    us[2] = 0.0;
    us[3] = 0.0;
    us[4] = 0.0;
    
    us[id-1] = 1.0;
    
    spline(5, ss, 0, dudt, 0.0, ts, us, b, c, d);
    uout = seval(5, t, ts, us, b, c, d);
    
    return(uout);
}

double spline_pos5(double t, double t1, double p1, double t2, double p2, double t3, double p3, double t4, double p4, double t5, double p5, int ss, double dudt) {   
    int is;
    double uout;
    
    double ts[5];
    double us[5];
    double uslog[5];
    
    double b[5];
    double c[5];
    double d[5];
    
    ts[0] = t1;
    ts[1] = t2;
    ts[2] = t3;
    ts[3] = t4;
    ts[4] = t5;
    
    us[0] = p1;
    us[1] = p2;
    us[2] = p3;
    us[3] = p4;
    us[4] = p5;
    
    for (is = 0; is<5; is++){
        uslog[is] = log(us[is]);
    }
    
    spline(5, ss, 0, dudt, 0.0, ts, uslog, b, c, d);
    uout = seval(5, t, ts, uslog, b, c, d);
    
    return(exp(uout));
}

double one_halo_from_file(double m)
{
  static double *x, *y, *z;
  static int n, flag = 1;
  FILE *fp;
  int i;
  double a;
  
  if(flag)
    {
      muh(0);
      fp = openfile("ncen.dat");
      muh(0);
      n = filesize(fp);
      x = dvector(1,n);
      y = dvector(1,n);
      z = dvector(1,n);
      for(i=1;i<=n;++i)
	fscanf(fp,"%lf %lf",&x[i],&y[i]);
      spline(x,y,n,1.0E+30,1.0E+30,z);
      flag = 0;
      muh(0);
    }
  if(log10(m)<x[1])return 0;
  splint(x,y,z,n,log10(m),&a);
  //printf("%e %e\n",m,pow(10.0,a));
  if(a>0) return 1;
  return pow(10.0,a);

}

/* This function tabulates the one-halo real-space term for spline interpolation.
 * If the requested radius is out-of-bounds of the tabulated function, a value of
 * zero is returned.
 */
double one_halo_real_space(double r)
{
  static int flag=0;
  static double *x,*y,*y2;
  int i,n=100;
  double a;

  if(!HOD.pdfs)return(0);

  if(!flag || RESET_FLAG_1H)
    {
      if(!flag)
	{
	  x=dvector(1,n);
	  y=dvector(1,n);
	  y2=dvector(1,n);
	}
      flag=1;
      RESET_FLAG_1H=0;
      calc_real_space_one_halo(x,y,n);
      spline(x,y,n,2.0E+30,2.0E+30,y2);
    }
  if(r>x[n])return(0);
  if(r<x[1])return(0);
  splint(x,y,y2,n,r,&a);
  return(a);

}

/**We treat the x variable as a bar and cast it to an integer to round down. After 
 * ensuring the bar number is valid we sum the components multiplied by a scaling
 * factor chosen from the provided parameters.
 *
 * \return The value of function with the specified bar number (x) and parameters p.
 */
double PeakFit::operator() (double *x, double *p) {
	//Compute the integer value of x by rounding
	unsigned int bar = x[0] + 0.5;
	if (bar >= components_.size()) return 0;

	double retVal = 0;
#ifdef USESPLINE
	double yVal[components_.size()];
	double xVal[components_.size()];
	for (bar = 0; bar < components_.size(); bar++) {
		xVal[bar] = bar;
		retVal = 0;
#endif

		for (unsigned int state=0; state < components_[bar].size(); state++) {
			retVal += fabs(p[state]) * components_[bar][state];
		}

#ifdef USESPLINE
		yVal[bar] = retVal;
	}

	TSpline3 spline("spline",xVal,yVal,components_.size());
	retVal = spline.Eval(x[0]);
#endif

	return retVal;
}

double spline_pos3(double t, double t1, double p1, double t2, double p2, double t3, double p3, int ss, double dudt) {   
    int is;
    double uout;
    
    double ts[3];
    double us[3];
    double uslog[3];
    
    double b[3];
    double c[3];
    double d[3];
    
    ts[0] = t1;
    ts[1] = t2;
    ts[2] = t3;
    
    us[0] = p1;
    us[1] = p2;
    us[2] = p3;
    
    for (is = 0; is<3; is++){
        uslog[is] = log(us[is]);
    }
    
    spline(3, ss, 0, dudt, 0.0, ts, uslog, b, c, d);
    uout = seval(3, t, ts, uslog, b, c, d);
    
    return(exp(uout));
}

void homologation6(int color) {
    unsigned int index = getMotionInstructionIndex();

    switch (index) {
        case 1:
            takeBullion1(color);
            break;
        case 2:
			// first bottle
			spline(color, X_BOTTLE, 0x0280, ANGLE_180, 0xEC, 0xC0, MOTION_SPEED_FACTOR_NORMAL, MOTION_SPEED_FACTOR_NORMAL);
            break;
		case 3: // Goto near 2 bottle
			setSonarStatus(0); // TODO
			bottle1ToFrontBottle2(color);
            break;
		case 4: // hit bottle 2
			setSonarStatus(0);
			frontBottle2ToBottle2(color);
            break;
		case 5:
			bottle2TakeCD(color);
			break;
		case 6:
			takeCDToDropZone1(color);
			break;
		case 7:
			cleanLintel1First(color);
			break;
		case 8:
			cleanLintel1Second(color);
			break;
	}
}

double spline4(double t, double t1, double p1, double t2, double p2, double t3, double p3, double t4, double p4, int ss, double dudt) {   
    double uout;
    
    double ts[4];
    double us[4];
    
    double b[4];
    double c[4];
    double d[4];
    
    ts[0] = t1;
    ts[1] = t2;
    ts[2] = t3;
    ts[3] = t4;
    
    us[0] = p1;
    us[1] = p2;
    us[2] = p3;
    us[3] = p4;
    
    spline(4, ss, 0, dudt, 0.0, ts, us, b, c, d);
    uout = seval(4, t, ts, us, b, c, d);
    
    return(uout);
}

double Dspline3(double t, double t1, double p1, double t2, double p2, double t3, double p3, int ss, double dudt, int id) {   
    double uout;
    
    double ts[3];
    double us[3];
    
    double b[3];
    double c[3];
    double d[3];
    
    ts[0] = t1;
    ts[1] = t2;
    ts[2] = t3;
    
    us[0] = 0.0;
    us[1] = 0.0;
    us[2] = 0.0;
    
    us[id-1] = 1.0;
    
    spline(3, ss, 0, dudt, 0.0, ts, us, b, c, d);
    uout = seval(3, t, ts, us, b, c, d);
    
    return(uout);
}