#include <math.h>
#include "handy.h"

int ncommon;
double *pcommon,*xicommon,(*obj_func)(double []);

void bracket_min(double *ax, double *bx, double *cx, double *fa, double *fb, double *fc,
	double (*func)(double))
{
  double ulim,u,r,q,fu,dum;
  *fa=(*func)(*ax);
  *fb=(*func)(*bx);
  if (*fb > *fa) {
    shift(dum,*ax,*bx,dum)
    shift(dum,*fb,*fa,dum)
  }
  *cx=(*bx)+1.618034*(*bx-*ax);
  *fc=(*func)(*cx);
  while (*fb > *fc) {
    r=(*bx-*ax)*(*fb-*fc);
    q=(*bx-*cx)*(*fb-*fa);
    u=(*bx)-((*bx-*cx)*q-(*bx-*ax)*r)/(2.0*sign(max(fabs(q-r),1.0e-20),q-r));
    ulim=(*bx)+100.0*(*cx-*bx);
    if ((*bx-u)*(u-*cx) > 0.0) {
      fu=(*func)(u);
      if (fu < *fc) {
        *ax=(*bx);
        *bx=u;
        *fa=(*fb);
        *fb=fu;
        return;
      }
      else if (fu > *fb) {
        *cx=u;
        *fc=fu;
        return;
      }
      u=(*cx)+1.618034*(*cx-*bx);
      fu=(*func)(u);
    }
    else if ((*cx-u)*(u-ulim) > 0.0) {
      fu=(*func)(u);
      if (fu < *fc) {
        shift(*bx,*cx,u,*cx+1.618034*(*cx-*bx))
        shift(*fb,*fc,fu,(*func)(u))
      }
    }
    else if ((u-ulim)*(ulim-*cx) >= 0.0) {
      u=ulim;
      fu=(*func)(u);
    }
    else {
      u=(*cx)+1.618034*(*cx-*bx);
      fu=(*func)(u);
    }
    shift(*ax,*bx,*cx,u)
    shift(*fa,*fb,*fc,fu)
  }
}


double projection(double x) {
  int j;
  double f,*xt;
  xt=(double *)malloc(ncommon*sizeof(double));
  for (j=0;j<ncommon;j++) xt[j]=pcommon[j]+x*xicommon[j];
  f=(*obj_func)(xt);
  free(xt);
  return f;
}


double parab_intrp(double ax, double bx, double cx, double (*f)(double), double tol,
  double *xmin) {
  int iter;
  double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm;
  double e=0.0;
  a=(ax < cx ? ax : cx);
  b=(ax > cx ? ax : cx);
  x=w=v=bx;
  fw=fv=fx=(*f)(x);
  for (iter=1;iter<=100;iter++) {
    xm=0.5*(a+b);
    tol2=2.0*(tol1=tol*fabs(x)+1.0e-10);
    if (fabs(x-xm) <= (tol2-0.5*(b-a))) {
      *xmin=x;
      return fx;
    }
    if (fabs(e) > tol1) {
      r=(x-w)*(fx-fv);
      q=(x-v)*(fx-fw);
      p=(x-v)*q-(x-w)*r;
      q=2.0*(q-r);
      if (q > 0.0) p = -p;
      q=fabs(q);
      etemp=e;
      e=d;
      if (fabs(p) >= fabs(0.5*q*etemp) || p <= q*(a-x) || p >= q*(b-x))
				d=0.3819660*(e=(x >= xm ? a-x : b-x));
      else {
        d=p/q;
	u=x+d;
	if (u-a < tol2 || b-u < tol2) d=sign(tol1,xm-x);
      }
    }
    else d=0.3819660*(e=(x >= xm ? a-x : b-x));
    u=(fabs(d) >= tol1 ? x+d : x+sign(tol1,d));
    fu=(*f)(u);
    if (fu <= fx) {
      if (u >= x) a=x; else b=x;
      shift(v,w,x,u)
      shift(fv,fw,fx,fu)
    }
    else {
      if (u < x) a=u; else b=u;
      if (fu <= fw || w == x) {
        v=w;
        w=u;
        fv=fw;
        fw=fu;
      }
      else if (fu <= fv || v == x || v == w) {
        v=u;
        fv=fu;
      }
    }
  }
  printf("Too many iterations in parab_intrp");
  *xmin=x;
  return fx;
}


void minimize_1d(double p[], double xi[], int n, double *fret, double (*func)(double [])) {
  int j;
  double xx,xmin,fx,fb,fa,bx,ax;
  ncommon=n;
  pcommon=(double *)malloc(n*sizeof(double));
  xicommon=(double *)malloc(n*sizeof(double));
  obj_func=func;
  for (j=0;j<n;j++) {
    pcommon[j]=p[j];
    xicommon[j]=xi[j];
  }
  ax=0.0;
  xx=1.0;
  bracket_min(&ax,&xx,&bx,&fa,&fx,&fb,projection);
  *fret=parab_intrp(ax,xx,bx,projection,2.0e-4,&xmin);
  for (j=0;j<n;j++) {
    xi[j] *= xmin;
    p[j] += xi[j];
  }
  free(xicommon);
  free(pcommon);
}


void conjgrad(double p[], int n, double ftol, int *iter, double *fret,
	double (*func)(double []), void (*dfunc)(double [], double [])) {
  int j,its;
  double gg,gam,fp,dgg;
  double *g,*h,*xi;
  g=(double *)malloc(n*sizeof(double));
  h=(double *)malloc(n*sizeof(double));
  xi=(double *)malloc(n*sizeof(double));
  fp=(*func)(p);
  (*dfunc)(p,xi);
  for (j=0;j<n;j++) {
    g[j] = -xi[j];
    xi[j]=h[j]=g[j];
  }
  for (its=1;its<=100000;its++) {
    *iter=its;
    minimize_1d(p,xi,n,fret,func);
    if (2.0*fabs(*fret-fp) <= ftol*(fabs(*fret)+fabs(fp)+1.0e-10)) {
      free(xi);
      free(h);
      free(g);
      return;
    }
    fp=(*func)(p);
    (*dfunc)(p,xi);
    dgg=gg=0.0;
    for (j=0;j<n;j++) {
      gg += g[j]*g[j];
      dgg += (xi[j]+g[j])*xi[j];
    }
    if (gg == 0.0) {
      free(xi);
      free(h);
      free(g);
      return;
    }
    gam=dgg/gg;
    for (j=0;j<n;j++) {
      g[j] = -xi[j];
      xi[j]=h[j]=g[j]+gam*h[j];
    }
  }
  printf("Too many iterations in conjgrad.");
}
