/*      FILE: lm.c
 *   PURPOSE: Do Levenberg-Marquardt minimization.
 *    AUTHOR: Kenneth J. Mighell (mighell@noao.edu)
 *  LANGUAGE: ANSI C
 *      DATE: 2001SEP10
 * COPYRIGHT: (C) 2001 Assoc. of Universities for Research in Astronomy Inc.
 */

#include "mx.h"
#include "inc.h"

#define DEBUGT (MX_TRUE)
#define DEBUGF (MX_FALSE)
#define DEBUG (DEBUGT)

#define OFFSET(j,k) (j+(k*MXNPARL))

int LevenbergMarquardt_i16(
    long nptsl,      /* number of data points */
  double xd[],       /* X coordinate vector */
  double yd[],       /* Y coordinate vector */
  double zd[],       /* Z measurement vector */
  double zerrd[],    /* Z error vector */
  double md[],       /* Z model vector */
  double wd[],       /* work vector */
    long nparl,      /* number of parameters */
  double ad_p[],     /* parameter vector */
  double aerrd[],    /* parameter error vector */
    char ausec[],    /* a string of nparl characters either "f" or "c" */
  double diffmnd,    /* stop fitting when (old_chisqd-chisqd)<=diffmnd */
  double *chisqd_p,  /* chi-square */
    long *ndfl_p,    /* number of degrees of freedom */
     int *mfiti_p,   /* number of free parameters */
  double *lambdad_p  /* Levenberg-Marquart method lambda */
){

  char funcnamec[] = "LevenbergMarquardt_i16.c";

  double 
    chisqd = *chisqd_p,
    lambdad = *lambdad_p,
    ad[MXNPARL],
    alphad[MXNPARL*MXNPARL],
    covard[MXNPARL*MXNPARL],
    betad[MXNPARL], 
    deltad[MXNPARL],
    delad[MXNPARL],
    detd,
    old_chisqd,
    diffd,
    tmpd;

  long 
    ndfl; /* number of degrees of freedom */




  int
    mfiti, /* number of free parameters <= nparl <= MXNPARL */
    ausei[MXNPARL],
    amapi[MXNPARL],
    i,
    j,
    k,
    index,
    ii,
    jj,
    kk;

  int status;

  const double multiplier_lambdad = 10.0; /* see Marquardt (1963) */

  status = 1;

  if ( strlen(ausec) != nparl ) { sprintf (MX.tmpmsg, 
    "\a*** ERROR *** "
    "strlen(\"%s\") != (nparl=%ld)\n", ausec, nparl);
    goto error;
  }

  mfiti = 0;
  for (j=0; j<nparl; j++) {
    ausei[j] = MX_FALSE;
    amapi[j] = -1;
    if ( ausec[j] == 'f' ) {
      ausei[j] = MX_TRUE;
      mfiti++;
      amapi[mfiti-1] = j;
    }
  }

  ndfl = nptsl - mfiti; 
  if (ndfl<=0) { sprintf (MX.tmpmsg,
    "number of degrees of freedom less than zero?  ndfl=%d\n",ndfl);
    goto error;
  }

  /* set initial chi-square value to a very large (i.e., bad) value */
  chisqd = 10.0*nptsl;
  old_chisqd = chisqd;

  /* Make working version of parameter vector */
  for (j=0; j<nparl; j++) ad[j] = ad_p[j];

  /* Clear the parameter error vector */
  for (j=0; j<nparl; j++) aerrd[j] = 0.0;


  /* Evaluate weights */
  for (j=0; j<nptsl; j++) {
    tmpd = zerrd[j];
    wd[j] = 1. / (tmpd*tmpd);
  }


  /* Initialize the BETAD vector */
  for (j=0; j<mfiti; j++) {
    betad[j] = 0.0;
  }

  /* Initialize the ALPHAD matrix */
  for (j=0; j<mfiti; j++) {
    for (k=0; k<mfiti; k++) {
      alphad[OFFSET(j,k)] = 0.0;
    }
  }

  /* Evaluate ALPHAD and BETAD matrices */
  for (i=0; i<nptsl; i++) {

    fdel_v6( xd[i], yd[i], &md[i], ad, delad, nparl);
    tmpd = wd[i] * (zd[i] - md[i]);

    for (j=0; j<mfiti; j++) {

      jj = amapi[j];
      if (jj<0) { sprintf (MX.tmpmsg, 
       "\a*** ERROR *** "
       "jj=%d but should be (%d<=jj<=%d) !\n", jj, (int)0,(int)(nparl-1) );
       goto error;
      }      
      betad[j] += tmpd * delad[jj];

      for (k=0; k<=j; k++) {
	
	alphad[OFFSET(j,k)] += wd[i] * delad[amapi[j]]* delad[amapi[k]];

      }
    }
  }

  /* Fill up the rest of ALPHAD matrix which is symmetric*/
  for (j=0; j<mfiti; j++) {
    for (k=0; k<j; k++) {
      alphad[OFFSET(k,j)] = alphad[OFFSET(j,k)];
    }
  }

  /* Compute the initial model */
  for (i=0; i<nptsl; i++) fdel_v6( xd[i], yd[i], &md[i], ad, delad, nparl);

  /* Determine its chi-square value */
  chisqd = 0.0;
  for (j=0; j<nptsl; j++) {
    tmpd = (zd[j] - md[j]) / zerrd[j];
    chisqd += (tmpd*tmpd);
  }

loop:
  
  /* Define the symmetric matrix COVARD */
  for (j=0; j<mfiti; j++) {
    for (k=0; k<mfiti; k++) {
      covard[OFFSET(j,k)] =  alphad[OFFSET(j,k)] / 
        sqrt( alphad[OFFSET(j,j)] * alphad[OFFSET(k,k)] );
    }
    /* set the diagonal elements */ 
    covard[OFFSET(j,j)] = 1.0 + lambdad; 
  }

  /* Invert COVARD */
  InvertSymmetricMatrix_v3( covard, mfiti, &detd);

  /* Determine the new parameter vector */
  for (j=0; j<mfiti; j++) {
    for (k=0; k<mfiti; k++) {
      ad[amapi[j]] += betad[k] * covard[OFFSET(j,k)] / 
        sqrt( alphad[OFFSET(j,j)] * alphad[OFFSET(k,k)] );
    }
  }

  /* Compute the model */
  for (i=0; i<nptsl; i++) fdel_v6( xd[i], yd[i], &md[i], ad, delad, nparl);

  /* Determine its chi-square value */
  chisqd = 0.0;
  for (j=0; j<nptsl; j++) {
    tmpd = (zd[j] - md[j]) / zerrd[j];
    chisqd += (tmpd*tmpd);
  }

  /* If chi-square has *increased*, increase LAMBDAD and try again */
  diffd = old_chisqd - chisqd;
  if ( diffd < 0 ) {
    lambdad *= multiplier_lambdad;
    goto loop;
  }

  /* Initialize the parameter error vector */
  for (j=0; j<nparl; j++) {
    aerrd[j] = 0.0;
  }
  for (j=0; j<mfiti; j++) {
    jj = amapi[j];
    if (jj<0) { sprintf (MX.tmpmsg, 
      "\a*** ERROR *** "
      "jj=%d but should be (%d<=jj<=%d) !\n", jj, (int)0,(int)(nparl-1) );
      goto error;
    }      
    aerrd[jj] = sqrt(covard[OFFSET(j,j)] / alphad[OFFSET(j,j)]);
  }
  
  lambdad /= multiplier_lambdad;

ok:
  status = 0;
  goto bye;

error:
  fprintf (stderr, "# %s: %s", funcnamec, MX.tmpmsg);

bye:
  for (j=0; j<MXNPARL; j++) ad_p[j] = ad[j];
  *lambdad_p = lambdad;
  *chisqd_p = chisqd;
  *ndfl_p = ndfl;
  *mfiti_p = mfiti;
  return(status);
}
#undef DEBUGT
#undef DEBUGF
#undef DEBUG
#undef OFFSET
/* end-of-file */