/*      FILE: /mxtools/src/strawman/lm.c
 *   PURPOSE: Do Levenberg-Marquardt minimization.
 *    AUTHOR: Kenneth J. Mighell (mighell@noao.edu)
 *  LANGUAGE: ANSI C
 *      DATE: 2001NOV09
 * 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_i19(
inc_PsfS *PsfS,      /* PsfS */
    long nptsl,      /* number of data points */
  struct mxip_image_s *xd,    /* X coordinate image */
  struct mxip_image_s *yd,    /* Y coordinate image */
  struct mxip_image_s *zd,    /* Z measurement image */
  struct mxip_image_s *zerrd, /* Z measurement error image */
  struct mxip_image_s *md,    /* Z data image */
  struct mxip_image_s *wd,    /* Work image */
  struct mxip_image_s **del,  /* Partial derivates of dimension MXNPARL */
    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 */
     int method_is_digital /* 1 <- digital, 0 <- analytical */
)
{

  char mxfunc[] = "LevenbergMarquardt_i19.c";
  int mxstatus = 0;
  int status;

  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,
    tmpd1,
    tmpd2,
    tmpd3,
    tmpd4;

  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,
    ix,
    iy,
    nx,
    ny;

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

  const int
    b0 = 0,
    bx = 1,
    by = 2,
    s0 = 3,
    sx = 4,
    sy = 5,
    ss = 6;

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

  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; 
  mxstatus++;
  if (ndfl<=0) { sprintf (MX.tmpmsg,
    "number of degrees of freedom less than zero?  ndfl=%d\n",ndfl);
    goto mx_error2;
  }

  /* 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->vectord[j];
    wd->vectord[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;
    }
  }

  /* Compute initial models and its partial derivatives */
  mxstatus++;
  status = fdelb0_i3( del[b0]->matrixd, del[b0]->nxi, del[b0]->nyi );
  if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdelb0!\n");
      goto mx_error2;
  }      
  mxstatus++;
  status = fdelbx_i4( 
    del[bx]->matrixd, xd->matrixd, del[bx]->nxi, del[bx]->nyi 
  );
  if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdelbx!\n");
      goto mx_error2;
  }      
  mxstatus++;
  status = fdelby_i4( 
    del[by]->matrixd, yd->matrixd, del[by]->nxi, del[by]->nyi 
  );
  if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdelby!\n");
      goto mx_error2;
  }      

  mxstatus++;
  if (method_is_digital) {
    status = fdels0d_i7( 
      PsfS,
      del[s0],
      xd->matrixd, 
      yd->matrixd, 
      del[s0]->nxi, 
      del[s0]->nyi, 
      ad
    );
    if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdels0d!\n");
      goto mx_error2;
    }      
  } else {
    status = fdels0a_i6( 
      del[s0]->matrixd, 
      xd->matrixd, 
      yd->matrixd, 
      del[s0]->nxi, 
      del[s0]->nyi, 
      ad
    );
    if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdels0a!\n");
      goto mx_error2;
    }      
  }

  mxstatus++;
  if (method_is_digital) {
    status = fdelsxd_i5(
      del[sx]->matrixd, 
      del[s0]->matrixd, 
      ad[s0],
      del[sx]->nxi, 
      del[sx]->nyi
    );
    if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdelsxd!\n");
      goto mx_error2;
    }      
  } else {
    status = fdelsxa_i6( 
      del[sx]->matrixd, 
      xd->matrixd, 
      yd->matrixd, 
      del[sx]->nxi, 
      del[sx]->nyi, 
      ad
    );
    if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdelsxa!\n");
      goto mx_error2;
    }      
  }

  mxstatus++;
  if (method_is_digital) {
    status = fdelsyd_i5(
      del[sy]->matrixd, 
      del[s0]->matrixd, 
      ad[s0],
      del[sx]->nxi, 
      del[sx]->nyi
    );
    if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdelsyd!\n");
      goto mx_error2;
    }      
  } else {
    status = fdelsya_i6( 
      del[sy]->matrixd, 
      xd->matrixd, 
      yd->matrixd, 
      del[sy]->nxi, 
      del[sy]->nyi, 
      ad
    );
    if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdelsya!\n");
      goto mx_error2;
    }      
  }
  mxstatus++;
  status = fdelss_i6( 
    del[ss]->matrixd, xd->matrixd, yd->matrixd, del[ss]->nxi, del[ss]->nyi, ad
  );
  if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdelss!\n");
      goto mx_error2;
  }      
  mxstatus++;
  status = fmodel_i7( 
    md->matrixd,
    del[s0]->matrixd, 
    xd->matrixd, 
    yd->matrixd, 
    md->nxi, 
    md->nyi, 
    ad
  );
  if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fmodel!\n");
      goto mx_error2;
  }      

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

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

    for (jj=0; jj<MXNPARL; jj++) delad[jj] = del[jj]->vectord[i];

    tmpd = wd->vectord[i] * (zd->vectord[i] - md->vectord[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 mx_error2;
      }      
      betad[j] += tmpd * delad[jj];

      for (k=0; k<=j; k++) {
	
	alphad[OFFSET(j,k)] += wd->vectord[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 */
  mxstatus++;
  status = fmodel_i7( md->matrixd,
    del[s0]->matrixd, xd->matrixd, yd->matrixd, md->nxi, md->nyi, ad
  );
  if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fmodel!\n");
      goto mx_error2;
  }      

  /* Determine its chi-square value */
  chisqd = 0.0;
  for (j=0; j<nptsl; j++) {
    tmpd = (zd->vectord[j] - md->vectord[j]) / zerrd->vectord[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 ==================================================
   */
  /* 
   * Determine the PSF at the position given in the new parameter vector 
   */
  mxstatus++;
  if (method_is_digital) {
    status = fdels0d_i7( 
      PsfS,
      del[s0],
      xd->matrixd, 
      yd->matrixd, 
      del[s0]->nxi, 
      del[s0]->nyi, 
      ad
    );
    if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdels0d!\n");
      goto mx_error2;
    }      
  } else {
    status = fdels0a_i6( 
      del[s0]->matrixd, 
      xd->matrixd, 
      yd->matrixd, 
      del[s0]->nxi, 
      del[s0]->nyi, 
      ad
    );
    if (status) { sprintf (MX.tmpmsg, 
      "Encountered problems computing fdels0a!\n");
      goto mx_error2;
    }      
  }
  /*
   * Now compute the model using the new PSF 
   */
  mxstatus++;
  status = fmodel_i7( 
    md->matrixd,
    del[s0]->matrixd, 
    xd->matrixd, 
    yd->matrixd, 
    md->nxi, 
    md->nyi, 
    ad
  );
  if (status) { sprintf (MX.tmpmsg, 
    "Encountered problems computing fmodel!\n");
    goto mx_error2;
  }      

  /* Determine its chi-square value */
  chisqd = 0.0;
  for (j=0; j<nptsl; j++) {
    tmpd = (zd->vectord[j] - md->vectord[j]) / zerrd->vectord[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;
  }
  mxstatus++;
  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 mx_error2;
    }      
    aerrd[jj] = sqrt(covard[OFFSET(j,j)] / alphad[OFFSET(j,j)]);
  }
  
  lambdad /= multiplier_lambdad;

ok:
  status = 0;
  goto bye;

mx_error1:
  mxp_tmpmsg_init_f0();
mx_error2:
  mxp_errmsg_append_f3 (mxfunc, mxstatus, MX.tmpmsg);
  mxp_tmpmsg_init_f0();
  status = 1;
  goto bye;

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 */