/* */
/*  Little cms - profiler construction set */
/*  Copyright (C) 1998-2001 Marti Maria <marti@littlecms.com> */
/* */
/* THIS SOFTWARE IS PROVIDED "AS-IS" AND WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS, IMPLIED OR OTHERWISE, INCLUDING WITHOUT LIMITATION, ANY */
/* WARRANTY OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. */
/* */
/* IN NO EVENT SHALL MARTI MARIA BE LIABLE FOR ANY SPECIAL, INCIDENTAL, */
/* INDIRECT OR CONSEQUENTIAL DAMAGES OF ANY KIND, */
/* OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, */
/* WHETHER OR NOT ADVISED OF THE POSSIBILITY OF DAMAGE, AND ON ANY THEORY OF */
/* LIABILITY, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE */
/* OF THIS SOFTWARE. */
/* */
/* This file is free software; you can redistribute it and/or modify it */
/* under the terms of the GNU General Public License as published by */
/* the Free Software Foundation; either version 2 of the License, or */
/* (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be useful, but */
/* WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU */
/* General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public License */
/* along with this program; if not, write to the Free Software */
/* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, ma 02110-1301, USA. */
/* */
/* As a special exception to the GNU General Public License, if you */
/* distribute this file as part of a program that contains a */
/* configuration script generated by Autoconf, you may include it under */
/* the same distribution terms that you use for the rest of that program. */
/* */
/* Version 1.09a */

#include "lcmsprf.h"


/* From "numerical recipes in C" */
/*  */
/* Levenberg-Marquardt method, attempting to reduce the value X2 of a  */
/* fit between a set of data points x[1..ndata], y[1..ndata] with individual  */
/* standard deviations sig[1..ndata], and a nonlinear function dependent  */
/* on ma coefficients a[1..ma]. The input array ia[1..ma] */
/* indicates by nonzero entries those components of a that should be  */
/* fitted for, and by zero entries those components that should be held  */
/* fixed at their input values. The program returns current best-fitt  */
/* values for the parameters a[1..ma], and chisq. The arrays */
/* covar[1..ma][1..ma], alpha[1..ma][1..ma] are used as  */
/* working space  during most iterations. Supply a routine  */
/* funcs(x, a, yfit, dyda, ma)  */
/* that evaluates the fitting function yfit, and its derivatives dyda[1..ma]  */
/* with respect to the fitting parameters a at x. On the first call provide  */
/* an initial guess for the parameters a, and set alamda<0 for initialization */
/* (which then sets alamda=.001). If a step succeeds chisq becomes smaller  */
/* and alamda decreases by a factor of 10. If a step fails alamda grows by  */
/* a factor of 10. You must call this routine repeatedly until convergence  */
/* is achieved. Then, make one final call with alamda=0, so that  */
/* covar[1..ma][1..ma] returns the covar matrix, and alpha the  */
/* alpha matrix. (Parameters held fixed will return zero covariances.) */


LCMSHANDLE cdecl cmsxLevenbergMarquardtInit(LPSAMPLEDCURVE x, LPSAMPLEDCURVE y, double sig,
								double a[],
								int ma,  
								void (*funcs)(double, double[], double*, double[], int)
								);

double    cdecl cmsxLevenbergMarquardtAlamda(LCMSHANDLE hMRQ);
double    cdecl cmsxLevenbergMarquardtChiSq(LCMSHANDLE hMRQ);
BOOL      cdecl cmsxLevenbergMarquardtIterate(LCMSHANDLE hMRQ);
BOOL      cdecl cmsxLevenbergMarquardtFree(LCMSHANDLE hMRQ);

/* ---------------------------------------------------------------------------- */



typedef struct {

	LPSAMPLEDCURVE x;
	LPSAMPLEDCURVE y;		     
	int     ndata;  	 
	double* a;		     
	int     ma;
	LPMATN  covar;		 
	LPMATN  alpha;		 
	double* atry;		 
	LPMATN  beta;		 
	LPMATN  oneda;		 
	double* dyda;	 	 		
	double  ochisq;
	double  sig;


	void (*funcs)(double, double[], double*, double[], int); 

	double alamda;
	double chisq;

} LMRTQMIN, FAR* LPLMRTQMIN;




static 
void  mrqcof(LPLMRTQMIN pLM, double *a, LPMATN alpha, LPMATN beta, double *chisq)
{
	int i, j, k;
	double ymod, wt, sig2i, dy;

	for(j = 0; j < pLM->ma; j++)
	{
		for(k = 0; k <= j; k++)
			alpha->Values[j][k] = 0.0;

		beta->Values[j][0] = 0.0;
	}
	
	*chisq = 0.0;
	sig2i = 1.0 / (pLM->sig * pLM->sig);

	for(i = 0; i < pLM->ndata; i++)
	{
		(*(pLM->funcs))(pLM->x ->Values[i], a, &ymod, pLM->dyda, pLM->ma);

		dy = pLM->y->Values[i] - ymod;

		for(j = 0; j < pLM->ma; j++)
		{
			wt = pLM->dyda[j] * sig2i;
			
			for(k = 0; k <= j; k++)
				alpha->Values[j][k] += wt * pLM->dyda[k];

			beta->Values[j][0] += dy * wt;
		}

		*chisq += dy * dy * sig2i;  
	}

	for(j = 1; j < pLM->ma; j++)	/* Fill in the symmetric side. */
		for(k = 0; k < j; k++)
			alpha->Values[k][j] = alpha->Values[j][k];
}



