/*$Id: s.nls.c 1.106 04/03/03 13:56:23-06:00 benson@rockies.mcs.anl.gov $*/

#include "nlmvm.h"             /*I "tao_solver.h" I*/

static int TaoDestroy_NLMVM(TAO_SOLVER tao, void *solver);
static int TaoView_NLMVM(TAO_SOLVER tao,void* solver);
static int TaoSolve_NLMVM(TAO_SOLVER tao,void *solver);
static int TaoSetOptions_NLMVM(TAO_SOLVER tao, void *solver);
static int TaoSetUp_NLMVM(TAO_SOLVER tao, void *solver);

/*
   Implements Newton's Method with a line search approach
   for solving unconstrained minimization problems.

   Note:
   The line search algorithm is taken from More and Thuente,
   "Line search algorithms with guaranteed sufficient decrease",
   Argonne National Laboratory, Technical Report MCS-P330-1092.
*/

class TaoH00Mat: public TaoMat{
 protected:
  TAO_SOLVER tao;
 public:  
  TaoH00Mat(TAO_SOLVER);
  ~TaoH00Mat();
  int Solve(TaoVec*, TaoVec*, TaoTruth*);
};

#undef __FUNCT__
#define __FUNCT__ "TaoH00Mat::TaoH00Mat"
TaoH00Mat::TaoH00Mat(TAO_SOLVER ttao){
  this->tao=ttao;
  return;
} 

#undef __FUNCT__
#define __FUNCT__ "TaoH00Mat::~TaoH00Mat"
TaoH00Mat::~TaoH00Mat(){
  return;
}

#undef __FUNCT__
#define __FUNCT__ "TaoH00Mat::Solve"
int TaoH00Mat::Solve(TaoVec* rhs, TaoVec* dx, TaoTruth *success){
  int info;
  TaoMat *HH;
  info=TaoGetHessian(this->tao,&HH);CHKERRQ(info);
  info=TaoLinearSolve(this->tao,HH,rhs,dx,success);CHKERRQ(info);
  return 0;
}

