#include <string.h>
#include <limits.h>
#include <float.h>
#include "cddefines.h"
#include "varypar.h"
#include "func.h"
#include "date.h"
#include "phymir.h"
#ifdef __unix
#define _INCLUDE_POSIX_SOURCE
#include <stddef.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <unistd.h>
#endif
#define	F0	1.4142136f
#define	F1	0.7071068f
#define	F2	0.1f

/* The optimization algorithm Phymir and its subsidiary routines have been
 * written by Peter van Hoof. An article describing the algorithm is
 * currently in preparation and is intended to be submitted to New Astronomy.
 */
void phymir(float xc[], 
  float del[], 
  long int nvarPhymir, 
  float *ymin, 
  float toler)
{
#define DELTA(i,j)  (((i) == (j)) ? 1.f : 0.f)

	char chDum1[STDLEN],
	  chDum2[STDLEN],
	  chDum3[STDLEN];
	int lgFinish, 
	  lgNegd2, 
	  lgNewCnt, 
	  lgRead;
#	ifdef __unix
	int res,
	  stat;
	char fnam1[20],
	  fnam2[20];
#	endif
	long int i, 
	  imax, 
	  j, 
	  j1, 
	  jmin=0,
	  limcp,
	  nvarcp,
	  usedCPU;
	float a2[LIMPAR][LIMPAR], 
	  amax, 
	  c1[LIMPAR], 
	  c2[LIMPAR], 
	  d1, 
	  d2, 
	  diff, 
	  dmax=0.f, 
	  dold=0.f, 
	  r1, 
	  r2, 
	  sgn, 
	  temp,
	  vers,
	  vpusedPhymir, 
	  xcold[LIMPAR], 
	  xhlp[LIMPAR], 
	  xnrm, 
	  xp[LIMPAR][2*LIMPAR + 1], 
	  yp[2*LIMPAR + 1];
#	ifdef __unix
	pid_t pid;
#	endif

#	ifdef DEBUG_FUN
	fputs( "<+>phymir()\n", debug_fp );
#	endif

	if( nvarPhymir > LIMPAR )
	{
		fprintf( ioQQQ, "phymir: too many parameters are varied, increase LIMPAR\n" );
		puts( "[Stop]" );
		exit(1);
	}

	VaryPar.nOptimiz = 0;
	usedCPU = 0;
	lgFinish = FALSE;
	lgRead = VaryPar.lgOptCont;
	if( VaryPar.lgOptCont )
		goto L_20;

	/* initialize hypercube dimensions and center */
	dmax = 0.f;
	for( i=0; i < nvarPhymir; i++ )
	{
		temp = (float)fabs(del[i]);
		dmax = MAX2(dmax,temp);
	}

	dold = dmax;
	for( i=0; i < nvarPhymir; i++ )
	{
		xcold[i] = xc[i] + 10.f*toler;
		c1[i] = (float)fabs(del[i])/dmax;
		c2[i] = c1[i];
		xp[i][0] = xc[i];
		VaryPar.vparm[0][i] = xc[i];
		/* this is done by child process and is lost, so copy code to parent process */
		if( VaryPar.lgParallel )
		{
			vpusedPhymir = MIN2(VaryPar.vparm[0][i],VaryPar.varang[i][1]);
			vpusedPhymir = MAX2(vpusedPhymir,VaryPar.varang[i][0]);
			VaryPar.varmax[i] = MAX2(VaryPar.varmax[i],vpusedPhymir);
			VaryPar.varmin[i] = MIN2(VaryPar.varmin[i],vpusedPhymir);
		}
	}
#	ifdef __unix
	if( VaryPar.lgParallel )
	{
		/* flush all open files */
		fflush(NULL);
		pid = fork();
		if( pid == 0 )
		{
			/* this is child process so execute */
			sprintf(fnam1,"chi2_%ld",0L);
			sprintf(fnam2,"output_%ld",0L);
			/* each child should have its own output file */
			ioQQQ = fopen(fnam2,"w");
			/* fail-safe in case func crashes */
			yp[0] = FLT_MAX;
			wr_block(&yp[0],(size_t)sizeof(float),fnam1);
			yp[0] = (float)func(xc);
			wr_block(&yp[0],(size_t)sizeof(float),fnam1);
			exit(0);
		}
		else
		{
			/* parent process waits, this way all initializations remain intact */
			pid = wait(&stat);
			sprintf(fnam1,"chi2_%ld",0L);
			sprintf(fnam2,"output_%ld",0L);
			rd_block(&yp[0],(size_t)sizeof(float),fnam1);
			res = remove(fnam1);
			cpfile(fnam2);
			res = remove(fnam2);
		}
		/* this was incremented by child process and is lost, so copy code here */
		VaryPar.nOptimiz++;
	}
	else
	{
		yp[0] = (float)func(xc);
	}
#	else
	/* on non-UNIX machines only sequential mode is supported */
	yp[0] = (float)func(xc);
#	endif
	*ymin = yp[0];
	jmin = 0;
	/* restart entry; initialize transformation matrix to unity */
L_10:
	for( i=0; i < nvarPhymir; i++ )
	{
		for( j=0; j < nvarPhymir; j++ )
		{
			a2[j][i] = DELTA(i,j);
		}
	}
	/* loop entry */
L_20:
	if( lgRead )
	{
		rd_continue(&vers,&limcp,a2,c1,c2,xc,xcold,&dmax,&dold,ymin,&nvarcp,&VaryPar.nOptimiz,
			    VaryPar.varmax,VaryPar.varmin,chDum1,chDum2,chDum3,CNTFILE);
		if( nvarcp != nvarPhymir )
		{
			printf( "optimize continue - wrong number of free parameters, sorry\n" );
			puts( "[Stop]" );
			exit(1);
		}
		for( i=0; i < nvarPhymir; i++ )
		{
			xp[i][0] = xc[i];
		}
		yp[0] = *ymin;
		jmin = 0;
		lgRead = FALSE;
	}
	else
	{
		strcpy(chDum1,date.chDate);
		strcpy(chDum2,date.chVersion);
		strcpy(chDum3,VaryPar.HostName);
		wr_continue(VRSNEW,LIMPAR,a2,c1,c2,xc,xcold,dmax,dold,*ymin,nvarPhymir,VaryPar.nOptimiz,
			    VaryPar.varmax,VaryPar.varmin,chDum1,chDum2,chDum3,CNTFILE);
	}

	if( lgFinish )
	{ 
#		ifdef DEBUG_FUN
		fputs( " <->phymir()\n", debug_fp );
#		endif
		return;
	}
	/*       maximum no. of iterations exceeded?  */
	if( VaryPar.nOptimiz >= VaryPar.nIterOptim )
	{
		fprintf( ioQQQ, " Optimizer exceeding maximum iterations.\n This can be reset with the OPTIMIZE ITERATIONS command.\n" );
#		ifdef DEBUG_FUN
		fputs( " <->phymir()\n", debug_fp );
#		endif
		return;
	}

	for( j=0; j < nvarPhymir; j++ )
	{
		sgn = 1.f;
		for( j1=2*j+1; j1 <= 2*j+2; j1++ )
		{
			sgn = -sgn;
			for( i=0; i < nvarPhymir; i++ )
			{
				xp[i][j1] = xc[i] + sgn*dmax*c2[j]*a2[j][i];
				xhlp[i] = xp[i][j1];
				VaryPar.vparm[0][i] = xhlp[i];
				/* this is done by child process, so copy code to parent process */
				if( VaryPar.lgParallel )
				{
					vpusedPhymir = MIN2(VaryPar.vparm[0][i],VaryPar.varang[i][1]);
					vpusedPhymir = MAX2(vpusedPhymir,VaryPar.varang[i][0]);
					VaryPar.varmax[i] = MAX2(VaryPar.varmax[i],vpusedPhymir);
					VaryPar.varmin[i] = MIN2(VaryPar.varmin[i],vpusedPhymir);
				}
			}
#			ifdef __unix
			if( VaryPar.lgParallel )
			{
				usedCPU++;
				if( usedCPU > VaryPar.maxCPU )
				{
					/* too many processes, wait for one to finish */
					pid = wait(&stat);
					usedCPU--;
				}
				/* flush all open files */
				fflush(NULL);
				pid = fork();
				if( pid == 0 )
				{
					/* this is child process so execute */
					sprintf(fnam1,"chi2_%ld",j1);
					sprintf(fnam2,"output_%ld",j1);
					/* each child should have its own output file */
					ioQQQ = fopen(fnam2,"w");
					/* fail-safe in case func crashes */
					yp[j1] = FLT_MAX;
					wr_block(&yp[j1],(size_t)sizeof(float),fnam1);
					yp[j1] = (float)func(xhlp);
					wr_block(&yp[j1],(size_t)sizeof(float),fnam1);
					exit(0);
				}
				/* this was incremented by child process, so copy code here */
				VaryPar.nOptimiz++;
			}
			else
			{
				yp[j1] = (float)func(xhlp);
			}
#			else
			/* on non-UNIX machines only sequential mode is supported */
			yp[j1] = (float)func(xhlp);
#			endif
		}
	}
#	ifdef __unix
	/* wait for child processes to terminate */
	if( VaryPar.lgParallel )
	{
		while( usedCPU > 0 )
		{
			pid = wait(&stat);
			usedCPU--;
		}
	}
#	endif
	/* find best model */
	for( j1=1; j1 <= 2*nvarPhymir; j1++ )
	{
#		ifdef __unix
		if( VaryPar.lgParallel )
		{
			sprintf(fnam1,"chi2_%ld",j1);
			sprintf(fnam2,"output_%ld",j1);
			rd_block(&yp[j1],(size_t)sizeof(float),fnam1);
			res = remove(fnam1);
			cpfile(fnam2);
			res = remove(fnam2);
		}
#		endif
		if( yp[j1] < *ymin )
		{
			*ymin = yp[j1];
			jmin = j1;
		}
	}

	lgNewCnt = jmin > 0;
	/* determine minimum and relative uncertainties */
	lgNegd2 = FALSE;
	xnrm = 0.f;
	for( i=0; i < nvarPhymir; i++ )
	{
		d1 = yp[2*i+2] - yp[2*i+1];
		d2 = 0.5f*yp[2*i+2] - yp[0] + 0.5f*yp[2*i+1];
		if( d2 <= 0.f )
			lgNegd2 = TRUE;
		xhlp[i] = -dmax*c2[i]*(MAX2(MIN2((0.25f*d1)/MAX2(d2,1.e-10f),F0),-F0) - DELTA(2*i+1,jmin) + DELTA(2*i+2,jmin));
		xnrm += POW2(xhlp[i]);
	}
	xnrm = (float)sqrt(xnrm);
	/* set up new transformation matrix */
	imax = 0;
	amax = 0.f;
	for( j=0; j < nvarPhymir; j++ )
	{
		for( i=0; i < nvarPhymir; i++ )
		{
			if( xnrm > 0.f )
			{
				if( j == 0 )
				{
					a2[0][i] *= xhlp[0];
				}
				else
				{
					a2[0][i] += xhlp[j]*a2[j][i];
					a2[j][i] = DELTA(i,j);
					if( j == nvarPhymir-1 && (float)fabs(a2[0][i]) > amax )
					{
						imax = i;
						amax = (float)fabs(a2[0][i]);
					}
				}
			}
			else
			{
				a2[j][i] = DELTA(i,j);
			}
		}
	}
	/*       this is to assure maximum linear independence of the base vectors */
	if( imax > 0 )
	{
		a2[imax][0] = 1.f;
		a2[imax][imax] = 0.f;
	}
	/* apply Gram-Schmidt procedure to get orthogonal base */
	phygrm(a2,nvarPhymir);

	/* set up hypercube dimensions in new base and choose new center */
	for( i=0; i < nvarPhymir; i++ )
	{
		c2[i] = 0.f;
		for( j=0; j < nvarPhymir; j++ )
		{
			c2[i] += POW2(a2[i][j]/c1[j]);
		}
		c2[i] = 1.f/(float)sqrt(c2[i]);
		xc[i] = xp[i][jmin];
		xp[i][0] = xc[i];
	}
	yp[0] = yp[jmin];
	jmin = 0;
	/*       choose size of next hypercube */
	if( lgNegd2 )
	{
		/* this means that the hypercube either is bigger than the scale
		 * on which the function varies, or is so small that we see noise.
		 * in both cases make the hypercube smaller. */
		r1 = F1;
		r2 = F1;
	}
	else
	{
		r1 = F2;
		if( lgNewCnt )
		{
			/* when we make progress, dmax may become bigger */
			r2 = (float)sqrt(1.f/F1);
		}
		else
		{
			/* when there is no progress force dmax smaller, otherwise there
			 * may never be an end */
			r2 = (float)sqrt(F1);
		}
	}
	dmax = MIN2(MAX2(xnrm/c2[0],dmax*r1),dmax*r2);
	/* fail-safe against excessive behaviour */
	dmax = MIN2(dmax,dold);

	if( dmax > toler )
		goto L_20;
	/*       do we restart? */
	diff = 0.f;
	for( i=0; i < nvarPhymir; i++ )
	{
		diff += POW2(xc[i] - xcold[i]);
		xcold[i] = xc[i];
		c2[i] = c1[i];
	}
	diff = (float)sqrt(diff);
	dmax = dold;
	lgFinish = diff <= toler;
	goto L_10;
#	undef DELTA
}

