#define VERSION 1.12
#define float double

int findroot(float (*f[MAX])(), float x[MAX]);
#define NUM_OF_STEPS 2000
#define INCREMENT_X 1.
#define TOLERANCE 1.e-5
#define PRECISION 1.e-3
#define X_BOX 10000
#define FAST_F 2
#define FAST_X .5
#define DEBUG 0

int eqsolv(float (*f[MAX])(), float x[MAX]);
int solve(float c[MAX][MAX], float b[MAX], float x[MAX]);
float df(int i, float x[MAX], float (*f)());
#define INCREMENT 1e-5
void version(void);


int findroot(float (*f[MAX])(), float x[MAX])
{
  int i,j;
  int error, error_t, key, flag, tot_steps;
  float f_i[MAX], f_f[MAX], fi, ff, step;
  float x_t[MAX], delta_x[MAX], delta_t;

  version(), error=0, tot_steps=0;
  flag=0;

  if(DEBUG){ printf("\nFindRoot: Initial values given:\n");
  for(i=0;i<MAX;i++)
    printf("x%i=%g\t\tf%i=%g\n", i, x[i], i, (*f[i])(x));
  }

  for(i=0;i<MAX;i++) x_t[i]=x[i], f_i[i]=(*f[i])(x);
  for(i=0;i<NUM_OF_STEPS;i++){

    for(j=0;j<2*MAX+1;j++){
      error=eqsolv(f, x_t);
      if(!error) break;
      if(j==2*MAX) return 3;
      if(j%2) x_t[j/2]=x[j/2]=x[j/2]+2*INCREMENT_X;
      if(!(j%2)){
	if(j/2!=0) x_t[j/2-1]=x[j/2-1]=x[j/2-1]-INCREMENT_X;
	x_t[j/2]=x[j/2]=x[j/2]-INCREMENT_X;
      }
    }
    tot_steps++;

    if(DEBUG){ printf("\nFindRoot: step number %i\n\
number of corrections to the initial point: %i\n", tot_steps, j);
    for(j=0;j<MAX;j++)
      printf("x%i=%g\t\tf%i=%g\n", j, x_t[j], j, (*f[j])(x_t));
    }

    if(error) for(j=0;j<MAX;j++) f_i[j]=(*f[j])(x);

    delta_t=0;
    for(j=0;j<MAX;j++){
      delta_x[j]=x_t[j]-x[j];
      if(fabs(delta_x[j])>delta_t) delta_t=fabs(delta_x[j]);
    }
    if(delta_t<PRECISION){
      for(j=0;j<MAX;j++) x[j]=x_t[j];
      return 0;
    }


    step=1, error=0;
    if(delta_t>X_BOX){
      step=X_BOX/delta_t, error=1;
      for(j=0;j<MAX;j++) x_t[j]=x[j]+step*delta_x[j];
      for(j=0;j<MAX;j++) f_f[j]=(*f[j])(x_t);
    }

    if(DEBUG){
      if(error){
	printf("step reduction occured: step = %g\n", step);
	for(j=0;j<MAX;j++)
	  printf("x%i=%g\t\tf%i=%g\n", j, x_t[j], j, (*f[j])(x_t));
      }
    }


    flag=0;
    for(j=0;j<MAX;j++){
      f_f[j]=(*f[j])(x_t);
      if(fabs(f_f[j])>TOLERANCE) flag=1;
    }
    if(!flag) return 1;

    key=0, error=error_t=-1;
    while(1){
      key++;
      if(!(error||error_t)) break;
      error_t=error;
      error=0;
      switch(2-key%2){
      case 1:
	do{
	  flag=0, ff=1;
	  for(j=0;j<MAX;j++) if((f_f[j]*f_i[j])<0){
	    fi=f_i[j]/(f_i[j]-f_f[j]);
	    if(fi<ff) ff=fi, flag=1;
	  }
	  step*=ff;
	  if(flag){
	    error=1;
	    for(j=0;j<MAX;j++) x_t[j]=x[j]+step*delta_x[j];
	    for(j=0;j<MAX;j++) f_f[j]=(*f[j])(x_t);
	  }
	}while(flag);
	break;

      case 2:
	do{
	  flag=0;
	  for(j=0;j<MAX;j++){
	    if(f_i[j]>0)
	      if(f_f[j]>(1-(1-FAST_F)*step)*f_i[j]){
		flag=1;
		break;
	      }
	    if(f_i[j]<0)
	      if(f_f[j]<(1-(1-FAST_F)*step)*f_i[j]){
		flag=1;
		break;
	      }
	  }
	  if(flag){
	    error=2;
	    step=FAST_X*step;
	    for(j=0;j<MAX;j++) x_t[j]=x[j]+step*delta_x[j];
	    for(j=0;j<MAX;j++) f_f[j]=(*f[j])(x_t);
	  }
	}while(flag);
	break;

      default:
      }

      if(DEBUG){
	printf("error is %i, substep %i, step is %g\n", error, key, step);
	if(error) for(j=0;j<MAX;j++)
	  printf("x%i=%g\t\tf%i=%g\n", j, x_t[j], j, (*f[j])(x_t));
      }

    }
    for(j=0;j<MAX;j++) x[j]=x_t[j], f_i[j]=f_f[j];
  }
  return 2;
}

  /*
    This subroutine is called FindRoot. It implements precision control
    and avoids some traps in the calculation. All you need to do is to
    define an array of pointers to functions which are to be set to zero,
    and an array of starting values of unknowns. It has several define
    options most of which are transparent. If you define DEBUG 1, it will
    print out an error log which indicates which special case is encounted
    and how it's being treated. FindRoot returns a value, which is zero,
    if the solution has been found to the specified precision, one, if the
    solution has been found, but the function being evaluated got smaller
    than TOLERANCE before reaching the required precision of the argument.
    You may set TOLERANCE to 0, to rid of this one. You will get two, if
    the required accuracy has not been achieved in NUM_OF_STEPS cycles,
    even though there no outstanding errors. If the solution cannot be
    found for these starting values of unknowns (even though it varies
    this value - see define INCREMENT_X 1.) you will get 3.
  */