static 
void FreeStruct(LPLMRTQMIN pLM)
{	
	if(pLM == NULL) return;

	if(pLM->covar) MATNfree (pLM->covar);
	if(pLM->alpha) MATNfree (pLM->alpha);
	if(pLM->atry)  free(pLM->atry);
	if(pLM->beta)  MATNfree (pLM->beta);
	if(pLM->oneda) MATNfree (pLM->oneda);
	if(pLM->dyda)  free(pLM->dyda);
	free(pLM);
}



LCMSHANDLE cmsxLevenbergMarquardtInit(LPSAMPLEDCURVE x, LPSAMPLEDCURVE y, double sig,
							double a[],
							int ma,  
							void (*funcs)(double, double[], double*, double[], int))
							
{
	int i;
	LPLMRTQMIN pLM;
	
	if (x ->nItems != y ->nItems) return NULL;

	pLM = (LPLMRTQMIN) malloc(sizeof(LMRTQMIN));
	if(!pLM)
		return NULL;

	ZeroMemory(pLM, sizeof(LMRTQMIN));
	
	if((pLM->atry = (double*)malloc(ma * sizeof(double))) == NULL) goto failed;
	if((pLM->beta = MATNalloc (ma, 1)) == NULL) goto failed;
	if((pLM->oneda = MATNalloc (ma, 1)) == NULL) goto failed;

	

	if((pLM->covar = MATNalloc(ma, ma)) == NULL) goto failed;
	if((pLM->alpha = MATNalloc(ma, ma)) == NULL) goto failed;		
	if((pLM->dyda = (double*)malloc(ma * sizeof(double))) == NULL) goto failed;

	pLM->alamda = 0.001;

	pLM->ndata = x ->nItems;
	pLM->x = x;
	pLM->y = y;
	pLM->ma = ma;
	pLM->a = a;
	pLM->funcs = funcs;
	pLM->sig = sig;
		
	mrqcof(pLM, a, pLM->alpha, pLM->beta, &pLM->chisq);	
	pLM->ochisq = (pLM->chisq);

	for(i = 0; i < ma; i++) pLM->atry[i] = a[i];

	return (LCMSHANDLE) pLM;

failed:
	FreeStruct(pLM);
	return NULL;
}


BOOL cmsxLevenbergMarquardtFree(LCMSHANDLE hMRQ)
{
	LPLMRTQMIN pLM = (LPLMRTQMIN)hMRQ;
	if(!pLM)
		return false;

	FreeStruct(pLM);	
	return true;
}


BOOL cmsxLevenbergMarquardtIterate(LCMSHANDLE hMRQ)
{
	int j, k;
	BOOL sts;
	LPLMRTQMIN pLM = (LPLMRTQMIN)hMRQ;
	if(!pLM)
		return false;

	for(j = 0; j < pLM->ma; j++) /* Alter linearized fitting matrix, by augmenting diagonal elements.  */
	{
		for(k = 0; k < pLM->ma; k++)
			pLM->covar->Values[j][k] = pLM->alpha->Values[j][k];

		pLM->covar->Values[j][j] = pLM->alpha->Values[j][j] * (1.0 + pLM ->alamda);
		pLM->oneda->Values[j][0] = pLM->beta->Values[j][0];
	}
	
	if((sts = MATNsolve (pLM->covar, pLM->oneda)) != true)  /* Matrix solution. */
		return sts;

	for(j = 0; j < pLM->ma; j++)							/* Did the trial succeed? */
		pLM->atry[j] = pLM->a[j] + pLM->oneda->Values[j][0];

	mrqcof(pLM, pLM->atry, pLM->covar, pLM->oneda, &pLM -> chisq);
	
	if (pLM->chisq < pLM->ochisq) {	 /* Success, accept the new solution. */

		pLM->alamda *= 0.1;
		pLM->ochisq = pLM->chisq;

		for(j = 0; j < pLM->ma; j++)
		{
			for(k = 0; k < pLM->ma; k++) 
				pLM->alpha->Values[j][k] = pLM->covar->Values[j][k];

			pLM->beta->Values[j][0] = pLM->oneda->Values[j][0];			
		}

		for (j=0; j < pLM ->ma; j++) pLM->a[j] = pLM->atry[j];
	}
	else   /* Failure, increase alamda and return. */
	{
		pLM -> alamda *= 10.0;
		pLM->chisq = pLM->ochisq;
	}
	
	return true;
}


double cmsxLevenbergMarquardtAlamda(LCMSHANDLE hMRQ)
{
		LPLMRTQMIN pLM = (LPLMRTQMIN)hMRQ;

		return pLM ->alamda;
}

double cmsxLevenbergMarquardtChiSq(LCMSHANDLE hMRQ)
{
		LPLMRTQMIN pLM = (LPLMRTQMIN)hMRQ;

		return pLM ->chisq;
}