#include <gpk_std.h>
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <assert.h>
#include <numerics.h>
#ifdef __SunOS5
# include <sunmath.h>
#endif

static C int num_for_decr = 4;

extern "C" int finite(double);

inline double xx(double x) {return x*x;}
inline double ssqrt(double x) { return x>=0 ? sqrt(x) : -sqrt(-x);}

inline double max(double a, double b)	{ return a>b  ? a : b;}
LOCAL double max(double a, double b, double c)
{ return max(a, max(b, c)); }

#define D if(1)

LOCAL void rotate_towards(double *a, double *b, double theta, int n)
{
 int i;
 C double c = cos(theta);
 C double s = sin(theta);
 for(i=0;i<n;i++)	{
	C double tmp1 = a[i]*c + b[i]*s;
	C double tmp2 = b[i]*c - a[i]*s;
	a[i] = tmp1;
	b[i] = tmp2;
	}
}

LOCAL void scalevec(double *to, double scale, int n)
{
 int i;
 for(i=0;i<n;i++)
	to[i] *= scale;
}

LOCAL void copya(double *to, C double *from, int n)
{
 int i;
 for(i=0;i<n;i++)
	to[i] = from[i];
}

LOCAL void TryStepDir(double *to, C double *from, C double *dir,
									double fac, int n)
{
 int i;
 for(i=0;i<n;i++)
	to[i] = from[i] + dir[i]*fac;
}

LOCAL void Subtract(double *to, C double *from1, C double *from2, int n)
{
 int i;
 for(i=0;i<n;i++)
	to[i] = from1[i] - from2[i];
}

LOCAL void update_average_direction(double *pn, C double *pchg, int n, double f)
{
 int i2;
 for(i2=0; i2<n; i2++)	{	
	pn[i2] = pn[i2]*(1-f) + pchg[i2]*f;
	}
}


LOCAL BOOL orthogonalize(double *pdi, C double *p_last, int n)
{
 double dot, pl;
 int i;
 for(pl=dot=0.0,i=0;i<n;i++)	{
	dot += pdi[i]*p_last[i];
	pl += xx(p_last[i]);
	}
 if(!(pl>0))
	return FALSE;

 C double fac = dot/pl;
 double pd0, pd;
 for(pd0=pd=0.0,i=0;i<n;i++)	{	/* Subtract out part of pdi in
											direction of p_last. */
	pd0 += xx(pdi[i]);
	pdi[i] -= fac*p_last[i];
	pd += xx(pdi[i]);
	}

 D fprintf(stderr, "#Fac for orthogonalization=%g = %g/%g length_ratio=%g\n", fac,
							dot, pl, sqrt(pd/pd0));

 if(pd < 0.001*pd0)	{
	D fprintf(stderr, "# loss of orthogonality: pd=%g, pd0=%g\n", pd, pd0);
	return FALSE;
	}
 return TRUE;
}


LOCAL int find_any_improved(int n, double *pr, double *pc, double *pdi,
					double (*funk)(C double *, int, void *, int),
					void *c, int nc, double *pxfbest, double *pybest,
					double pylast)
{
 int mreps = 1;
 double ytmp_f, ytmp_b, xf = 1.5, xf_b;

 while(TRUE)	{
	assert(finite(xf));
	TryStepDir(pr, pc, pdi, xf, n);
	ytmp_f = (*funk)(pr, n, c, nc);
	if(ytmp_f<=*pybest && ytmp_f<=pylast)	{
		D fprintf(stderr, "improvement to %g(%g) at %g\n", ytmp_f, *pybest, xf);
		*pybest = ytmp_f;
		*pxfbest = xf;
		return mreps;
		}
	xf_b = -0.5*xf;
	TryStepDir(pr, pc, pdi, xf_b, n);
	ytmp_b = (*funk)(pr, n, c, nc);
	if(ytmp_b<=*pybest && ytmp_b<ytmp_f)	{
		D fprintf(stderr, "improvement to %g(%g) at %g\n", ytmp_b, *pybest, xf_b);
		*pybest = ytmp_b;
		*pxfbest = xf_b;
		return mreps+1;
		}
	else if(ytmp_f <= *pybest)	{
		D fprintf(stderr, "improvement to %g(%g) at %g\n", ytmp_f, *pybest, xf);
		*pybest = ytmp_f;
		*pxfbest = xf;
		return mreps;
		}

	if(mreps++ >= 3)
		break;

	xf *= 0.25;
	}
 D fprintf(stderr, "No improvement, but use %g(%g) at %g\n", ytmp_f,
						*pybest, xf);
 if(ytmp_f < ytmp_b)	{
	*pybest = ytmp_f;
	*pxfbest = xf;
	}
 else	{
	*pybest = ytmp_b;
	*pxfbest = xf_b;
	}

 return 0;
}


