#ifdef PETSC_RCS_HEADER
static char vcid[] = "$Id: mlSNES.c,v 1.18 2001/01/27 21:32:36 knepley Exp $";
#endif

#include "petscsnes.h"
#include "gvec.h"

#undef __FUNCT__
#define __FUNCT__ "PCMultiLevelCubicLineSearch"
/*@C
  PCMultiLevelCubicLineSearch - This function performs a cubic line search, but
  the function evaluations are P^T f so that the residual is computed on the
  constrained manifold.

  Input Parameters:
+ snes  - The SNES
. ctx   - The optional user context
. x     - The current iterate
. f     - The residual evaluated at x (contains projected residual on output)
. y     - The search direction (contains new iterate on output)
. w     - A work vector
- fnorm - The 2-norm of f

  Output Parameters:
+ f     - The projected residual P^T g evaluated at new iterate y
. g     - The residual evaluated at new iterate y
. y     - The new iterate (contains search direction on input)
. gnorm - The 2-norm of P^T g
. ynorm - The 2-norm of y, the search length
- flag  - 0 if line search succeeds; -1 on failure.

  Level: advanced

  Notes:
  This line search is taken from "Numerical Methods for Unconstrained
  Optimization and Nonlinear Equations" by Dennis and Schnabel, page 325.

.keywords: SNES, nonlinear, line search, cubic

.seealso: SNESNoLineSearch(), SNESNoLineSearch(), SNESSetLineSearch()
@*/
int PCMultiLevelCubicLineSearch(SNES snes, void *ctx, Vec x, Vec f, Vec g, Vec y, Vec w, PetscReal fnorm, PetscReal *ynorm, PetscReal *gnorm, int *flag)
{
  /*
    Note that for line search purposes we work with with the related
    minimization problem:
      min  z(x):  R^n -> R,
    where z(x) = .5 * fnorm*fnorm, and fnorm = || f ||_2.
   */

  SLES        sles;
  PC          pc;
  Mat         jac;
  PetscReal   abstol, steptol, initslope, lambdaprev, gnormprev, a, b, d, t1, t2;
  PetscReal   maxstep, minlambda, alpha, lambda, lambdatemp, lambdaneg;
#if defined(PETSC_USE_COMPLEX)
  PetscScalar cinitslope, clambda;
#endif
  PetscScalar scale;
  PetscScalar mone = -1.0;
  int         count;
  int         ierr;

  PetscFunctionBegin;
  ierr = PetscLogEventBegin(SNES_LineSearch, snes, x, f, g);                                               CHKERRQ(ierr);
  ierr = SNESGetTolerances(snes, &abstol, PETSC_NULL, PETSC_NULL, PETSC_NULL, PETSC_NULL);                CHKERRQ(ierr);
  ierr = SNESGetJacobian(snes, &jac, PETSC_NULL, PETSC_NULL, PETSC_NULL);                                 CHKERRQ(ierr);
  ierr = SNESGetLineSearchParams(snes, &alpha, &maxstep, &steptol);                                       CHKERRQ(ierr);
  ierr = SNESGetSLES(snes, &sles);                                                                        CHKERRQ(ierr);
  ierr = SLESGetPC(sles, &pc);                                                                            CHKERRQ(ierr);
  *flag = 0;

  /* First we must recover the true solution */
  ierr = PCMultiLevelBuildSolution(pc, y);                                                                CHKERRQ(ierr);

  ierr = VecNorm(y, NORM_2, ynorm);                                                                       CHKERRQ(ierr);
  if (*ynorm < abstol) {
    PetscLogInfo(snes, "PCMultiLevelCubicLineSearch: Search direction and size are nearly 0\n");
    *gnorm = fnorm;
    ierr = VecCopy(x, y);                                                                                 CHKERRQ(ierr);
    ierr = VecCopy(f, g);                                                                                 CHKERRQ(ierr);
    ierr = PCApplySymmetricLeft(pc, g, f);                                                                CHKERRQ(ierr);
    goto theend1;
  }
  if (*ynorm > maxstep) {
    /* Step too big, so scale back */
    scale  = maxstep/(*ynorm);
#if defined(PETSC_USE_COMPLEX)
    PetscLogInfo(snes, "PCMultiLevelCubicLineSearch: Scaling step by %g\n", real(scale));
#else
    PetscLogInfo(snes, "PCMultiLevelCubicLineSearch: Scaling step by %g\n", scale);
#endif
    ierr = VecScale(&scale, y);                                                                           CHKERRQ(ierr);
    *ynorm = maxstep;
  }
  minlambda = steptol/(*ynorm);
  ierr = MatMult(jac, y, g);                                                                              CHKERRQ(ierr);
  ierr = PCApplySymmetricLeft(pc, g, w);                                                                  CHKERRQ(ierr);
  ierr = PCApplySymmetricLeft(pc, f, g);                                                                  CHKERRQ(ierr);
#if defined(USE_PETSC_COMPLEX)
  ierr = VecDot(g, w, &cinitslope);                                                                       CHKERRQ(ierr);
  initslope = real(cinitslope);
#else
  ierr = VecDot(g, w, &initslope);                                                                        CHKERRQ(ierr);
#endif
  if (initslope > 0.0)  initslope = -initslope;
  if (initslope == 0.0) initslope = -1.0;

  ierr = VecCopy(y, w);                                                                                   CHKERRQ(ierr);
  ierr = VecAYPX(&mone, x, w);                                                                            CHKERRQ(ierr);
  ierr = SNESComputeFunction(snes, w, g);                                                                 CHKERRQ(ierr);
  ierr = PCApplySymmetricLeft(pc, g, f);                                                                  CHKERRQ(ierr);
  ierr = VecNorm(f, NORM_2, gnorm);
  if ((*gnorm)*(*gnorm)*0.5 <= fnorm*fnorm*0.5 + alpha*initslope) {
    /* Sufficient reduction */
    ierr = VecCopy(w, y);                                                                                 CHKERRQ(ierr);
    PetscLogInfo(snes, "PCMultiLevelCubicLineSearch: Using full step\n");
    goto theend1;
  }

  /* Fit points with quadratic */
  count      = 0;
  lambda     = 1.0;
  lambdatemp = -initslope/((*gnorm)*(*gnorm) - fnorm*fnorm - 2.0*initslope);
  lambdaprev = lambda;
  gnormprev  = *gnorm;
  if (lambdatemp <= 0.1*lambda) {
    lambda   = 0.1*lambda;
  } else {
    lambda   = lambdatemp;
  }
  ierr = VecCopy(x, w);                                                                                   CHKERRQ(ierr);
  lambdaneg  = -lambda;
#if defined(PETSC_USE_COMPLEX)
  clambda    = lambdaneg;
  ierr = VecAXPY(&clambda, y, w);                                                                         CHKERRQ(ierr);
#else
  ierr = VecAXPY(&lambdaneg, y, w);                                                                       CHKERRQ(ierr);
#endif
  ierr = SNESComputeFunction(snes, w, g);                                                                 CHKERRQ(ierr);
  ierr = PCApplySymmetricLeft(pc, g, f);                                                                  CHKERRQ(ierr);
  ierr = VecNorm(f, NORM_2, gnorm);                                                                       CHKERRQ(ierr);
  if ((*gnorm)*(*gnorm)*0.5 <= fnorm*fnorm*0.5 + alpha*initslope) {
    /* Sufficient reduction */
    ierr = VecCopy(w, y);                                                                                 CHKERRQ(ierr);
    PetscLogInfo(snes, "PCMultiLevelCubicLineSearch: Quadratically determined step, lambda=%g\n", lambda);
    goto theend1;
  }

  /* Fit points with cubic */
  count = 1;
  while (1) {
    if (lambda <= minlambda) {
      /* Bad luck; use full step */
      PetscLogInfo(snes, "PCMultiLevelCubicLineSearch:Unable to find good step length! %d \n",count);
      PetscLogInfo(snes, "PCMultiLevelCubicLineSearch:fnorm=%g, gnorm=%g, ynorm=%g, lambda=%g, initial slope=%g\n",
                   fnorm, *gnorm, *ynorm, lambda, initslope);
      ierr  = VecCopy(w, y);                                                                              CHKERRQ(ierr);
      *flag = -1;
      break;
    }
    t1 = ((*gnorm)*(*gnorm)    - fnorm*fnorm)*0.5 - lambda*initslope;
    t2 = (gnormprev*gnormprev  - fnorm*fnorm)*0.5 - lambdaprev*initslope;
    a  = (t1/(lambda*lambda) - t2/(lambdaprev*lambdaprev))/(lambda-lambdaprev);
    b  = (-lambdaprev*t1/(lambda*lambda) + lambda*t2/(lambdaprev*lambdaprev))/(lambda-lambdaprev);
    d  = b*b - 3*a*initslope;
    if (d < 0.0) d = 0.0;
    if (a == 0.0) {
      lambdatemp = -initslope/(2.0*b);
    } else {
      lambdatemp = (-b + sqrt(d))/(3.0*a);
    }
    if (lambdatemp > .5*lambda) lambdatemp = .5*lambda;
    lambdaprev = lambda;
    gnormprev  = *gnorm;
    if (lambdatemp <= .1*lambda) {
      lambda   = .1*lambda;
    } else {
      lambda   = lambdatemp;
    }
    ierr = VecCopy(x, w);                                                                                 CHKERRQ(ierr);
    lambdaneg  = -lambda;
#if defined(USE_PETSC_COMPLEX)
    clambda    = lambdaneg;
    ierr = VecAXPY(&clambda, y, w);                                                                       CHKERRQ(ierr);
#else
    ierr = VecAXPY(&lambdaneg, y, w);                                                                     CHKERRQ(ierr);
#endif
    ierr = SNESComputeFunction(snes, w, g);                                                               CHKERRQ(ierr);
    ierr = PCApplySymmetricLeft(pc, g, f);                                                                CHKERRQ(ierr);
    ierr = VecNorm(f, NORM_2, gnorm);                                                                     CHKERRQ(ierr);
    if ((*gnorm)*(*gnorm)*0.5 <= fnorm*fnorm*0.5 + alpha*initslope) {
      /* Sufficient reduction */
      ierr = VecCopy(w, y);                                                                               CHKERRQ(ierr);
      PetscLogInfo(snes, "PCMultiLevelCubicLineSearch: Cubically determined step, lambda=%g\n", lambda);
      goto theend1;
    } else {
      PetscLogInfo(snes, "PCMultiLevelCubicLineSearch: Cubic step no good, shrinking lambda, lambda=%g\n", lambda);
    }
    count++;
  }

  theend1:
  ierr = PetscLogEventEnd(SNES_LineSearch, snes, x, f, g);                                                 CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "PCMultiLevelUpdateSNES"
/*@C
  PCMultiLevelUpdateSNES - This function updates the nonlinear iterate for the PC
  context at each Newton step so that the appropriate system reduction may be
  performed.

  Collective on SNES

  Input Parameters:
+ snes - The SNES
- step - The Newton step

  Level: advanced

  Note:
  This is usually used internally to keep track of the current
  solution in an nonlinear iteration.

.keywords SNES, multilevel, update
.seealso PCMultiLevelCubicLineSearch
@*/
int PCMultiLevelUpdateSNES(SNES snes, int step)
{
  SLES  sles;
  PC    pc;
  GVec  sol;
  int   ierr;

  PetscFunctionBegin;
  PetscValidHeaderSpecific(snes, SNES_COOKIE);
  ierr = SNESGetSLES(snes, &sles);                                                                        CHKERRQ(ierr);
  ierr = SLESGetPC(sles, &pc);                                                                            CHKERRQ(ierr);
  ierr = SNESGetSolution(snes, &sol);                                                                     CHKERRQ(ierr);
  ierr = PCMultiLevelSetNonlinearIterate(pc, sol);                                                        CHKERRQ(ierr);
  PetscFunctionReturn(0);
}
