/*	GaussFit - A System for Least Squares and Robust Estimation

	Source Code Copyright (C) 1987 by William H. Jefferys,
	Michael J. Fitzpatrick and Barbara E. McArthur
	All Rights Reserved.
*/

/* main loop */

#include <math.h>
#include <stdio.h>
#include "array.h"
#include "datum.h"
#include "simpledefs.h"
#include "symboltable.h"
#include "defines.h"
#include "house.h"
#include "files.h"
#include "alloc.h"
#ifdef THINK_C
#include <Memory.h>
#endif
#include "prototypes.p"

extern FILE *fp;  /* result file piinter */
/*int totsize, totfree, totrealloc;*/
int running = 0;

double tolerance;  /* tolerance */
double fit;
double envhuber, envtukey, envfair, envtrim;
int envminsum, envirls, envorm, envprec;
double lambda, factor;
int MARQ = 0;
int maxiters;
int space;
int limit;
int tracedisplay = 0;
double *OldPrm = NULL;

double UpdateValues()  /* update all parameters */
{
	int i;
	int type;
	double dahat;
	char *name;
	short index[5];
	short symInx, fvInx;
	int indsz,prmsz;
	double error;
	extern double DeltaV;
	double ahat;

    /* cycle through all paramters */
	error = DeltaV; /* error starts with value returned by phi calculation */
	i = 0;

	indsz = getindexsz();
	prmsz = getmaxprmname();
	/* get data on next parameter */
	while(GetDeltaValue(i,&name,index,&type,&dahat))
	{                    
		i++;
		symInx = findsymbol(name, 0);
		fvInx = FileVarOf(symInx);
		if (filevarptr[fvInx].xvars[4]>0) {
			ahat = getxparval(symInx,index);/* get current value of paramter */
			/* increment value and update paramter file */
			putxparval(symInx,index,ahat + dahat);
		} else {
			ahat = getparamval(symInx); /*get current value of parameter */
			putparamval(symInx,ahat + dahat); /* increment value and update parameter file */
		}
		/* print results in results file and to screen */
		if ((index[4] == 0) && (indsz == 0)) {
			printf("%*s = ",prmsz,name);
			fprintf(fp,"%*s = ",prmsz,name);
		}else if ((index[4] == 0) && (indsz != 0)) {
			printf("%*s%*s = ",prmsz,name,indsz," ");
			fprintf(fp,"%*s = ",prmsz+indsz,name);
		} else {
			printf("%*s",prmsz, name);
			prIndex(stdout,index,-indsz);
			printf(" = ");
			fprintf(fp,"%*s", prmsz,name);
			prIndex(fp,index,-indsz);
			fprintf(fp, " = ");
		}
		printdouble(stdout,ahat+dahat,0);
		printf(" delta = ");
		printdouble(stdout,dahat,0);
		printf("\n");

		printdouble(fp,ahat+dahat,0);
		fprintf(fp," delta = ");
		printdouble(fp,dahat,0);
		fprintf(fp,"\n");

		/* update convergence error */
		error = maxval(fabs(dahat)/(tolerance +fabs(ahat)),error);
	}
	i++;  /* ????? */
	printf("\n");
	fprintf(fp,"\n");
	return error;
}

