#include "filter.h"
#include <math.h>
#include <float.h>

#include "adjust.h"

lmfunc	fcn; 
static int			AllocateLMStruct( struct LMStruct *LM );
static void			FreeLMStruct	( struct LMStruct *LM );
void 			bracket( struct LMStruct	*LM );
double 			sumSquared( double *a, int n );

#define FUNCS_PER_CP getFcnPanoNperCP()  // number of functions per control point

// Call Levenberg-Marquard optimizer
#if 1
void  RunLMOptimizer( OptInfo	*o)
{
	struct 	LMStruct	LM;
	int 	iflag;
	char	*warning;
	char 	*infmsg[] = {
				"improper input parameters",
				"the relative error in the sum of squares is at most tol",
				"the relative error between x and the solution is at most tol",
				"conditions for info = 1 and info = 2 both hold",
				"fvec is orthogonal to the columns of the jacobian to machine precision",
				"number of calls to fcn has reached or exceeded 200*(n+1)",
				"tol is too small. no further reduction in the sum of squares is possible",
				"tol too small. no further improvement in approximate solution x possible",
				"Interrupted"
				};
	int istrat;       // strategy
	int totalfev;     // total function evaluations
	int numconstraints;  // number of constraints imposed by control points
	int i;
	int lmInfo;
	AlignInfo	*g;	              // obtained from adjust.c

	// PrintError("RunLMOptimizer");
	
	// The method used here is a hybrid of two optimization strategies.
	// In the first strategy, fcnPano is configured to return one function per
	// control point, that function being total distance without regard for
	// direction.  In the second strategy, fcnPano is configured to return two
	// functions per control point, those functions being distance in two
	// directions, typically longitude and latitude.  The second strategy
	// converges much faster, but may be less stable with poor initial estimates.
	// So, we use the first method as long as it makes significant progress
	// (currently 5% reduction in error per iteration), and then switch to
	// the second method to rapidly polish the estimate.  Final result
	// returned to the user is that of the second method.
	//
	// Older versions of Panorama Tools used just the first strategy,
	// with error tolerances set to make it run to full convergence,
	// which often took hundreds or thousands of iterations.  The hybrid
	// approach typically converges much faster (a few tens of iterations)
	// and appears to be equally robust in testing to date.  Full convergence
	// (to am lmdif ftol of 1.0e-14) is not always achieved faster than the old
	// version.  However the convergence rate (error reduction per wall-clock
	// second) has been significantly better in all cases tested, and the final
	// accuracy has been equal or improved.
	//
	// So, in the interest of behavior that is friendlier to the user, I have
	// set an ftol convergence criterion that is looser than before, 1.0e-6
	// instead of 1.0e-14.  By this point, it is very unlikely that
	// significant reductions can be achieved by more iterating, since
	// even 10,000 more iterations would be predicted to make at most 1%
	// improvement in the total error.
	//
	// I have also made the diagnosis of too few control points more precise
	// and more obvious to the user.  The old version complained if the
	// number of control points was less than the number of parameters,
	// although in fact each normal control point contributes two independent
	// constraints (x and y) so the actual critical number is
	// 2*controlpoints >= parameters.  As a result, the old version often
	// complained even when things were fine, and never complained more loudly
	// even when things were awful.  This version does not complain
	// at all unless there are not enough actual constraints, and then it puts
	// out an error dialog that must be dismissed by the user.
	//
	//   Rik Littlefield (rj.littlefield@computer.org), May 2004.

	LM.n = o->numVars;
	
	g = GetGlobalPtr();
	numconstraints = 0;
	for(i=0; i < g->numPts; i++) {
		if (g->cpt[i].type == 0)
		     numconstraints += 2;
		else numconstraints += 1;
	}

	warning = "";
	if( numconstraints < LM.n )
	{
		char msgx[200];
		warning	= "Warning: Number of Data Points is smaller than Number of Variables to fit.\n";
		sprintf (msgx,"You have too few control points (%d) or too many parameters (%d).  Strange values may result!",o->numData,LM.n);
		PrintError(msgx);
	}
	
	totalfev = 0;
	for (istrat=1; istrat <= 2; istrat++) {

		setFcnPanoNperCP(istrat);

		LM.m = o->numData*FUNCS_PER_CP;
		if( LM.m < LM.n ) LM.m = LM.n;  // in strategy #1, fcnpano will pad fvec if needed

		fcn = o->fcn;

		if( AllocateLMStruct( &LM ) != 0 )
		{
			PrintError( "Not enough Memory" );
			return;
		}

		// Initialize optimization params

		if( o->SetVarsToX( LM.x ) != 0)
		{
			PrintError("Internal Error");
			return;
		}

		iflag 		= -100; // reset counter and initialize dialog
		fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
		if (istrat == 2) setFcnPanoDoNotInitAvgFov();

		// infoDlg ( _initProgress, "Optimizing Variables" );

		/* Call lmdif. */
		LM.ldfjac 	= LM.m;
		LM.mode 	= 1;
		LM.nprint 	= 1; // 10
		LM.info 	= 0;
		LM.factor 	= 100.0;

		LM.ftol 	= 	1.0e-6; // used to be DBL_EPSILON; //1.0e-14;
		if (istrat == 1) {
			LM.ftol = 0.05;  // for distance-only strategy, bail out when convergence slows
		}

		lmdif(	LM.m,		LM.n,		LM.x,		LM.fvec,	LM.ftol,	LM.xtol,
				LM.gtol,	LM.maxfev,	LM.epsfcn,	LM.diag,	LM.mode,	LM.factor,
				LM.nprint,	&LM.info,	&LM.nfev,	LM.fjac,	LM.ldfjac,	LM.ipvt,
				LM.qtf,		LM.wa1,		LM.wa2,		LM.wa3,		LM.wa4);

		lmInfo = LM.info;

		// At end, one final evaluation to get errors that do not have fov stabilization applied,
		// for reporting purposes.
		
		if (istrat == 2) {
			forceFcnPanoReinitAvgFov();
			iflag = 1;
			fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
		}
		
		o->SetXToVars( LM.x );

		iflag 		= -99; // reset counter and dispose dialog
		fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
		// infoDlg ( _disposeProgress, "" );

		// Display solver info
		
		if(LM.info >= 8)
				LM.info = 4;
		if(LM.info < 0)
				LM.info = 8;
		totalfev += LM.nfev;

		sprintf( (char*) o->message, "# %s%d function evaluations\n# %s\n# final rms error %g units\n",
									warning, totalfev, infmsg[LM.info],
									sqrt(sumSquared(LM.fvec,LM.m)/LM.m) * sqrt((double)FUNCS_PER_CP));

		FreeLMStruct( &LM );
		
		if (lmInfo < 0) break;  // to honor user cancel in strategy 1
	}
	setFcnPanoNperCP(1); // Force back to startegy 1 for backwards compatability

}

