/* multifit/lmmisc.c

 * 

 * Copyright (C) 2014 Patrick Alken

 * 

 * This program 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 3 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.

 */

/* compute step dx by solving (J^T J + mu*I) dx = -J^T f */

static int

lmniel_calc_dx(const double mu, const gsl_matrix *A, const gsl_vector *rhs,

               gsl_vector *dx, lmniel_state_t *state)

{

  int status;

  gsl_matrix *A_copy = state->A_copy;

  gsl_vector_view diag = gsl_matrix_diagonal(A_copy);

  /* make a copy of J^T J matrix */

  gsl_matrix_memcpy(A_copy, A);

  /* augment normal equations with LM parameter: A -> A + mu*I */

  gsl_vector_add_constant(&diag.vector, mu);

  status = gsl_linalg_QR_decomp(A_copy, state->work);

  if (status)

    return status;

  status = gsl_linalg_QR_solve(A_copy, state->work, rhs, dx);

  if (status)

    return status;

  return GSL_SUCCESS;

} /* lmniel_calc_dx() */

/* compute x_trial = x + dx */

static void

lmniel_trial_step(const gsl_vector * x, const gsl_vector * dx,

                  gsl_vector * x_trial)

{

  size_t i, N = x->size;

  for (i = 0; i < N; i++)

    {

      double dxi = gsl_vector_get (dx, i);

      double xi = gsl_vector_get (x, i);

      gsl_vector_set (x_trial, i, xi + dxi);

    }

} /* lmniel_trial_step() */

/*

lmniel_calc_dF()

  Compute dF = F(x) - F(x + dx) = 1/2 (f - f_new)^T (f + f_new)

*/

static double

lmniel_calc_dF(const gsl_vector *f, const gsl_vector *f_new)

{

  const size_t N = f->size;

  size_t i;

  double dF = 0.0;

  for (i = 0; i < N; ++i)

    {

      double fi = gsl_vector_get(f, i);

      double fnewi = gsl_vector_get(f_new, i);

      dF += (fi - fnewi) * (fi + fnewi);

    }

  dF *= 0.5;

  return dF;

} /* lmniel_calc_dF() */

/*

lmniel_calc_dL()

  Compute dL = L(0) - L(dx) = 1/2 dx^T (mu * D^T D dx - g)

Here, the mg input is -g

*/

static double

lmniel_calc_dL(const double mu, const gsl_vector *diag,

               const gsl_vector *dx, const gsl_vector *mg)

{

  const size_t p = dx->size;

  size_t i;

  double dL = 0.0;

  for (i = 0; i < p; ++i)

    {

      double dxi = gsl_vector_get(dx, i);

      double di = gsl_vector_get(diag, i);

      double mgi = gsl_vector_get(mg, i); /* -g_i */

      dL += dxi * (mu * di * di * dxi + mgi);

    }

  dL *= 0.5;

  return dL;

} /* lmniel_calc_dL() */