gaussmain(t,s)  /* main loop routine */
char *t,*s;
{
	int i;
	static double  oldSigma,Curr_Sigma;

	printcopyright(stdout);
	
	getenvvars(s);

	mycompile(t); /* compile file "t" */
		
	oldSigma = -1.0;
	space = 0;

	for(i=0;i<maxiters;i++) /* iterate at most maxite times */
	{
			/* inform user of progress */
		printf("\nStart of Iteration %d...\n\n",i+1);

		/* initialize Householder routines */
/*		totsize = 0;
		totrealloc = 0;
		totfree = 0;*/
		InitHouse("results", getenvint("prmat"), 10.0);

		fprintf(fp,"\nStart of Iteration %d...\n\n",i+1);

		Forming_Eqns(); /* form the equations of condition */

		/*if double  iteration method  or Marq thnn proceed with second */
		if ((getenvval("double")) || (MARQ))
		{
			InitHouse("results", getenvint("prmat"), 10.0);
			Forming_Eqns(); /* form the equations of condition */
		}

		if (MARQ)
		{
			if (!space)
				Allocate_Param_Space();
			
			Curr_Sigma=Get_Sigma();


			if (oldSigma < 0.00 || Curr_Sigma < oldSigma)
			{
				oldSigma = Curr_Sigma;
				Current_into_Old_Params();
				SolveMatrix(); /* solve the estimation problem */
				ShowResults(i+1);/* show results and update parameters */
				lambda = lambda * factor;
			}
			else
			{
				Old_into_Current_Params();
				lambda = lambda/(sqr(factor));
				oldSigma = -1.0;
			}
			fprintf(fp,"lambda = %28.17lf\n",lambda);
			printold();
		}
		else
		{
			SolveMatrix(); /* solve the estimation problem */
			ShowResults(i+1);/* show results and update parameters */
		}
/*		printf("TOTALS:  allocated: %d, reallocated %d, free %d\n",totsize,totrealloc,totfree); */
		if(fit<tolerance)   /* quit if converged */
			break;
	}
	if(tolerance < fit)   /* if reached iters without convergence */
	{
		itlimit(stdout);
		itlimit(fp);
	}
	if (getenvval("prvar") && (fit < tolerance))
		CovarianceMtx();

	ChiSquare();
	epilog();  /* clean up */
}

Forming_Eqns()
{
		printf("\nForming Eqs...\n\n");
		/* define global variables */
		setpc2label("_Globals");
		running = 1;
		interpret();
		running = 0;
		/* execute user's model starting at main */
		setpc2label("main");
		running = 1;
		interpret();
		running = 0;
		/*popval();*/ /* pop top of stack */
		ResetDeclareStack(); /* discard all defined variables */
		SumIt(); /* sum sigmas */
}

ShowResults(count) 
int count;
{

	PrintResults(); /* print results */
	fit = UpdateValues(); /* update the paramters */
	/* print information for user */
	printf("fit =\t");
	fprintf(fp,"fit =\t");
	printdouble(fp,fit,0);
	printdouble(stdout,fit,0);
	printf(", tol =\t");
	fprintf(fp,", tol =\t");
	printdouble(stdout,tolerance,0);
	printdouble(fp,tolerance,0);
	printf("\n");
	fprintf(fp,"\n");
	printf("\nEnd of Iteration %d...\n\n",count);
	fprintf(fp,"\nEnd of Iteration %d...\n\n",count);
	if (getenvint("prmwrt") != -1)
		writexpar();
}

itlimit(fpp)
FILE *fpp;
{
	fprintf(fpp,"\n***********************************");
	fprintf(fpp,"***********************************\n");
	fprintf(fpp,"\nIteration limit reached.  Convergence has not occurred.\n");
	fprintf(fpp,"\n***********************************");
	fprintf(fpp,"***********************************\n\n");
}

prolog()
{
#ifdef THINK_C
	SetApplLimit((char*)CurStackBase - 48000L);
	MaxApplZone(); /* MAC requires large heap for space allocation */
#endif
}