#endif

#if 0
void  RunLMOptimizer( OptInfo	*o){
	RunBROptimizer ( o, 1.0e-9);
}
#endif
// Call Bracketing optimizer


void  RunBROptimizer ( OptInfo	*o, double minStepWidth)
{
	struct 	LMStruct	LM;
	int 	iflag;

	// PrintError("RunBROptimizer");
	LM.n = o->numVars;
	
	setFcnPanoNperCP(1);  // This optimizer does not use direction, don't waste time computing it

	if( o->numData*FUNCS_PER_CP < LM.n )
	{
		LM.m 		= LM.n;
	}
	else
	{
		LM.m 		= o->numData*FUNCS_PER_CP;
	}

	fcn = o->fcn;
		
	if( AllocateLMStruct( &LM ) != 0 )
	{
		PrintError( "Not enough Memory to allocate Data for BR-solver" );
		return;
	}
				
				
	// Initialize optimization params

	if( o->SetVarsToX( LM.x ) != 0)
	{
		PrintError("Internal Error");
		return;
	}

	iflag 		= -100; // reset counter
	fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
		
	//infoDlg ( _initProgress, "Optimizing Params" );

	/* Call lmdif. */
	LM.ldfjac 	= LM.m;
	LM.mode 	= 1;
	LM.nprint 	= 1;
	// Set stepwidth to angle corresponding to one pixel in final pano
	LM.epsfcn	= minStepWidth; // g->pano.hfov / (double)g->pano.width; 
	
	LM.info 	= 0;
	LM.factor 	= 1.0;

#if 0		
	lmdif(	LM.m,		LM.n,		LM.x,		LM.fvec,	LM.ftol,	LM.xtol,
			LM.gtol,	LM.maxfev,	LM.epsfcn,	LM.diag,	LM.mode,	LM.factor,
			LM.nprint,	&LM.info,	&LM.nfev,	LM.fjac,	LM.ldfjac,	LM.ipvt,
			LM.qtf,		LM.wa1,		LM.wa2,		LM.wa3,		LM.wa4);

#endif

	bracket( &LM );

	o->SetXToVars( LM.x );
	iflag 		= -99; // 
	fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
	//infoDlg ( _disposeProgress, "" );
	

	FreeLMStruct( &LM );
	
}



