/* multiroots/dogleg.c
 * 
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
 * 
 * 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 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.
 */

#include "enorm.c"

static void compute_diag (const gsl_matrix * J, gsl_vector * diag);
static void update_diag (const gsl_matrix * J, gsl_vector * diag);
static double compute_delta (gsl_vector * diag, gsl_vector * x);
static void compute_df (const gsl_vector * f_trial, const gsl_vector * f, gsl_vector * df);
static void compute_wv (const gsl_vector * qtdf, const gsl_vector *rdx, const gsl_vector *dx, const gsl_vector *diag, double pnorm, gsl_vector * w, gsl_vector * v);

static double scaled_enorm (const gsl_vector * d, const gsl_vector * f);

static double scaled_enorm (const gsl_vector * d, const gsl_vector * f) {
  double e2 = 0 ;
  size_t i, n = f->size ;
  for (i = 0; i < n ; i++) {
    double fi= gsl_vector_get(f, i);
    double di= gsl_vector_get(d, i);
    double u = di * fi;
    e2 += u * u ;
  }
  return sqrt(e2);
}

static double enorm_sum (const gsl_vector * a, const gsl_vector * b);

static double enorm_sum (const gsl_vector * a, const gsl_vector * b) {
  double e2 = 0 ;
  size_t i, n = a->size ;
  for (i = 0; i < n ; i++) {
    double ai= gsl_vector_get(a, i);
    double bi= gsl_vector_get(b, i);
    double u = ai + bi;
    e2 += u * u ;
  }
  return sqrt(e2);
}

static void
compute_wv (const gsl_vector * qtdf, const gsl_vector *rdx, const gsl_vector *dx, const gsl_vector *diag, double pnorm, gsl_vector * w, gsl_vector * v)
{
  size_t i, n = qtdf->size;

  for (i = 0; i < n; i++)
    {
      double qtdfi = gsl_vector_get (qtdf, i);
      double rdxi = gsl_vector_get (rdx, i);
      double dxi = gsl_vector_get (dx, i);
      double diagi = gsl_vector_get (diag, i);

      gsl_vector_set (w, i, (qtdfi - rdxi) / pnorm);
      gsl_vector_set (v, i, diagi * diagi * dxi / pnorm);
    }
}


static void
compute_df (const gsl_vector * f_trial, const gsl_vector * f, gsl_vector * df)
{
  size_t i, n = f->size;

  for (i = 0; i < n; i++)
    {
      double dfi = gsl_vector_get (f_trial, i) - gsl_vector_get (f, i);
      gsl_vector_set (df, i, dfi);
    }
}

static void
compute_diag (const gsl_matrix * J, gsl_vector * diag)
{
  size_t i, j, n = diag->size;

  for (j = 0; j < n; j++)
    {
      double sum = 0;
      for (i = 0; i < n; i++)
        {
          double Jij = gsl_matrix_get (J, i, j);
          sum += Jij * Jij;
        }
      if (sum == 0)
        sum = 1.0;

      gsl_vector_set (diag, j, sqrt (sum));
    }
}

static void
update_diag (const gsl_matrix * J, gsl_vector * diag)
{
  size_t i, j, n = diag->size;

  for (j = 0; j < n; j++)
    {
      double cnorm, diagj, sum = 0;
      for (i = 0; i < n; i++)
        {
          double Jij = gsl_matrix_get (J, i, j);
          sum += Jij * Jij;
        }
      if (sum == 0)
        sum = 1.0;

      cnorm = sqrt (sum);
      diagj = gsl_vector_get (diag, j);

      if (cnorm > diagj)
        gsl_vector_set (diag, j, cnorm);
    }
}

static double
compute_delta (gsl_vector * diag, gsl_vector * x)
{
  double Dx = scaled_enorm (diag, x);
  double factor = 100;

  return (Dx > 0) ? factor * Dx : factor;
}

static double
compute_actual_reduction (double fnorm, double fnorm1)
{
  double actred;

  if (fnorm1 < fnorm)
    {
      double u = fnorm1 / fnorm;
      actred = 1 - u * u;
    }
  else
    {
      actred = -1;
    }

  return actred;
}

static double
compute_predicted_reduction (double fnorm, double fnorm1)
{
  double prered;

  if (fnorm1 < fnorm)
    {
      double u = fnorm1 / fnorm;
      prered = 1 - u * u;
    }
  else
    {
      prered = 0;
    }

  return prered;
}

static void 
compute_qtf (const gsl_matrix * q, const gsl_vector * f, gsl_vector * qtf)
{
  size_t i, j, N = f->size ;

  for (j = 0; j < N; j++)
    {
      double sum = 0;
      for (i = 0; i < N; i++)
        sum += gsl_matrix_get (q, i, j) * gsl_vector_get (f, i);

      gsl_vector_set (qtf, j, sum);
    }
}

static void 
compute_rdx (const gsl_matrix * r, const gsl_vector * dx, gsl_vector * rdx)
{
  size_t i, j, N = dx->size ;

  for (i = 0; i < N; i++)
    {
      double sum = 0;

      for (j = i; j < N; j++)
        {
          sum += gsl_matrix_get (r, i, j) * gsl_vector_get (dx, j);
        }

      gsl_vector_set (rdx, i, sum);
    }
}