void cpfile(const char *fnam)
{
	char chr;

	FILE *fdes;

#	ifdef DEBUG_FUN
	fputs( "<+>cpfile()\n", debug_fp );
#	endif

	/* append output produced by child process on <fnam>, to output of parent process */

	if( (fdes = fopen(fnam,"r")) == 0 )
		return;
	chr = (char)fgetc(fdes);
	while( ! feof(fdes) )
	{
		fputc( chr , ioQQQ );
		chr = (char)fgetc(fdes);
	}
	fclose(fdes);
	
#	ifdef DEBUG_FUN
	fputs( " <->cpfile()\n", debug_fp );
#	endif
	return;
}

void phygrm(float a[LIMPAR][LIMPAR], 
  long int n)
{
	long int i, 
	  j, 
	  k;
	float ip; 

#	ifdef DEBUG_FUN
	fputs( "<+>phygrm()\n", debug_fp );
#	endif

	/* apply modified Gram-Schmidt procedure to a */
	for( i=0; i < n; i++ )
	{
		ip = 0.f;
		for( k=0; k < n; k++ )
			ip += POW2(a[i][k]);
		ip = (float)sqrt(ip);
		for( k=0; k < n; k++ )
			a[i][k] /= ip;
		for( j=i+1; j < n; j++ )
		{
			ip = 0.f;
			for( k=0; k < n; k++ )
				ip += a[i][k]*a[j][k];
			for( k=0; k < n; k++ )
				a[j][k] -= ip*a[i][k];
		}
	}
	
#	ifdef DEBUG_FUN
	fputs( " <->phygrm()\n", debug_fp );
#	endif
	return;
}

