/*$Id: cg_fr.c 1.22 05/05/10 15:21:38-05:00 sarich@zorak.(none) $*/

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

#undef __FUNCT__  
#define __FUNCT__ "TaoSolve_CG_FR"
static int TaoSolve_CG_FR(TAO_SOLVER tao, void *solver)
{
  TAO_CG *cg = (TAO_CG *) solver;
  TaoVec  *xx,*gg; /* solution vector,  gradient vector */
  TaoVec*  dx=cg->DX, *ww=cg->Work;
  int    iter=0,lsflag=0,info;
  double gnorm2Prev,gdx,f,gnorm,gnorm2,step=0;
  TaoTerminateReason reason;

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

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

  info = dx->SetToZero(); CHKERRQ(info); 

  cg->beta=0; cg->restarts=0;
  gnorm2 = gnorm*gnorm; gnorm2Prev = gnorm2;

  /* Enter loop */
  while (1){

    /* Test for convergence */
    info = TaoMonitor(tao,iter++,f,gnorm,0.0,step,&reason);CHKERRQ(info);
    if (reason!=TAO_CONTINUE_ITERATING) break;

    cg->beta=(gnorm2)/(gnorm2Prev);
    info = dx->Axpby(-1.0,gg,cg->beta); CHKERRQ(info);
    
    info = dx->Dot(gg,&gdx); CHKERRQ(info);
    if (gdx>=0){ /* If not a descent direction, use gradient */
      info = dx->ScaleCopyFrom(-1.0,gg); CHKERRQ(info);
      gdx=-gnorm2;
      cg->restarts++; cg->beta=0.0;
      info = PetscLogInfo((tao,"TaoCG: Not Descent Direction: Restart CG at iterate %d with gradient direction.\n",iter)); CHKERRQ(info);
    } 

    /* Line Search */
    gnorm2Prev = gnorm2;  step=1.0;
    info = TaoLineSearchApply(tao,xx,gg,dx,ww,&f,&step,&lsflag);CHKERRQ(info);
    info = gg->Norm2(&gnorm);CHKERRQ(info);
    gnorm2 = gnorm*gnorm;

  }
  
  TaoFunctionReturn(0);
}


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

  TaoFunctionBegin;

  info = TaoOptionsHead("Nonlinear Conjugate Gradient method for unconstrained optimization");CHKERRQ(info);
  info = TaoLineSearchSetFromOptions(tao);CHKERRQ(info);
  info = TaoOptionsTail();CHKERRQ(info);

  TaoFunctionReturn(0);
}


/* ---------------------------------------------------------- */
/*
    TAOCreate_CGFR - Creates the data structure for the nonlinear CG method
    and sets the function pointers for all the routines it needs to call
    (TAOSolve_CG() etc.)

    It must be wrapped in EXTERN_C_BEGIN to be dynamically linkable in C++
*/
EXTERN_C_BEGIN
#undef __FUNCT__  
#define __FUNCT__ "TaoCreate_CG_FR"
int TaoCreate_CG_FR(TAO_SOLVER tao)
{
  TAO_CG *cg;
  int    info;

  TaoFunctionBegin;

  info = TaoNew(TAO_CG,&cg); CHKERRQ(info);
  info = PetscLogObjectMemory(tao,sizeof(TAO_CG)); CHKERRQ(info);

  info=TaoSetTaoSolveRoutine(tao,TaoSolve_CG_FR,(void*)cg); CHKERRQ(info);
  info=TaoSetTaoSetUpDownRoutines(tao,TaoSetUp_CG,TaoDestroy_CG); CHKERRQ(info);
  info=TaoSetTaoOptionsRoutine(tao,TaoSetOptions_CG_FR); CHKERRQ(info);
  info=TaoSetTaoViewRoutine(tao,TaoView_CG); CHKERRQ(info);

  info = TaoSetMaximumIterates(tao,2000); CHKERRQ(info);
  info = TaoSetTolerances(tao,1e-4,1e-4,0,0); CHKERRQ(info);
  info = TaoSetMaximumFunctionEvaluations(tao,4000); CHKERRQ(info);

  info = TaoCreateMoreThuenteLineSearch(tao,0,0.1); CHKERRQ(info);

  cg->eta = 1000.0;
  
  TaoFunctionReturn(0);
}
EXTERN_C_END

