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


struct cc_s	{
	void *c;
	int n, nn;
	int *map, *imap;
	C double *prm;
	double *p;
	double (*funk)(C double *p, int n, void *c, int nc);
	};

LOCAL double ff(C double *p, int n, void *cc, int nc)
{
 int i;
 struct cc_s *ccc = (struct cc_s *)cc;
 assert(ccc->n >= n);
 for(i=0;i<ccc->n;i++)	{
	if(ccc->map[i] == -1)
		ccc->p[i] = ccc->prm[i];
	else ccc->p[i] = p[ccc->map[i]];
	}
 return (*ccc->funk)(ccc->p, ccc->n, ccc->c, nc);
}

LOCAL double **amoeba_fix(double **p,
				int n,
				double ftol,
				double (*funk)(C double *p, int n, void *c, int nc),
				int itmax,
				void *c,
				int nc,
				double T0, double Tfac,
				double **(meeb)(double **, int, double,
								double (*)(C double *, int, void*, int),
								int, void*, int),
				double **(meebA)(double **, int, double,
								double (*)(C double *, int, void*, int),
								int, void*, int, double, double)
				)
{
 int *map = new int [n+1];
 int *imap = new int[n+1];
 double *prm = new double [n];
 int i, nn;
 for(nn=0,i=0;i<n;i++)	{
	int j;
	BOOL isfree = FALSE;
	for(j=1;j<=n;j++)
		isfree += (p[j][i] != p[0][i]);
	if(isfree)
		{ prm[i] = signaling_nan(0); map[i] = nn; imap[nn] = i; nn++;}
	else	{ prm[i] = p[0][i]; map[i] = -1; imap[nn] = -1;}
	}
 map[nn] = n;
 imap[n] = nn;

 double **pp = double2alloc(nn+1, nn);
 assert(pp);
 for(i=0;i<nn;i++)	{
	assert(imap[i] >= 0);
	int j;
	for(j=0;j<=nn;j++)
		pp[imap[j]][i] = p[imap[j]][imap[i]];
	}
 struct cc_s cc;
 cc.c = c; cc.map = map; cc.imap = imap; cc.prm = prm;
 cc.nn = nn; cc.funk = funk; cc.n = n;
 cc.p = new double [n];	/* Just temp storage space to avoid excess mallocs */
 assert(cc.p);

 double **tmp = 0;
 if(meeb)
 	tmp = (*meeb)(pp, nn, ftol, ff, itmax, &cc, nc);
 else if(meebA)
 	tmp = (*meebA)(pp, nn, ftol, ff, itmax, &cc, nc, T0, Tfac);
 delete [] cc.p;
 /* Put parameters back in the array from whence they came. */
 for(i=0;i<nn;i++)	{
	int j;
	for(j=0;j<=nn;j++)
		p[map[j]][i] = pp[map[j]][i];
	}
 delete [] prm;
 delete [] map;
 delete [] imap;
 double2free(pp);
 return p;
}

double **amoeba_dirset(double **p,
				int n,
				double ftol,
				double (*funk)(C double *p, int n, void *c, int nc),
				int itmax,
				void *c,
				int nc)
{
 return amoeba_fix(p, n, ftol, funk, itmax, c, nc,
						0.0, 0.0,
						amoeba_dirset_allfree, 0);
}

double **amoeba(double **p,
				int n,
				double ftol,
				double (*funk)(C double *p, int n, void *c, int nc),
				int itmax,
				void *c,
				int nc)
{
 return amoeba_fix(p, n, ftol, funk, itmax, c, nc,
							0.0, 0.0,
							amoeba_allfree, 0);
}

double **amoeba_anneal(double **p,
				int n,
				double ftol,
				double (*funk)(C double *p, int n, void *c, int nc),
				int itmax,
				void *c,
				int nc, double T0, double Tfac)
{
 return amoeba_fix(p, n, ftol, funk, itmax, c, nc,
							T0, Tfac,
							0,
							amoeba_anneal_allfree);
}