void wr_block(void *ptr,
  size_t len,
  const char *fnam)
{
	FILE *fdes;

#	ifdef DEBUG_FUN
	fputs( "<+>wr_block()\n", debug_fp );
#	endif

	/* write <len> bytes of data from buffer <*ptr> into unformatted file <fnam> */

	if( (fdes = fopen(fnam,"wb")) == 0 ) {
		printf( "error opening file: %s\n",fnam );
		puts( "[Stop]" );
		exit(1);
	}
	if( fwrite(ptr,len,(size_t)1,fdes) != 1 ) {
		printf( "error writing on file: %s\n",fnam );
		fclose(fdes);
		puts( "[Stop]" );
		exit(1);
	}
	fclose(fdes);
	
#	ifdef DEBUG_FUN
	fputs( " <->wr_block()\n", debug_fp );
#	endif
	return;
}

void rd_block(void *ptr,
  size_t len,
  const char *fnam)
{
	FILE *fdes;

#	ifdef DEBUG_FUN
	fputs( "<+>rd_block()\n", debug_fp );
#	endif

	/* read <len> bytes of data into buffer <*ptr> from unformatted file <fnam> */

	if( (fdes = fopen(fnam,"rb")) == 0 ) {
		printf( "error opening file: %s\n",fnam );
		puts( "[Stop]" );
		exit(1);
	}
	if( fread(ptr,len,(size_t)1,fdes) != 1 ) {
		printf( "error reading on file: %s\n",fnam );
		fclose(fdes);
		puts( "[Stop]" );
		exit(1);
	}
	fclose(fdes);
	
#	ifdef DEBUG_FUN
	fputs( " <->rd_block()\n", debug_fp );
#	endif
	return;
}