static void
compute_trial_step (gsl_vector *x, gsl_vector * dx, gsl_vector * x_trial)
{
  size_t i, N = x->size;

  for (i = 0; i < N; i++)
    {
      double pi = gsl_vector_get (dx, i);
      double xi = gsl_vector_get (x, i);
      gsl_vector_set (x_trial, i, xi + pi);
    }
}

static int
newton_direction (const gsl_matrix * r, const gsl_vector * qtf, gsl_vector * p)
{
  const size_t N = r->size2;
  size_t i;
  int status;

  status = gsl_linalg_R_solve (r, qtf, p);

#ifdef DEBUG
  printf("rsolve status = %d\n", status);
#endif

  for (i = 0; i < N; i++)
    {
      double pi = gsl_vector_get (p, i);
      gsl_vector_set (p, i, -pi);
    }

  return status;
}

static void
gradient_direction (const gsl_matrix * r, const gsl_vector * qtf,
                    const gsl_vector * diag, gsl_vector * g)
{
  const size_t M = r->size1;
  const size_t N = r->size2;

  size_t i, j;

  for (j = 0; j < M; j++)
    {
      double sum = 0;
      double dj;

      for (i = 0; i < N; i++)
        {
          sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
        }

      dj = gsl_vector_get (diag, j);
      gsl_vector_set (g, j, -sum / dj);
    }
}

static void
minimum_step (double gnorm, const gsl_vector * diag, gsl_vector * g)
{
  const size_t N = g->size;
  size_t i;

  for (i = 0; i < N; i++)
    {
      double gi = gsl_vector_get (g, i);
      double di = gsl_vector_get (diag, i);
      gsl_vector_set (g, i, (gi / gnorm) / di);
    }
}

static void
compute_Rg (const gsl_matrix * r, const gsl_vector * gradient, gsl_vector * Rg)
{
  const size_t N = r->size2;
  size_t i, j;

  for (i = 0; i < N; i++)
    {
      double sum = 0;

      for (j = i; j < N; j++)
        {
          double gj = gsl_vector_get (gradient, j);
          double rij = gsl_matrix_get (r, i, j);
          sum += rij * gj;
        }

      gsl_vector_set (Rg, i, sum);
    }
}

static void
scaled_addition (double alpha, gsl_vector * newton, double beta, gsl_vector * gradient, gsl_vector * p)
{
  const size_t N = p->size;
  size_t i;

  for (i = 0; i < N; i++)
    {
      double ni = gsl_vector_get (newton, i);
      double gi = gsl_vector_get (gradient, i);
      gsl_vector_set (p, i, alpha * ni + beta * gi);
    }
}

static int
dogleg (const gsl_matrix * r, const gsl_vector * qtf,
        const gsl_vector * diag, double delta,
        gsl_vector * newton, gsl_vector * gradient, gsl_vector * p)
{
  double qnorm, gnorm, sgnorm, bnorm, temp;

  newton_direction (r, qtf, newton);

#ifdef DEBUG
  printf("newton = "); gsl_vector_fprintf(stdout, newton, "%g"); printf("\n");
#endif

  qnorm = scaled_enorm (diag, newton);

  if (qnorm <= delta)
    {
      gsl_vector_memcpy (p, newton);
#ifdef DEBUG
      printf("took newton (qnorm = %g  <=   delta = %g)\n", qnorm, delta);
#endif
      return GSL_SUCCESS;
    }

  gradient_direction (r, qtf, diag, gradient);

#ifdef DEBUG
  printf("grad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
#endif

  gnorm = enorm (gradient);

  if (gnorm == 0)
    {
      double alpha = delta / qnorm;
      double beta = 0;
      scaled_addition (alpha, newton, beta, gradient, p);
#ifdef DEBUG
      printf("took scaled newton because gnorm = 0\n");
#endif
      return GSL_SUCCESS;
    }

  minimum_step (gnorm, diag, gradient);

  compute_Rg (r, gradient, p);  /* Use p as temporary space to compute Rg */

#ifdef DEBUG
  printf("mingrad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
  printf("Rg = "); gsl_vector_fprintf(stdout, p, "%g"); printf("\n");
#endif

  temp = enorm (p);
  sgnorm = (gnorm / temp) / temp;

  if (sgnorm > delta)
    {
      double alpha = 0;
      double beta = delta;
      scaled_addition (alpha, newton, beta, gradient, p);
#ifdef DEBUG
      printf("took gradient\n");
#endif
      return GSL_SUCCESS;
    }

  bnorm = enorm (qtf);

  {
    double bg = bnorm / gnorm;
    double bq = bnorm / qnorm;
    double dq = delta / qnorm;
    double dq2 = dq * dq;
    double sd = sgnorm / delta;
    double sd2 = sd * sd;

    double t1 = bg * bq * sd;
    double u = t1 - dq;
    double t2 = t1 - dq * sd2 + sqrt (u * u + (1-dq2) * (1 - sd2));

    double alpha = dq * (1 - sd2) / t2;
    double beta = (1 - alpha) * sgnorm;

#ifdef DEBUG
    printf("bnorm = %g\n", bnorm);
    printf("gnorm = %g\n", gnorm);
    printf("qnorm = %g\n", qnorm);
    printf("delta = %g\n", delta);
    printf("alpha = %g   beta = %g\n", alpha, beta);
    printf("took scaled combination of newton and gradient\n");
#endif

    scaled_addition (alpha, newton, beta, gradient, p);
  }

  return GSL_SUCCESS;
}