int eqsolv(float (*f[MAX])(), float x[MAX])
{
  int i,j;
  float c[MAX][MAX], b[MAX];

  version();

  /*
    This function solves system of equations f[i](x[j])=0.
    Each call gets better approximation. You have to initialize
    array of unknown variables x[j], though.
  */

  for(i=0;i<MAX;i++){
    b[i]=-(*f[i])(x);
    for (j=0;j<MAX;j++) b[i]+=df(j, x, (*f[i]))*x[j];
  }

  for(i=0;i<MAX;i++) for(j=0;j<MAX;j++) c[i][j]=df(j, x, f[i]);

  return solve(c, b, x);
}


int solve(float c[MAX][MAX], float b[MAX], float x[MAX])
{
  int i,j,n,k;
  int flag1, flag2, error;
  float div;

  version();

  /*
    This is the actual calculation - by Gauss' method. You need to
    supply the matrix and the colomn of your linear equation. Another
    parameter (x) gives the location at which the roots are stored.
  */

  error=0;
  for(j=0;j<MAX;j++){
    flag1=0, flag2=0;
    for(i=0;i<MAX;i++) if(c[i][j]!=0){ flag1=1;
      for(k=0;k<j;k++) if(c[i][k]!=0){flag2=1; break;}
      if(flag2) flag1=0,flag2=0;
      if(flag1) break;
    }

    if(!flag1){ error++; continue;}
    div=c[i][j];
    for(k=j;k<MAX;k++) c[i][k]=c[i][k]/div;
    b[i]=b[i]/div;

    for(n=0;n<MAX;n++){
      if(n!=i){
        div=c[n][j];
        for(k=j;k<MAX;k++) c[n][k]-=c[i][k]*div;
        b[n]-=b[i]*div;
      }
    }
  }

  /*
    This is where the found roots are written in their order
  */

  if(!error) for(i=0;i<MAX;i++) for(j=0;j<MAX;j++) if(c[i][j]!=0) x[j]=b[i];

  return error;
}


float df(int i, float x[MAX], float (*f)())
{
  int j;
  static float inc=INCREMENT;
  float x1[MAX], x2[MAX];

  /*
    Finding derivative of a function f - no comment
  */

  for(j=0;j<MAX;j++){
    if(j==i) x1[j]=x[j]-inc, x2[j]=x[j]+inc;
    else x1[j]=x[j], x2[j]=x[j];
  }
  return ((*f)(x2)-(*f)(x1))/(2*inc);
}


void version(void)
{
  static int calls=0;
  if(!calls)  printf("FindRoot v%g\n",VERSION), calls=1;
}