#undef __FUNCT__  
#define __FUNCT__ "TaoSolve_NLMVM"
static int TaoSolve_NLMVM(TAO_SOLVER tao,void *solver)
{
  TAO_NLMVM            *neP = (TAO_NLMVM *) solver;
  int                ii,iter=0, info, line=0;
  double             r0,f,dxnorm,gdx, gnorm, two = 2.0,step=0.0;
  TaoVec             *gg, *xx;
  TaoVec             *rrhs=neP->RHS, *ss=neP->S, *ww=neP->W;
  TaoMat             *HH;
  TaoTerminateReason reason;
  TaoTruth           success;
  double             gamma,gamma_factor;
  TaoH00Mat *HH0;
  TaoFunctionBegin;

  info=TaoGetSolution(tao,&xx);CHKERRQ(info);
  info=TaoGetGradient(tao,&gg);CHKERRQ(info);
  info=TaoGetHessian(tao,&HH);CHKERRQ(info);

  HH0=new TaoH00Mat(tao);
  info = neP->M->SetH0( HH0 );CHKERRQ(info);

  info = TaoComputeMeritFunctionGradient(tao,xx,&f,gg);CHKERRQ(info);
  info = gg->Norm2(&gnorm);CHKERRQ(info);         /* gnorm = || gg || */

  gamma       = neP->gamma;
  gamma_factor= neP->gamma_factor;
  
  while (1) {
    
    info = TaoMonitor(tao,iter++,f,gnorm,0.0,step,&reason);CHKERRQ(info);
    if (reason!=TAO_CONTINUE_ITERATING) break;

    gamma_factor /= two;
    gamma = gamma_factor*(gnorm);

    info = rrhs->Axpby(-1.0,gg,0.0); CHKERRQ(info);

    info = TaoComputeHessian(tao,xx,HH);CHKERRQ(info);

    info = neP->M->reset(); CHKERRQ(info);
    info = neP->M->update(xx,gg); CHKERRQ(info);

    success = TAO_FALSE;
    while (success==TAO_FALSE) {
      info = TaoPreLinearSolve(tao,HH);CHKERRQ(info);
      info = TaoLinearSolve(tao,HH,rrhs,ss,&success);CHKERRQ(info);

      info = ss->Dot(gg,&gdx); CHKERRQ(info);

      if (gdx>=0 /*|| success==TAO_FALSE*/) { /* Modify diagonal of Hessian */
	gamma_factor *= two; 
        gamma = gamma_factor*(gnorm); 
#if !defined(PETSC_USE_COMPLEX)
        info = PetscLogInfo((tao,"TaoSolve_NLMVM:  modify diagonal (assume same nonzero structure), gamma_factor=%g, gamma=%g\n",
			     gamma_factor,gamma)); CHKERRQ(info);
#else
        info = PetscLogInfo((tao,"TaoSolve_NLMVM:  modify diagonal (assume same nonzero structure), gamma_factor=%g, gamma=%g\n",
			     gamma_factor,PetscReal(gamma))); CHKERRQ(info);
#endif
        info = HH->ShiftDiagonal(gamma);CHKERRQ(info);
	success = TAO_FALSE;

        /* We currently assume that all diagonal elements were allocated in
	   original matrix, so that nonzero pattern is same ... should fix this */
      } else {
	//        gamma_factor /= two; 
	success = TAO_TRUE;
      }
    }   

    /* Line search */
    step=1.0;
    info = TaoLineSearchApply(tao,xx,gg,ss,ww,&f,&step,&line);
    CHKERRQ(info);
    info = gg->Norm2(&gnorm);CHKERRQ(info);

    for (ii=0;ii<neP->lmvmits;ii++){
      /* Test for convergence */
      info=TaoMonitor(tao,iter,f,gnorm,0,step,&reason);CHKERRQ(info);
      if (reason!=TAO_CONTINUE_ITERATING) break;
      
      /* Compute step direction */
      info = neP->M->update(xx,gg); CHKERRQ(info);
      info = neP->M->Solve(gg,ss,&success); CHKERRQ(info);
      info = ss->Negate(); CHKERRQ(info);
      info = ss->Dot(gg,&gdx); CHKERRQ(info);
      if (gdx>=0){
	info = ss->ScaleCopyFrom(-1.0,gg); CHKERRQ(info);
	gdx = -gnorm*gnorm;
      }
      
      /* Line Search */
      info = ss->Norm2(&dxnorm);  CHKERRQ(info);
      info = TaoGetInitialTrustRegionRadius(tao,&r0); CHKERRQ(info);
      step = TaoMin(1.0,r0/dxnorm);
      info = TaoLineSearchApply(tao,xx,gg,ss,ww,&f,&step,&line);
      CHKERRQ(info);
      info = gg->Norm2(&gnorm);CHKERRQ(info);
    }
    
    //    neP->gamma=gamma;
    //.    neP->gamma_factor=gamma_factor;

  }

  delete HH0;
  TaoFunctionReturn(0);
}
/* ---------------------------------------------------------- */
#undef __FUNCT__  
#define __FUNCT__ "TaoSetUp_NLMVM"
static int TaoSetUp_NLMVM(TAO_SOLVER tao, void *solver)
{
  int        info,kspmaxit,lm,n;
  TAO_NLMVM *ctx = (TAO_NLMVM *)solver;
  TaoVec *xx;
  TaoMat *HH;
  TaoLinearSolver *tls;

  TaoFunctionBegin;
  info = TaoGetSolution(tao,&xx);CHKERRQ(info);

  info=xx->Clone(&ctx->RHS);CHKERRQ(info);
  info=xx->Clone(&ctx->S);CHKERRQ(info);
  info=xx->Clone(&ctx->W);CHKERRQ(info);
  info=xx->Clone(&ctx->G);CHKERRQ(info);

  info = TaoSetLagrangianGradientVector(tao,ctx->G);CHKERRQ(info);
  info = TaoSetStepDirectionVector(tao,ctx->S);CHKERRQ(info);

  info = TaoLMVMGetSize(tao,&lm); CHKERRQ(info);
  ctx->M = new TaoLMVMMat(xx,TaoMin(lm,ctx->lmvmits));

  info = TaoGetHessian(tao,&HH); CHKERRQ(info);
  info = TaoCreateLinearSolver(tao,HH,200,0); CHKERRQ(info);
  info = TaoGetLinearSolver(tao,&tls); CHKERRQ(info); 
  info = xx->GetDimension(&n);CHKERRQ(info);
  kspmaxit = ctx->max_kspiter_factor * ((int) sqrt((double)n));
  info = tls->SetTolerances(1e-5,1e-40,1e30,kspmaxit);CHKERRQ(info);

  info = TaoCheckFGH(tao);CHKERRQ(info);

  TaoFunctionReturn(0);
}
/*------------------------------------------------------------*/
#undef __FUNCT__  
#define __FUNCT__ "TaoDestroy_NLMVM"
static int TaoDestroy_NLMVM(TAO_SOLVER tao, void *solver)
{
  TAO_NLMVM *ctx = (TAO_NLMVM *)solver;
  int     info;

  TaoFunctionBegin;

  info=TaoVecDestroy(ctx->RHS);CHKERRQ(info);
  info=TaoVecDestroy(ctx->S);CHKERRQ(info);
  info=TaoVecDestroy(ctx->W);CHKERRQ(info);
  info=TaoVecDestroy(ctx->G);CHKERRQ(info);
  info=TaoMatDestroy(ctx->M);CHKERRQ(info);
  info = TaoDestroyLinearSolver(tao);CHKERRQ(info);

  TaoFunctionReturn(0);
}
/*------------------------------------------------------------*/
#undef __FUNCT__  
#define __FUNCT__ "TaoSetOptions_NLMVM"
static int TaoSetOptions_NLMVM(TAO_SOLVER tao, void *solver)
{
  TAO_NLMVM    *ctx = (TAO_NLMVM *)solver;
  int        info;
  TaoTruth   flg;

  TaoFunctionBegin;
  info = TaoOptionsHead("Newton line search method for unconstrained optimization");CHKERRQ(info);

  info = TaoOptionDouble("-tao_nls_gamma_factor","damping parameter","",ctx->gamma_factor,&ctx->gamma_factor,&flg);CHKERRQ(info);

  info = TaoOptionInt("-tao_nls_maxkspf","max ksp iterates","",ctx->max_kspiter_factor,&ctx->max_kspiter_factor,&flg);CHKERRQ(info);

  info = TaoOptionInt("-tao_nlmvmits","LMVM updates per iteration","",ctx->lmvmits,&ctx->lmvmits,&flg);CHKERRQ(info);

  info = TaoLineSearchSetFromOptions(tao);CHKERRQ(info);
  /*  info = TaoSetLinearSolverOptions(tao);CHKERRQ(info); */

  info = TaoOptionsTail();CHKERRQ(info);

  TaoFunctionReturn(0);
}