// Allocate Memory and set default values. n must be set!

int	AllocateLMStruct( struct LMStruct *LM )
{
	int i,k;
	

	if( LM->n <= 0 || LM->m <= 0 || LM->n > LM->m )
		return -1;
		
	LM->ftol 	= 	DBL_EPSILON;//1.0e-14;
	LM->xtol 	= 	DBL_EPSILON;//1.0e-14;
	LM->gtol 	= 	DBL_EPSILON;//1.0e-14;
	LM->epsfcn 	=  	DBL_EPSILON * 10.0;//1.0e-15;
	LM->maxfev 	=  	100 * (LM->n+1) * 100; 
	
	LM->ipvt = NULL;
	LM->x = LM->fvec = LM->diag = LM->qtf = LM->wa1 = LM->wa2 = LM->wa3 = LM->wa4 = LM->fjac = NULL;

	LM->ipvt 	= (int*) 	malloc(  LM->n * sizeof( int ));		
	LM->x 		= (double*) malloc(  LM->n * sizeof( double )); 		
	LM->fvec 	= (double*) malloc(  LM->m * sizeof( double )); 		
	LM->diag 	= (double*) malloc(  LM->n * sizeof( double )); 		
	LM->qtf 	= (double*) malloc(  LM->n * sizeof( double )); 		
	LM->wa1 	= (double*) malloc(  LM->n * sizeof( double )); 		
	LM->wa2 	= (double*) malloc(  LM->n * sizeof( double )); 		
	LM->wa3 	= (double*) malloc(  LM->n * sizeof( double )); 		
	LM->wa4 	= (double*) malloc(  LM->m * sizeof( double )); 		
	LM->fjac 	= (double*) malloc(  LM->m  * LM->n * sizeof( double ));

	if( LM->ipvt == NULL ||  LM->x    == NULL 	|| LM->fvec == NULL || LM->diag == NULL || 
		LM->qtf  == NULL ||  LM->wa1  == NULL 	|| LM->wa2  == NULL || LM->wa3  == NULL || 
		LM->wa4  == NULL ||  LM->fjac == NULL )
	{
		FreeLMStruct( LM );
		return -1;
	}


	// Initialize to zero

	for(i=0; i<LM->n; i++)
	{
		LM->x[i] = LM->diag[i] = LM->qtf[i] = LM->wa1[i] = LM->wa2[i] = LM->wa3[i] =  0.0;
		LM->ipvt[i] = 0;
	}

	for(i=0; i<LM->m; i++)
	{
		LM->fvec[i] = LM->wa4[i] = 0.0;
	}

	k = LM->m * LM->n;
	for( i=0; i<k; i++)
			LM->fjac[i] = 0.0;
	
	return 0;
}
		



	
void FreeLMStruct( struct LMStruct *LM )
{
	if(LM->x 	!= NULL) 	free( LM->x );
	if(LM->fvec != NULL) 	free( LM->fvec );
	if(LM->diag != NULL) 	free( LM->diag );
	if(LM->qtf 	!= NULL) 	free( LM->qtf );
	if(LM->wa1 	!= NULL) 	free( LM->wa1 );
	if(LM->wa2 	!= NULL) 	free( LM->wa2 );
	if(LM->wa3 	!= NULL) 	free( LM->wa3 );
	if(LM->wa4 	!= NULL) 	free( LM->wa4 );
	if(LM->fjac != NULL) 	free( LM->fjac );
	if(LM->ipvt != NULL) 	free( LM->ipvt );
}