getenvvars(s) /* get values from environment file and test them */
char *s;
{
	openenvtable(s); /* open environment table "s" */
	tolerance = getenvval("tol"); /* get tolerance from environment */
	if ((tolerance < 1.0e-13)   && (tolerance > -1.0e-13))   
		tolerance = 0.000001;
	envprec = getenvint("prec");
	envfair = getenvval("fair");
	envirls = getenvint("irls");
	envorm  = getenvint("orm");
	envtrim = getenvval("trim");
	envhuber  = getenvval("huber");
	envtukey  = getenvval("tukey");
	envminsum = getenvint("minsum");
	if (envminsum != 0 && (notzero(envfair)))
		fatalerror("Both minsum and fair have been set to non-zero in the environment file.\n"," ");
	lambda = getenvval("lambda");
	factor = getenvval("factor");
	tracedisplay = getenvint("trace");
	if (notzero(lambda))
		MARQ = 1.0;
	if ((notzero(envminsum)) && (MARQ))
		fatalerror("Both minsum and lambda have been set to non-zero in the environment file.\nLevenburg-Marquardt cannot be used with minsum.\n"," ");
	if (((lambda < .0000000000001)||(lambda > 100.0)) && (MARQ)) 
	{
		lambda = .0001;
		warningerror("Keyword lambda missing or less than or equal to zero in the environment file.\n"," ",0,0);
	}
	if (((factor < .0000000000001)||(factor >100.0)) && (MARQ)) 
	{
		factor = 0.1;
		warningerror("Keyword factor missing or less than or equal to zero in the environment file.\n"," ",0,0);
	}
	maxiters = getenvint("iters"); /* get max iterations from environment */
	if ((maxiters < 1) || (maxiters >1000))
	{
		maxiters = 10;
		warningerror("Keyword iters missing or less than zero in environment file.\n"," ",0,0);
	}
#ifdef NOTYET
	if (getenvint("derivs") > 1) {
		do2ndDerivs = 1;
		doObservations = 1;
	}
#endif
}


Allocate_Param_Space()
{
	int i;

	limit = Get_LastCol();
	space++;
	OldPrm = (double*)MemAlloc("OldPrm",(long)(limit+1)*sizeof(double));
	for (i=0;i<=limit;i++)
	{
		*(OldPrm+i) = 0.0;
	}
}

Old_into_Current_Params()
{
	int i, type;
	double dahat,ahat;
	char *name;
	short index[5];
	short symInx, fvInx;


 	i = 0;   /* cycle through all paramters */
	fprintf(fp,"\n");
 	while(GetDeltaParams(i,&name,index,&type,&dahat)) /* get data on next parame ter */
 	{
		 i++;
		symInx = findsymbol(name, 0);
		fvInx = FileVarOf(symInx);
		if (type != RightHandType) {
			if (filevarptr[fvInx].xvars[4]>0) {
 				ahat = getxparval(symInx,index);/* get current value of paramter */
 				putxparval(symInx,index,OldPrm[i-1]);/* increment value and update paramter file */
			} else {
 				ahat = getparamval(symInx); /*get current value of parameter */
 				putparamval(symInx,OldPrm[i-1]);
 			}
 		}
	}
	fprintf(fp,"\n");
}


Current_into_Old_Params()
{
	int i, type;
	double dahat;
	char *name;
	short index[5];
	short symInx, fvInx;

	i = 0;   /* cycle through all paramters */
	fprintf(fp,"\n");
	while(GetDeltaParams(i,&name,index,&type,&dahat)) /* get data on next parameter */
	{                    
		i++;
		symInx = findsymbol(name, 0);
		fvInx = FileVarOf(symInx);
		if (type != RightHandType) {
			if (filevarptr[fvInx].xvars[4]>0) {
				OldPrm[i-1] = getxparval(symInx,index);/* get current value of paramter */
			} else {
				OldPrm[i-1] = getparamval(symInx); /*get current value of parameter */
			}
		}
	}

	fprintf(fp,"\n");
}

epilog()
{
	if (fp != NULL)
		fclose(fp);
	savexpar(); /* save paramter file */
	saveenv(); /* save environment file */
}
printold()
{
	int i;
	fprintf(fp,"\n");
	for (i=0;i<=limit;i++)
	{
		fprintf(fp,"OldPrm[%d] = %28.17lf\n",i,OldPrm[i]);
	}
	fprintf(fp,"\n");
}