# include "data.h"

# define FTOL 0.001
# define ITMAX 200

void linmin (float *p, float *xi, int n, float *fret, float (*func)());

/*** not well tested yet !! ***/

void powell (float *p, int Npar, float (func)() ) {
  
  int i, ibig, j, Niter;
  float t, fptt, fp, del;
  float *pt, *ptt, *xit, **xi;
  float ftemp;

  ALLOCATE (pt, float, Npar);
  ALLOCATE (ptt, float, Npar);

  ALLOCATE (xi, float *, Npar);
  for (i = 0; i < Npar; i++) {
    ALLOCATE (xi[i], float, Npar);
    bzero (xi[i], Npar * sizeof (float));
    xi[i][i] = 1;
  }
  ALLOCATE (xit, float, Npar);
  
  ftemp = func (p);

  for (j = 0; j < Npar; j++) pt[j] = p[j];

  for (Niter = 0; Niter < ITMAX; Niter++) {
    fp = ftemp;
    ibig = 0;
    del = 0.0;

    for (i = 0; i < Npar; i++) {
      for (j = 0; j < Npar; j++) xit[j] = xi[j][i];

      fptt = ftemp;
      linmin (p, xit, Npar, &ftemp, func);

      if (fabs (fptt - ftemp) > del) {
	del = fabs (fptt - ftemp);
	ibig = i;
      }
    }

    if (fabs (fp - ftemp) <= 0.5*FTOL*(fabs(fp) + fabs(ftemp))) {
      free (xit);
      free (ptt);
      free (pt);
      for (i = 0; i < Npar; i++) free (xi[i]);
      free (xi);
      return;
    }

    for (j = 0; j < Npar; j++) {
      ptt[j] = 2.0*p[j] - pt[j];
      xit[j] = p[j] - pt[j];
      pt[j]  = p[j];
    }

    fptt = func (ptt);

    if (fptt < fp) {
      t = 2.0*(fp - 2.0*ftemp + fptt) * SQ(fp - ftemp - del) - del * SQ(fp - fptt);
      if (t < 0.0) {
	linmin (p, xit, Npar, &ftemp, func);
	for (j = 0; j < Npar; j++) xi[j][ibig] = xit[j];
      }
    }
  }

  if (Niter == ITMAX) {
    gprint (GP_ERR, "Too many iterations in routine POWELL\n");
    return;
  }

}

#undef ITMAX

/* 
   function has N parameters
   p is the initial guess for the N parameters 
   xi are the N unit vectors - x[i][j] = delta(i,j)
   Npar = N parameters
   
   FTOL - ending tolerance
*/
   
#define TOL 2.0e-4

int ncom=0;	/* defining declarations */
float *pcom=0,*xicom=0,(*nrfunc)();

void linmin (float *p, float *xi, int n, float *fret, float (*func)()) {

	int j;
	float xx,xmin,fx,fb,fa,bx,ax;
	float brent(),f1dim(),*vector();
	void mnbrak(),free_vector();

	ncom=n;
	pcom=vector(1,n);
	xicom=vector(1,n);
	nrfunc=func;
	for (j=1;j<=n;j++) {
		pcom[j]=p[j];
		xicom[j]=xi[j];
	}
	ax=0.0;
	xx=1.0;
	bx=2.0;
	mnbrak(&ax,&xx,&bx,&fa,&fx,&fb,f1dim);
	*fret=brent(ax,xx,bx,f1dim,TOL,&xmin);
	for (j=1;j<=n;j++) {
		xi[j] *= xmin;
		p[j] += xi[j];
	}
	free_vector(xicom,1,n);
	free_vector(pcom,1,n);
}

#undef TOL
