/*$Id: cg_pr.c 1.21 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_PR(TAO_SOLVER tao, void *solver)
{
  TAO_CG *cg = (TAO_CG *) solver;
  TaoVec*    X,*G; /* solution vector,  gradient vector */
  TaoVec*    Gprev=cg->Gprev;
  TaoVec*    DX=cg->DX, *Work=cg->Work;
  int    iter=0,lsflag=0,info;
  double gnorm2Prev,gdx,gDotgprev,f,gnorm,gnorm2,step=0;
  TaoTerminateReason reason;

  TaoFunctionBegin;
  info=TaoGetSolution(tao,&X);CHKERRQ(info);
  info=TaoGetGradient(tao,&G);CHKERRQ(info);

  info = TaoComputeMeritFunctionGradient(tao,X,&f,G);CHKERRQ(info);
  info = G->Norm2(&gnorm);  CHKERRQ(info);

  info = DX->SetToZero(); CHKERRQ(info); 
  info = Gprev->CopyFrom(G); 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;

    info = G->Dot(Gprev,&gDotgprev); CHKERRQ(info);
    cg->beta=( gnorm2-gDotgprev )/(gnorm2Prev);
    info = DX->Axpby(-1.0,G,cg->beta); CHKERRQ(info);
    
    info = DX->Dot(G,&gdx); CHKERRQ(info);
    if (gdx>=0){ /* If not a descent direction, use gradient */
      info = DX->ScaleCopyFrom(-1.0,G); 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);
    } 

    if ( fabs(gDotgprev)/(gnorm2) > cg->eta){ 
      /* The gradients are somewhat orthogonal */
      gdx=-gnorm2;
      cg->beta=0;
      info = DX->Axpby(-1.0,G,0.0); CHKERRQ(info);
      info = PetscLogInfo((tao,"TaoCG: Restart CG at iterate %d with gradient direction.\n",iter)); CHKERRQ(info);
    }

    /* Line Search */ 
    gnorm2Prev = gnorm2;  step=1.0;
    info = Gprev->CopyFrom(G); CHKERRQ(info);
    info = TaoLineSearchApply(tao,X,G,DX,Work,&f,&step,&lsflag);CHKERRQ(info);
    info = G->Norm2(&gnorm);CHKERRQ(info);
    gnorm2=gnorm*gnorm; 
  }
  
  TaoFunctionReturn(0);
}

/* ---------------------------------------------------------- */
/*
    TAOCreate_CG_PR - 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_PR"
int TaoCreate_CG_PR(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_PR,(void*)cg); CHKERRQ(info);
  info=TaoSetTaoSetUpDownRoutines(tao,TaoSetUp_CG,TaoDestroy_CG); CHKERRQ(info);
  info=TaoSetTaoOptionsRoutine(tao,TaoSetOptions_CG); 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.4); CHKERRQ(info);

  cg->eta = 0.5;

  TaoFunctionReturn(0);
}
EXTERN_C_END