LOCAL int try_to_extend(int n, double *pr, double *pc, double *pdi,
					double (*funk)(C double *, int, void *, int),
					void *c, int nc, double *pxfbest, double *pybest,
					double yaccept)
{
 int mreps = 0;
 double xf = *pxfbest * 1.3;
 BOOL found = FALSE, repeat;
 double y_real_best = *pybest;

 do	{
	repeat = FALSE;
	TryStepDir(pr, pc, pdi, xf, n);
	double ytmp = (*funk)(pr, n, c, nc);
	if(ytmp <= 0.5*(y_real_best + yaccept))	{
		D fprintf(stderr, "Extending to %g(%g) at %g\n", ytmp, *pybest, xf);
		if(ytmp <= y_real_best)	
			y_real_best = ytmp;

		*pybest = ytmp;
		*pxfbest = xf;
		found = TRUE;
		xf *= 1.414;
		}

	} while(repeat && mreps++<5);

 return found ? mreps : 0;
}

LOCAL double lbi(C double *d, int n, int but)
{
 static int q = 0;
 int idx = -1;
 int i, ibut;
 C int rootn = (int)(sqrt(n) + 0.5);
 double worst = infinity();
 for(ibut=i=0;i<n && (ibut<rootn || idx==(-1)) ;i++)	{		/* find a bad finite one */
	C int ii = (i+q)%n;
	if(ii != but)	{
		ibut++;
		if(d[ii] < worst)	{
			idx = ii;
			worst = d[ii];
			}
		}
	}

 if(idx < 0)		/* Find from among the NaNs and infinities */
 	for(i=0;i<n;i++)	{
		idx = (i+q)%n;
		if(!finite(d[idx]))
			break;
		}

 q = (idx+1)%n;
 return idx;
}		

LOCAL double biggest_but(C double *d, int n, int but)
{
 double rv = 0.0;
 int i;
 for(i=0;i<n;i++)
	if(i!=but && d[i]>rv)
		rv = d[i];
 return rv;
}		

LOCAL double typ_decr(C double *d, int n)
{
 double rv = 0.0;
 int i;
 for(i=0;i<n;i++)
	rv += d[i];
 return rv/n;
}		

void deal_with_bad_direction(double **p, int worst, int n)
{
 static int iter = 0;
 int j;
 for(j=1;j<n;j++)
	if(n>1 && !orthogonalize(p[worst], p[(worst+n-j)%n], n))	{
			D fprintf(stderr, "Crevice orthogonalization fails between %d and %d\n", worst, (worst+n-j)%n);
			break;
			}
 if(j<n)	{
	int i;
	/* Desperation manouver: */
	for(i=0;i<n;i++)
		p[worst][i] = p[(worst+n+i-1)%n][i] * (i==worst ? 1 : 0.8);
	D fprintf(stderr, "# can't completely orthogonalize: j=%d\n", j);
	/* Do a random rotation (partial interchange) of two directions: */
	rotate_towards(p[worst], p[(worst+iter)%n], 0.3, n);
	}
 iter++;
}