void wr_continue(float vers,
  long dim,
  float a2[LIMPAR][LIMPAR],
  const float c1[LIMPAR],
  const float c2[LIMPAR],
  const float xc[LIMPAR],
  const float xcold[LIMPAR],
  float dmax,
  float dold,
  float ymin,
  long nvar,
  long noptim,
  const float varmax[LIMPAR],
  const float varmin[LIMPAR],
  const char chDum1[STDLEN],
  const char chDum2[STDLEN],
  const char chDum3[STDLEN],
  const char *fnam)
{
	int lgErr,
	  res;
	size_t num;
	FILE *fdes;

#	ifdef DEBUG_FUN
	fputs( "<+>wr_continue()\n", debug_fp );
#	endif

	if( (fdes = fopen(fnam,"wb")) == 0 ) {
		printf( "error opening file: %s\n",fnam );
		return;
	}
	lgErr = FALSE;
	num = 1;
	lgErr = lgErr || ( fwrite(&vers,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fwrite(&dim,sizeof(long),num,fdes) != num );
	num = LIMPAR*LIMPAR;
	lgErr = lgErr || ( fwrite(a2,sizeof(float),num,fdes) != num );
	num = LIMPAR;
	lgErr = lgErr || ( fwrite(c1,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fwrite(c2,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fwrite(xc,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fwrite(xcold,sizeof(float),num,fdes) != num );
	num = 1;
	lgErr = lgErr || ( fwrite(&dmax,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fwrite(&dold,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fwrite(&ymin,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fwrite(&nvar,sizeof(long),num,fdes) != num );
	lgErr = lgErr || ( fwrite(&noptim,sizeof(long),num,fdes) != num );
	num = LIMPAR;
	lgErr = lgErr || ( fwrite(varmax,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fwrite(varmin,sizeof(float),num,fdes) != num );
	num = STDLEN;
	lgErr = lgErr || ( fwrite(chDum1,sizeof(char),num,fdes) != num );
	lgErr = lgErr || ( fwrite(chDum2,sizeof(char),num,fdes) != num );
	lgErr = lgErr || ( fwrite(chDum3,sizeof(char),num,fdes) != num );
	fclose(fdes);
	if( lgErr )
	{
		printf( "error writing file: %s\n",fnam );
		res = remove(fnam);
	}
	
#	ifdef DEBUG_FUN
	fputs( " <->wr_continue()\n", debug_fp );
#	endif
	return;
}

void rd_continue(float *vers,
  long *dim,
  float a2[LIMPAR][LIMPAR],
  float c1[LIMPAR],
  float c2[LIMPAR],
  float xc[LIMPAR],
  float xcold[LIMPAR],
  float *dmax,
  float *dold,
  float *ymin,
  long *nvar,
  long *noptim,
  float varmax[LIMPAR],
  float varmin[LIMPAR],
  char chDum1[STDLEN],
  char chDum2[STDLEN],
  char chDum3[STDLEN],
  const char *fnam)
{
	int lgErr;
	size_t num;
	FILE *fdes;

#	ifdef DEBUG_FUN
	fputs( "<+>rd_continue()\n", debug_fp );
#	endif

	if( (fdes = fopen(fnam,"rb")) == 0 ) {
		printf( "error opening file: %s\n",fnam );
		puts( "[Stop]" );
		exit(1);
	}
	lgErr = FALSE;
	num = 1;
	lgErr = lgErr || ( fread(vers,sizeof(float),num,fdes) != num );
	if( lgErr || *vers != VRSNEW )
	{
		printf( "optimize continue - file has incompatible version, sorry\n" );
		puts( "[Stop]" );
		exit(1);
	}
	lgErr = lgErr || ( fread(dim,sizeof(long),num,fdes) != num );
	if( lgErr || *dim != LIMPAR )
	{
		printf( "optimize continue - arrays have wrong dimension, sorry\n" );
		puts( "[Stop]" );
		exit(1);
	}
	num = LIMPAR*LIMPAR;
	lgErr = lgErr || ( fread(a2,sizeof(float),num,fdes) != num );
	num = LIMPAR;
	lgErr = lgErr || ( fread(c1,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fread(c2,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fread(xc,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fread(xcold,sizeof(float),num,fdes) != num );
	num = 1;
	lgErr = lgErr || ( fread(dmax,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fread(dold,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fread(ymin,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fread(nvar,sizeof(long),num,fdes) != num );
	lgErr = lgErr || ( fread(noptim,sizeof(long),num,fdes) != num );
	num = LIMPAR;
	lgErr = lgErr || ( fread(varmax,sizeof(float),num,fdes) != num );
	lgErr = lgErr || ( fread(varmin,sizeof(float),num,fdes) != num );
	num = STDLEN;
	lgErr = lgErr || ( fread(chDum1,sizeof(char),num,fdes) != num );
	lgErr = lgErr || ( fread(chDum2,sizeof(char),num,fdes) != num );
	lgErr = lgErr || ( fread(chDum3,sizeof(char),num,fdes) != num );
	fclose(fdes);
	if( lgErr )
	{
		printf( "error reading file: %s\n",fnam );
		puts( "[Stop]" );
		exit(1);
	}
	
#	ifdef DEBUG_FUN
	fputs( " <->rd_continue()\n", debug_fp );
#	endif
	return;
}