/*------------------------------------------------------------*/
#undef __FUNCT__  
#define __FUNCT__ "TaoView_NLMVM"
static int TaoView_NLMVM(TAO_SOLVER tao,void* solver)
{
  TAO_NLMVM *ctx = (TAO_NLMVM *)solver;
  int     info;

  TaoFunctionBegin;

  info = TaoPrintDouble(tao,"  gamma_f=%g,",ctx->gamma_factor);CHKERRQ(info);
  info = TaoPrintInt(tao," maxkspf=%d\n",ctx->max_kspiter_factor);CHKERRQ(info);

  info = TaoPrintStatement(tao,"  Linear Solver for symmetric matrix: \n");CHKERRQ(info);
  info = TaoLineSearchView(tao);CHKERRQ(info);

  TaoFunctionReturn(0);
}

/* ---------------------------------------------------------- */
EXTERN_C_BEGIN
#undef __FUNCT__  
#define __FUNCT__ "TaoCreate_NLMVM"
int TaoCreate_NLMVM(TAO_SOLVER tao)
{
  TAO_NLMVM         *neP;
  int             info;

  TaoFunctionBegin;

  info = TaoNew(TAO_NLMVM,&neP); CHKERRQ(info);
  info = PetscLogObjectMemory(tao,sizeof(TAO_NLMVM)); CHKERRQ(info);

  info=TaoSetTaoSolveRoutine(tao,TaoSolve_NLMVM,(void*)neP); CHKERRQ(info);
  info=TaoSetTaoSetUpDownRoutines(tao,TaoSetUp_NLMVM,TaoDestroy_NLMVM); CHKERRQ(info);
  info=TaoSetTaoOptionsRoutine(tao,TaoSetOptions_NLMVM); CHKERRQ(info);
  info=TaoSetTaoViewRoutine(tao,TaoView_NLMVM); CHKERRQ(info);

  info = TaoSetMaximumIterates(tao,50); CHKERRQ(info);
  info = TaoSetTolerances(tao,1e-16,1e-16,0,0); CHKERRQ(info);
  info = TaoLMVMSetSize(tao,5); CHKERRQ(info);
  info = TaoSetTrustRegionRadius(tao,1.0e16); CHKERRQ(info);

  neP->gamma		        = 0.0;
  neP->gamma_factor	        = 0.01;
  neP->max_kspiter_factor       = 5;
  neP->lmvmits                  = 2;

  info = TaoCreateMoreThuenteLineSearch(tao,0,0); CHKERRQ(info);
  /*
  info = PetscObjectComposeFunctionDynamic((PetscObject)tao,
				    "TaoLineSearchGetDampingParameter_C",
				    "TaoLineSearchGetDampingParameter_NLMVM",
				    (void*)TaoLineSearchGetDampingParameter_NLMVM);
  CHKERRQ(info);
  */
  TaoFunctionReturn(0);
}
EXTERN_C_END

EXTERN_C_BEGIN
#undef __FUNCT__  
#define __FUNCT__ "TaoLineSearchGetDampingParameter_NLMVM"
int TaoLineSearchGetDampingParameter_NLMVM(TAO_SOLVER tao,double *damp)
{
  TAO_NLMVM *neP;
  int info;

  TaoFunctionBegin;
  info = TaoGetSolverContext(tao,"tao_nlmvm",(void**)&neP); CHKERRQ(info);
  if (neP){
    *damp = neP->gamma;
  }
  TaoFunctionReturn(0);
}
EXTERN_C_END