double **amoeba_dirset_allfree(double **p,
				int n,
				double ftol,
				double (*funk)(C double *p, int n, void *c, int nc),
				int itmax,
				void *c,
				int nc)
{
 int i, ilo;
 double *pc, *decr, *plast, *pr;
 double yc, yc_last, yc_global_min;

 assert(n > 0);
 pc = (double *)malloc(sizeof(*pc) * (n+1));
 assert(pc);
 plast = (double *)malloc(sizeof(*plast) * n);
 assert(plast);
 pr = (double *)malloc(sizeof(*pr) * n);
 assert(pr);
 decr = (double *)malloc(sizeof(*decr) * (n+1));
 assert(decr);
 {
 int n_finite;
 double *y = (double *)malloc(sizeof(double) * (n+1));
 assert(y);
 for(n_finite=i=0;i<=n && n_finite<num_for_decr;i++)	{
 	y[i] = (*funk)(p[i], n, c, nc);
	if(finite(y[i]))
		n_finite++;
	}
 for(;i<=n;i++)
	y[i] = quiet_nan(0);

 yc_last = infinity();
 yc = infinity();
 for(ilo=-1,i=0;i<=n;i++)	{
	if(y[i] < yc)	{ ilo = i; yc_last = yc; yc = y[i]; }
	else if(y[i] < yc_last)
		yc_last = y[i];
	}
 yc_global_min = yc;
 assert(finite(yc) && ilo>=0);
 {
 double typdecr;
 for(n_finite=0,typdecr=0.0,i=0;i<n;i++)	{
	if(finite(y[i]))	{
		decr[i] = y[i]-yc;
		typdecr += sqrt(y[i]-yc);
		n_finite++;
		}
	/* We do a square of average of root to emphasize typical small changes
	at the expense of one huge difference. */
	D fprintf(stderr, "#y[%d]=%g, yc=%g, n=%d, typdecr=%g\n",
						i, y[i], yc, n, typdecr);
	}
 assert(n_finite > 0);
 typdecr = xx(typdecr/n_finite);
 for(i=0;i<n;i++)
	if(!finite(y[i]))
		decr[i] = typdecr;
 D fprintf(stderr, "#Typdecr=%g, initially\n", typdecr);
 }

 /* First we set up the center and direction set: */
 for(i=0;i<n;i++)	{
	assert(finite(p[ilo][i]));
	pc[i] = p[ilo][i];
	}
 int ii, iii;
 for(iii=ii=0;ii<n+1;ii++)
	if(ii != ilo)	{
		Subtract(p[iii], pc, p[ii], n);
		iii++;
		}
 for(i=0;i<n;i++)
	p[n][i] = 0.0;
 free((char *)y);
 }
 FILE *afp;
 D afp = fopen("amoeba.tmp", "w");

 copya(plast, pc, n);

 int iter = 0, asincelast, sincelast, rejects=0, pn_count=0;
 int quit = 0;
 unsigned di = 0;
 double Estore = 0;
 for(asincelast=sincelast=iter=0;iter<itmax;iter++)	{
	double yaccept = max(yc_global_min+Estore+ftol, yc, 0.5*(yc_last+yc));
	double ybest = yaccept;
	double xfbest = quiet_nan(0);
	int rtmp;
	int worst = -1;
	/* Search for minimum; first finding any improved point: */

	if((rtmp=find_any_improved(n, pr, pc, p[di],
								funk, c, nc, &xfbest, &ybest, yc)))	{
		if(rtmp <= 2)
			try_to_extend(n, pr, pc, p[di], funk, c, nc, &xfbest, &ybest, yaccept);

			/* xfbest is now an estimate of the minimum position in the direction,
						and ybest is the function value. */
		assert(finite(xfbest));

		int i1;
		assert(finite(ybest));
		TryStepDir(pc, pc, p[di], xfbest, n);		/* we accept the move - move pc[]. */
		char pbchar = ' ';
					/* This is an unusually good direction: */
		if(di!=n && sincelast>1 && (yc-ybest)>biggest_but(decr, n, di)
				|| di!=n && sincelast>0 && asincelast>=n
					&& (yc-ybest)>decr[(di+n-1)%n])	{
			assert(sincelast > 0);

			D fprintf(afp, "# ");
			/* Direction of last few steps will now be used as a search
				direction: */
			Subtract(p[di], pc, plast, n);
			scalevec(p[di], n, 1.0/sqrt(n));
			update_average_direction(p[n], p[di], n, 1.0/n);
			pn_count++;
			asincelast = sincelast = 0;
			copya(plast, pc, n);
			pbchar = '!';
			worst = lbi(decr, n, di);
			}
		else	{
			scalevec(p[di], ssqrt(xfbest), n);
			sincelast++;
			worst = -1;
			}
		D fprintf(stderr, "#Accept%c, di=%dp%d, yc= %g -> %g -> %g//%g xf=%g\n",
								pbchar, di, worst, yaccept, yc, ybest, yc-biggest_but(decr, n, di), xfbest);
		decr[di] = yc>ybest ? yc-ybest : 0;
		yc_last = yc;
		yc = ybest;
		if(ybest < yc_global_min)	{
			Estore += 0.5*(yc_global_min - ybest);
			yc_global_min = ybest;
			}
		Estore *= exp(-1.0/n);
		rejects = 0;
		}
	else	{
		decr[di] = 0;
		worst = di;
		
		scalevec(p[di], ssqrt(xfbest), n);
		pn_count = 0;
		D fprintf(stderr, "#Reject: di=%d.\n", di);
		}

	if(worst>=0 && di!=n)	{
		/* orthogonalize this failure against all others: */
		deal_with_bad_direction(p, worst, n);
		decr[worst] = quiet_nan(0);
		}
	asincelast++;
	di++;
	if(pn_count>1 ? di>n : di>=n)
		di=0;
	if((typ_decr(decr, n)*n*n<ftol || rejects>2*n) && quit++>n)
		break;
	}
 D fclose(afp);
 {
  int ii;
  for(ii=n;ii>0;ii--)
	TryStepDir(p[ii], pc, p[ii-1], 1.0, n);
 copya(p[0], pc, n);
 }
 free((char *)plast);
 free((char *)pr);
 free((char *)pc);
 free((char *)decr);
 return p;
}