void bracket( struct LMStruct	*LM )
{
	int iflag = 1,i;
	double eps, delta, delta_max;
	int changed, c = 1;
	
	
	fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);
	if( iflag < 0 ) return;

	// and do a print
	iflag = 0;
	fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);
	if( iflag < 0 ) return;
	iflag = 1;
	
	eps = sumSquared( LM->fvec, LM->m );
	
	// Choose delta_max to be between 1 and 2 degrees
	
	if( LM->epsfcn <= 0.0 ) return; // This is an error
	
	for( delta_max = LM->epsfcn; delta_max < 1.0; delta_max *= 2.0){}
	
	for( delta = delta_max; 
		 delta >= LM->epsfcn; 
		 delta /= 2.0  )
	{
		c = 1;
		
		// PrintError("delta = %lf", delta);
		while( c )
		{
			c = 0;
		
			for( i = 0; i < LM->n; i++ )
			{
				changed = 0;
				LM->x[i] += delta;
				fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag); if( iflag < 0 ) return;
				
				if( delta == delta_max ) // search everywhere
				{
					while(  sumSquared( LM->fvec, LM->m ) < eps )
					{
						changed = 1;
						eps = sumSquared( LM->fvec, LM->m );
						LM->x[i] += delta;
						fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);if( iflag < 0 ) return;
					}
					LM->x[i] -= delta;
				}
				else // do just this one step
				{
					if( sumSquared( LM->fvec, LM->m ) < eps )
					{
						eps = sumSquared( LM->fvec, LM->m );
						changed = 1;
					}
					else
						LM->x[i] -= delta;
				}
		
				if( !changed )	// Try other direction
				{
					LM->x[i] -= delta;
					fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);if( iflag < 0 ) return;
					
					if( delta == delta_max ) // search everywhere
					{
						while(  sumSquared( LM->fvec, LM->m ) < eps )
						{
							changed = 1;
							eps = sumSquared( LM->fvec, LM->m );
							LM->x[i] -= delta;
							fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);if( iflag < 0 ) return;
						}
						LM->x[i] += delta;
					}
					else // do just this one step
					{
						if( sumSquared( LM->fvec, LM->m ) < eps )
						{
							eps = sumSquared( LM->fvec, LM->m );
							changed = 1;
						}
						else
							LM->x[i] += delta;
					}
				}
				
				if( changed ) c = 1;
		
				if (c) { // an improvement, let's see it (and give the user a chance to bail out)
					iflag = 0;
					fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);
					if( iflag < 0 ) return;
					iflag = 1;
				}
								
			}
		}
		// PrintError("%lf %ld %lf", delta, c, eps);
					iflag = 0;
					LM->fvec[0] = sqrt(eps/LM->m);
					fcn(LM->m, LM->n, LM->x, LM->fvec, &iflag);
					if( iflag < 0 ) return;
					iflag = 1;

	}
	
}
	
	
double sumSquared( double *a, int n )
{
	double result = 0.0;
	int i;
	
	for( i=0; i<n; i++ )
		result += a[i] * a[i];
		
	return result;
}