#include <../src/tao/unconstrained/impls/bmrm/bmrm.h>

static PetscErrorCode init_df_solver(TAO_DF*);
static PetscErrorCode ensure_df_space(PetscInt, TAO_DF*);
static PetscErrorCode destroy_df_solver(TAO_DF*);
static PetscReal phi(PetscReal*,PetscInt,PetscReal,PetscReal*,PetscReal,PetscReal*,PetscReal*,PetscReal*);
static PetscInt project(PetscInt,PetscReal*,PetscReal,PetscReal*,PetscReal*,PetscReal*,PetscReal*,PetscReal*,TAO_DF*);
static PetscErrorCode solve(TAO_DF*);


/*------------------------------------------------------------*/
/* The main solver function

   f = Remp(W)          This is what the user provides us from the application layer
   So the ComputeGradient function for instance should get us back the subgradient of Remp(W)

   Regularizer assumed to be L2 norm = lambda*0.5*W'W ()
*/

#undef __FUNCT__
#define __FUNCT__ "make_grad_node"
static PetscErrorCode make_grad_node(Vec X, Vec_Chain **p)
{
  PetscErrorCode ierr;

  PetscFunctionBegin;
  ierr = PetscNew(p);CHKERRQ(ierr);
  ierr = VecDuplicate(X, &(*p)->V);CHKERRQ(ierr);
  ierr = VecCopy(X, (*p)->V);CHKERRQ(ierr);
  (*p)->next = NULL;
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "destroy_grad_list"
static PetscErrorCode destroy_grad_list(Vec_Chain *head)
{
  PetscErrorCode ierr;
  Vec_Chain      *p = head->next, *q;

  PetscFunctionBegin;
  while(p) {
    q = p->next;
    ierr = VecDestroy(&p->V);CHKERRQ(ierr);
    ierr = PetscFree(p);CHKERRQ(ierr);
    p = q;
  }
  head->next = NULL;
  PetscFunctionReturn(0);
}


#undef __FUNCT__
#define __FUNCT__ "TaoSolve_BMRM"
static PetscErrorCode TaoSolve_BMRM(Tao tao)
{
  PetscErrorCode     ierr;
  TaoConvergedReason reason;
  TAO_DF             df;
  TAO_BMRM           *bmrm = (TAO_BMRM*)tao->data;

  /* Values and pointers to parts of the optimization problem */
  PetscReal          f = 0.0;
  Vec                W = tao->solution;
  Vec                G = tao->gradient;
  PetscReal          lambda;
  PetscReal          bt;
  Vec_Chain          grad_list, *tail_glist, *pgrad;
  PetscInt           i;
  PetscMPIInt        rank;

  /* Used in converged criteria check */
  PetscReal          reg;
  PetscReal          jtwt, max_jtwt, pre_epsilon, epsilon, jw, min_jw;
  PetscReal          innerSolverTol;
  MPI_Comm           comm;

  PetscFunctionBegin;
  ierr = PetscObjectGetComm((PetscObject)tao,&comm);CHKERRQ(ierr);
  ierr = MPI_Comm_rank(comm, &rank);CHKERRQ(ierr);
  lambda = bmrm->lambda;

  /* Check Stopping Condition */
  tao->step = 1.0;
  max_jtwt = -BMRM_INFTY;
  min_jw = BMRM_INFTY;
  innerSolverTol = 1.0;
  epsilon = 0.0;

  if (!rank) {
    ierr = init_df_solver(&df);CHKERRQ(ierr);
    grad_list.next = NULL;
    tail_glist = &grad_list;
  }

  df.tol = 1e-6;
  reason = TAO_CONTINUE_ITERATING;

  /*-----------------Algorithm Begins------------------------*/
  /* make the scatter */
  ierr = VecScatterCreateToZero(W, &bmrm->scatter, &bmrm->local_w);CHKERRQ(ierr);
  ierr = VecAssemblyBegin(bmrm->local_w);CHKERRQ(ierr);
  ierr = VecAssemblyEnd(bmrm->local_w);CHKERRQ(ierr);

  /* NOTE: In application pass the sub-gradient of Remp(W) */
  ierr = TaoComputeObjectiveAndGradient(tao, W, &f, G);CHKERRQ(ierr);
  ierr = TaoMonitor(tao,tao->niter,f,1.0,0.0,tao->step,&reason);CHKERRQ(ierr);
  while (reason == TAO_CONTINUE_ITERATING) {
    /* compute bt = Remp(Wt-1) - <Wt-1, At> */
    ierr = VecDot(W, G, &bt);CHKERRQ(ierr);
    bt = f - bt;

    /* First gather the gradient to the master node */
    ierr = VecScatterBegin(bmrm->scatter, G, bmrm->local_w, INSERT_VALUES, SCATTER_FORWARD);CHKERRQ(ierr);
    ierr = VecScatterEnd(bmrm->scatter, G, bmrm->local_w, INSERT_VALUES, SCATTER_FORWARD);CHKERRQ(ierr);

    /* Bring up the inner solver */
    if (!rank) {
      ierr = ensure_df_space(tao->niter+1, &df); CHKERRQ(ierr);
      ierr = make_grad_node(bmrm->local_w, &pgrad);CHKERRQ(ierr);
      tail_glist->next = pgrad;
      tail_glist = pgrad;

      df.a[tao->niter] = 1.0;
      df.f[tao->niter] = -bt;
      df.u[tao->niter] = 1.0;
      df.l[tao->niter] = 0.0;

      /* set up the Q */
      pgrad = grad_list.next;
      for (i=0; i<=tao->niter; i++) {
        ierr = VecDot(pgrad->V, bmrm->local_w, &reg);CHKERRQ(ierr);
        df.Q[i][tao->niter] = df.Q[tao->niter][i] = reg / lambda;
        pgrad = pgrad->next;
      }

      if (tao->niter > 0) {
        df.x[tao->niter] = 0.0;
        ierr = solve(&df); CHKERRQ(ierr);
      } else
        df.x[0] = 1.0;

      /* now computing Jt*(alpha_t) which should be = Jt(wt) to check convergence */
      jtwt = 0.0;
      ierr = VecSet(bmrm->local_w, 0.0); CHKERRQ(ierr);
      pgrad = grad_list.next;
      for (i=0; i<=tao->niter; i++) {
        jtwt -= df.x[i] * df.f[i];
        ierr = VecAXPY(bmrm->local_w, -df.x[i] / lambda, pgrad->V); CHKERRQ(ierr);
        pgrad = pgrad->next;
      }

      ierr = VecNorm(bmrm->local_w, NORM_2, &reg); CHKERRQ(ierr);
      reg = 0.5*lambda*reg*reg;
      jtwt -= reg;
    } /* end if rank == 0 */

    /* scatter the new W to all nodes */
    ierr = VecScatterBegin(bmrm->scatter,bmrm->local_w,W,INSERT_VALUES,SCATTER_REVERSE);CHKERRQ(ierr);
    ierr = VecScatterEnd(bmrm->scatter,bmrm->local_w,W,INSERT_VALUES,SCATTER_REVERSE);CHKERRQ(ierr);

    ierr = TaoComputeObjectiveAndGradient(tao, W, &f, G);CHKERRQ(ierr);

    ierr = MPI_Bcast(&jtwt,1,MPIU_REAL,0,comm);CHKERRQ(ierr);
    ierr = MPI_Bcast(&reg,1,MPIU_REAL,0,comm);CHKERRQ(ierr);

    jw = reg + f;                                       /* J(w) = regularizer + Remp(w) */
    if (jw < min_jw) min_jw = jw;
    if (jtwt > max_jtwt) max_jtwt = jtwt;

    pre_epsilon = epsilon;
    epsilon = min_jw - jtwt;

    if (!rank) {
      if (innerSolverTol > epsilon) innerSolverTol = epsilon;
      else if (innerSolverTol < 1e-7) innerSolverTol = 1e-7;

      /* if the annealing doesn't work well, lower the inner solver tolerance */
      if(pre_epsilon < epsilon) innerSolverTol *= 0.2;

      df.tol = innerSolverTol*0.5;
    }

    tao->niter++;
    ierr = TaoMonitor(tao,tao->niter,min_jw,epsilon,0.0,tao->step,&reason);CHKERRQ(ierr);
  }

  /* free all the memory */
  if (!rank) {
    ierr = destroy_grad_list(&grad_list);CHKERRQ(ierr);
    ierr = destroy_df_solver(&df);CHKERRQ(ierr);
  }

  ierr = VecDestroy(&bmrm->local_w);CHKERRQ(ierr);
  ierr = VecScatterDestroy(&bmrm->scatter);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}


/* ---------------------------------------------------------- */

#undef __FUNCT__
#define __FUNCT__ "TaoSetup_BMRM"
static PetscErrorCode TaoSetup_BMRM(Tao tao) 
{

  PetscErrorCode ierr;

  PetscFunctionBegin;
  /* Allocate some arrays */
  if (!tao->gradient) {
    ierr = VecDuplicate(tao->solution, &tao->gradient);   CHKERRQ(ierr);
  }
  PetscFunctionReturn(0);
}

/*------------------------------------------------------------*/
#undef __FUNCT__
#define __FUNCT__ "TaoDestroy_BMRM"
static PetscErrorCode TaoDestroy_BMRM(Tao tao)
{
  PetscErrorCode ierr;

  PetscFunctionBegin;
  ierr = PetscFree(tao->data);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "TaoSetFromOptions_BMRM"
static PetscErrorCode TaoSetFromOptions_BMRM(PetscOptions *PetscOptionsObject,Tao tao)
{
  PetscErrorCode ierr;
  TAO_BMRM*      bmrm = (TAO_BMRM*)tao->data;

  PetscFunctionBegin;
  ierr = PetscOptionsHead(PetscOptionsObject,"BMRM for regularized risk minimization");CHKERRQ(ierr);
  ierr = PetscOptionsReal("-tao_bmrm_lambda", "regulariser weight","", 100,&bmrm->lambda,NULL); CHKERRQ(ierr);
  ierr = PetscOptionsTail();CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

/*------------------------------------------------------------*/
#undef __FUNCT__
#define __FUNCT__ "TaoView_BMRM"
static PetscErrorCode TaoView_BMRM(Tao tao, PetscViewer viewer)
{
  PetscBool      isascii;
  PetscErrorCode ierr;

  PetscFunctionBegin;
  ierr = PetscObjectTypeCompare((PetscObject)viewer,PETSCVIEWERASCII,&isascii);CHKERRQ(ierr);
  if (isascii) {
    ierr = PetscViewerASCIIPushTab(viewer);CHKERRQ(ierr);
    ierr = PetscViewerASCIIPopTab(viewer);CHKERRQ(ierr);
  }
  PetscFunctionReturn(0);
}

/*------------------------------------------------------------*/
/*MC
  TAOBMRM - bundle method for regularized risk minimization

  Options Database Keys:
. - tao_bmrm_lambda - regulariser weight

  Level: beginner
M*/

#undef __FUNCT__
#define __FUNCT__ "TaoCreate_BMRM"
PETSC_EXTERN PetscErrorCode TaoCreate_BMRM(Tao tao)
{
  TAO_BMRM       *bmrm;
  PetscErrorCode ierr;

  PetscFunctionBegin;
  tao->ops->setup = TaoSetup_BMRM;
  tao->ops->solve = TaoSolve_BMRM;
  tao->ops->view  = TaoView_BMRM;
  tao->ops->setfromoptions = TaoSetFromOptions_BMRM;
  tao->ops->destroy = TaoDestroy_BMRM;

  ierr = PetscNewLog(tao,&bmrm);CHKERRQ(ierr);
  bmrm->lambda = 1.0;
  tao->data = (void*)bmrm;

  /* Override default settings (unless already changed) */
  if (!tao->max_it_changed) tao->max_it = 2000;
  if (!tao->max_funcs_changed) tao->max_funcs = 4000;
  if (!tao->fatol_changed) tao->fatol = 1.0e-12;
  if (!tao->frtol_changed) tao->frtol = 1.0e-12;
  if (!tao->gatol_changed) tao->gatol = 1.0e-12;
  if (!tao->grtol_changed) tao->grtol = 1.0e-12;

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "init_df_solver"
PetscErrorCode init_df_solver(TAO_DF *df)
{
  PetscInt       i, n = INCRE_DIM;
  PetscErrorCode ierr;

  PetscFunctionBegin;
  /* default values */
  df->maxProjIter = 200;
  df->maxPGMIter = 300000;
  df->b = 1.0;

  /* memory space required by Dai-Fletcher */
  df->cur_num_cp = n;
  ierr = PetscMalloc1(n, &df->f); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->a); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->l); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->u); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->x); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->Q); CHKERRQ(ierr);

  for (i = 0; i < n; i ++) {
    ierr = PetscMalloc1(n, &df->Q[i]); CHKERRQ(ierr);
  }

  ierr = PetscMalloc1(n, &df->g); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->y); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->tempv); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->d); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->Qd); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->t); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->xplus); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->tplus); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->sk); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->yk); CHKERRQ(ierr);

  ierr = PetscMalloc1(n, &df->ipt); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->ipt2); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->uv); CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "ensure_df_space"
PetscErrorCode ensure_df_space(PetscInt dim, TAO_DF *df)
{
  PetscErrorCode ierr;
  PetscReal      *tmp, **tmp_Q;
  PetscInt       i, n, old_n;

  PetscFunctionBegin;
  df->dim = dim;
  if (dim <= df->cur_num_cp) PetscFunctionReturn(0);

  old_n = df->cur_num_cp;
  df->cur_num_cp += INCRE_DIM;
  n = df->cur_num_cp;

  /* memory space required by dai-fletcher */
  ierr = PetscMalloc1(n, &tmp); CHKERRQ(ierr);
  ierr = PetscMemcpy(tmp, df->f, sizeof(PetscReal)*old_n); CHKERRQ(ierr);
  ierr = PetscFree(df->f); CHKERRQ(ierr);
  df->f = tmp;

  ierr = PetscMalloc1(n, &tmp); CHKERRQ(ierr);
  ierr = PetscMemcpy(tmp, df->a, sizeof(PetscReal)*old_n); CHKERRQ(ierr);
  ierr = PetscFree(df->a); CHKERRQ(ierr);
  df->a = tmp;

  ierr = PetscMalloc1(n, &tmp); CHKERRQ(ierr);
  ierr = PetscMemcpy(tmp, df->l, sizeof(PetscReal)*old_n); CHKERRQ(ierr);
  ierr = PetscFree(df->l); CHKERRQ(ierr);
  df->l = tmp;

  ierr = PetscMalloc1(n, &tmp); CHKERRQ(ierr);
  ierr = PetscMemcpy(tmp, df->u, sizeof(PetscReal)*old_n); CHKERRQ(ierr);
  ierr = PetscFree(df->u); CHKERRQ(ierr);
  df->u = tmp;

  ierr = PetscMalloc1(n, &tmp); CHKERRQ(ierr);
  ierr = PetscMemcpy(tmp, df->x, sizeof(PetscReal)*old_n); CHKERRQ(ierr);
  ierr = PetscFree(df->x); CHKERRQ(ierr);
  df->x = tmp;

  ierr = PetscMalloc1(n, &tmp_Q); CHKERRQ(ierr);
  for (i = 0; i < n; i ++) {
    ierr = PetscMalloc1(n, &tmp_Q[i]); CHKERRQ(ierr);
    if (i < old_n) {
      ierr = PetscMemcpy(tmp_Q[i], df->Q[i], sizeof(PetscReal)*old_n); CHKERRQ(ierr);
      ierr = PetscFree(df->Q[i]); CHKERRQ(ierr);
    }
  }

  ierr = PetscFree(df->Q); CHKERRQ(ierr);
  df->Q = tmp_Q;

  ierr = PetscFree(df->g); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->g); CHKERRQ(ierr);

  ierr = PetscFree(df->y); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->y); CHKERRQ(ierr);

  ierr = PetscFree(df->tempv); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->tempv); CHKERRQ(ierr);

  ierr = PetscFree(df->d); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->d); CHKERRQ(ierr);

  ierr = PetscFree(df->Qd); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->Qd); CHKERRQ(ierr);

  ierr = PetscFree(df->t); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->t); CHKERRQ(ierr);

  ierr = PetscFree(df->xplus); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->xplus); CHKERRQ(ierr);

  ierr = PetscFree(df->tplus); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->tplus); CHKERRQ(ierr);

  ierr = PetscFree(df->sk); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->sk); CHKERRQ(ierr);

  ierr = PetscFree(df->yk); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->yk); CHKERRQ(ierr);

  ierr = PetscFree(df->ipt); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->ipt); CHKERRQ(ierr);

  ierr = PetscFree(df->ipt2); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->ipt2); CHKERRQ(ierr);

  ierr = PetscFree(df->uv); CHKERRQ(ierr);
  ierr = PetscMalloc1(n, &df->uv); CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "destroy_df_solver"
PetscErrorCode destroy_df_solver(TAO_DF *df)
{
  PetscErrorCode ierr;
  PetscInt       i;

  PetscFunctionBegin;
  ierr = PetscFree(df->f); CHKERRQ(ierr);
  ierr = PetscFree(df->a); CHKERRQ(ierr);
  ierr = PetscFree(df->l); CHKERRQ(ierr);
  ierr = PetscFree(df->u); CHKERRQ(ierr);
  ierr = PetscFree(df->x); CHKERRQ(ierr);

  for (i = 0; i < df->cur_num_cp; i ++) {
    ierr = PetscFree(df->Q[i]); CHKERRQ(ierr);
  }
  ierr = PetscFree(df->Q); CHKERRQ(ierr);
  ierr = PetscFree(df->ipt); CHKERRQ(ierr);
  ierr = PetscFree(df->ipt2); CHKERRQ(ierr);
  ierr = PetscFree(df->uv); CHKERRQ(ierr);
  ierr = PetscFree(df->g); CHKERRQ(ierr);
  ierr = PetscFree(df->y); CHKERRQ(ierr);
  ierr = PetscFree(df->tempv); CHKERRQ(ierr);
  ierr = PetscFree(df->d); CHKERRQ(ierr);
  ierr = PetscFree(df->Qd); CHKERRQ(ierr);
  ierr = PetscFree(df->t); CHKERRQ(ierr);
  ierr = PetscFree(df->xplus); CHKERRQ(ierr);
  ierr = PetscFree(df->tplus); CHKERRQ(ierr);
  ierr = PetscFree(df->sk); CHKERRQ(ierr);
  ierr = PetscFree(df->yk); CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

/* Piecewise linear monotone target function for the Dai-Fletcher projector */
#undef __FUNCT__
#define __FUNCT__ "phi"
PetscReal phi(PetscReal *x,PetscInt n,PetscReal lambda,PetscReal *a,PetscReal b,PetscReal *c,PetscReal *l,PetscReal *u)
{
  PetscReal r = 0.0;
  PetscInt  i;

  for (i = 0; i < n; i++){
    x[i] = -c[i] + lambda*a[i];
    if (x[i] > u[i])     x[i] = u[i];
    else if(x[i] < l[i]) x[i] = l[i];
    r += a[i]*x[i];
  }
  return r - b;
}

/** Modified Dai-Fletcher QP projector solves the problem:
 *
 *      minimise  0.5*x'*x - c'*x
 *      subj to   a'*x = b
 *                l \leq x \leq u
 *
 *  \param c The point to be projected onto feasible set
 */
#undef __FUNCT__
#define __FUNCT__ "project"
PetscInt project(PetscInt n,PetscReal *a,PetscReal b,PetscReal *c,PetscReal *l,PetscReal *u,PetscReal *x,PetscReal *lam_ext,TAO_DF *df)
{
  PetscReal      lambda, lambdal, lambdau, dlambda, lambda_new;
  PetscReal      r, rl, ru, s;
  PetscInt       innerIter;
  PetscBool      nonNegativeSlack = PETSC_FALSE;
  PetscErrorCode ierr;

  *lam_ext = 0;
  lambda  = 0;
  dlambda = 0.5;
  innerIter = 1;

  /*  \phi(x;lambda) := 0.5*x'*x + c'*x - lambda*(a'*x-b)
   *
   *  Optimality conditions for \phi:
   *
   *  1. lambda   <= 0
   *  2. r        <= 0
   *  3. r*lambda == 0
   */

  /* Bracketing Phase */
  r = phi(x, n, lambda, a, b, c, l, u);

  if(nonNegativeSlack) {
    /* inequality constraint, i.e., with \xi >= 0 constraint */
    if (r < TOL_R) return 0;
  } else  {
    /* equality constraint ,i.e., without \xi >= 0 constraint */
    if (fabs(r) < TOL_R) return 0;
  }

  if (r < 0.0){
    lambdal = lambda;
    rl      = r;
    lambda  = lambda + dlambda;
    r       = phi(x, n, lambda, a, b, c, l, u);
    while (r < 0.0 && dlambda < BMRM_INFTY)  {
      lambdal = lambda;
      s       = rl/r - 1.0;
      if (s < 0.1) s = 0.1;
      dlambda = dlambda + dlambda/s;
      lambda  = lambda + dlambda;
      rl      = r;
      r       = phi(x, n, lambda, a, b, c, l, u);
    }
    lambdau = lambda;
    ru      = r;
  } else {
    lambdau = lambda;
    ru      = r;
    lambda  = lambda - dlambda;
    r       = phi(x, n, lambda, a, b, c, l, u);
    while (r > 0.0 && dlambda > -BMRM_INFTY) {
      lambdau = lambda;
      s       = ru/r - 1.0;
      if (s < 0.1) s = 0.1;
      dlambda = dlambda + dlambda/s;
      lambda  = lambda - dlambda;
      ru      = r;
      r       = phi(x, n, lambda, a, b, c, l, u);
    }
    lambdal = lambda;
    rl      = r;
  }

  if(fabs(dlambda) > BMRM_INFTY) SETERRQ(PETSC_COMM_SELF,PETSC_ERR_PLIB,"L2N2_DaiFletcherPGM detected Infeasible QP problem!");

  if(ru == 0){
    lambda = lambdau;
    r = phi(x, n, lambda, a, b, c, l, u);
    return innerIter;
  }

  /* Secant Phase */
  s       = 1.0 - rl/ru;
  dlambda = dlambda/s;
  lambda  = lambdau - dlambda;
  r       = phi(x, n, lambda, a, b, c, l, u);

  while (fabs(r) > TOL_R
         && dlambda > TOL_LAM * (1.0 + fabs(lambda))
         && innerIter < df->maxProjIter){
    innerIter++;
    if (r > 0.0){
      if (s <= 2.0){
        lambdau = lambda;
        ru      = r;
        s       = 1.0 - rl/ru;
        dlambda = (lambdau - lambdal) / s;
        lambda  = lambdau - dlambda;
      } else {
        s          = ru/r-1.0;
        if (s < 0.1) s = 0.1;
        dlambda    = (lambdau - lambda) / s;
        lambda_new = 0.75*lambdal + 0.25*lambda;
        if (lambda_new < (lambda - dlambda))
          lambda_new = lambda - dlambda;
        lambdau    = lambda;
        ru         = r;
        lambda     = lambda_new;
        s          = (lambdau - lambdal) / (lambdau - lambda);
      }
    } else {
      if (s >= 2.0){
        lambdal = lambda;
        rl      = r;
        s       = 1.0 - rl/ru;
        dlambda = (lambdau - lambdal) / s;
        lambda  = lambdau - dlambda;
      } else {
        s          = rl/r - 1.0;
        if (s < 0.1) s = 0.1;
        dlambda    = (lambda-lambdal) / s;
        lambda_new = 0.75*lambdau + 0.25*lambda;
        if (lambda_new > (lambda + dlambda))
          lambda_new = lambda + dlambda;
        lambdal    = lambda;
        rl         = r;
        lambda     = lambda_new;
        s          = (lambdau - lambdal) / (lambdau-lambda);
      }
    }
    r = phi(x, n, lambda, a, b, c, l, u);
  }

  *lam_ext = lambda;
  if(innerIter >= df->maxProjIter) {
    ierr = PetscPrintf(PETSC_COMM_SELF, "WARNING: DaiFletcher max iterations\n");CHKERRQ(ierr);
  }
  return innerIter;
}


#undef __FUNCT__
#define __FUNCT__ "solve"
PetscErrorCode solve(TAO_DF *df)
{
  PetscErrorCode ierr;
  PetscInt       i, j, innerIter, it, it2, luv, info, lscount = 0, projcount = 0;
  PetscReal      gd, max, ak, bk, akold, bkold, lamnew, alpha, kktlam=0.0, lam_ext;
  PetscReal      DELTAsv, ProdDELTAsv;
  PetscReal      c, *tempQ;
  PetscReal      *x = df->x, *a = df->a, b = df->b, *l = df->l, *u = df->u, tol = df->tol;
  PetscReal      *tempv = df->tempv, *y = df->y, *g = df->g, *d = df->d, *Qd = df->Qd;
  PetscReal      *xplus = df->xplus, *tplus = df->tplus, *sk = df->sk, *yk = df->yk;
  PetscReal      **Q = df->Q, *f = df->f, *t = df->t;
  PetscInt       dim = df->dim, *ipt = df->ipt, *ipt2 = df->ipt2, *uv = df->uv;

  /*** variables for the adaptive nonmonotone linesearch ***/
  PetscInt    L, llast;
  PetscReal   fr, fbest, fv, fc, fv0;

  c = BMRM_INFTY;

  DELTAsv = EPS_SV;
  if (tol <= 1.0e-5 || dim <= 20) ProdDELTAsv = 0.0F;
  else  ProdDELTAsv = EPS_SV;

  for (i = 0; i < dim; i++)  tempv[i] = -x[i];

  lam_ext = 0.0;

  /* Project the initial solution */
  projcount += project(dim, a, b, tempv, l, u, x, &lam_ext, df);

  /* Compute gradient
     g = Q*x + f; */

  it = 0;
  for (i = 0; i < dim; i++) {
    if (fabs(x[i]) > ProdDELTAsv) ipt[it++] = i;
  }

  ierr = PetscMemzero(t, dim*sizeof(PetscReal)); CHKERRQ(ierr);
  for (i = 0; i < it; i++){
    tempQ = Q[ipt[i]];
    for (j = 0; j < dim; j++) t[j] += (tempQ[j]*x[ipt[i]]);
  }
  for (i = 0; i < dim; i++){
    g[i] = t[i] + f[i];
  }


  /* y = -(x_{k} - g_{k}) */
  for (i = 0; i < dim; i++){
    y[i] = g[i] - x[i];
  }

  /* Project x_{k} - g_{k} */
  projcount += project(dim, a, b, y, l, u, tempv, &lam_ext, df);

  /* y = P(x_{k} - g_{k}) - x_{k} */
  max = ALPHA_MIN;
  for (i = 0; i < dim; i++){
    y[i] = tempv[i] - x[i];
    if (fabs(y[i]) > max) max = fabs(y[i]);
  }

  if (max < tol*1e-3){
    lscount = 0;
    innerIter    = 0;
    return 0;
  }

  alpha = 1.0 / max;

  /* fv0 = f(x_{0}). Recall t = Q x_{k}  */
  fv0   = 0.0;
  for (i = 0; i < dim; i++) fv0 += x[i] * (0.5*t[i] + f[i]);

  /*** adaptive nonmonotone linesearch ***/
  L     = 2;
  fr    = ALPHA_MAX;
  fbest = fv0;
  fc    = fv0;
  llast = 0;
  akold = bkold = 0.0;

  /***      Iterator begins     ***/
  for (innerIter = 1; innerIter <= df->maxPGMIter; innerIter++) {

    /* tempv = -(x_{k} - alpha*g_{k}) */
    for (i = 0; i < dim; i++)  tempv[i] = alpha*g[i] - x[i];

    /* Project x_{k} - alpha*g_{k} */
    projcount += project(dim, a, b, tempv, l, u, y, &lam_ext, df);


    /* gd = \inner{d_{k}}{g_{k}}
        d = P(x_{k} - alpha*g_{k}) - x_{k}
    */
    gd = 0.0;
    for (i = 0; i < dim; i++){
      d[i] = y[i] - x[i];
      gd  += d[i] * g[i];
    }

    /* Gradient computation  */

    /* compute Qd = Q*d  or  Qd = Q*y - t depending on their sparsity */

    it = it2 = 0;
    for (i = 0; i < dim; i++){
      if (fabs(d[i]) > (ProdDELTAsv*1.0e-2)) ipt[it++]   = i;
    }
    for (i = 0; i < dim; i++) {
      if (fabs(y[i]) > ProdDELTAsv) ipt2[it2++] = i;
    }

    ierr = PetscMemzero(Qd, dim*sizeof(PetscReal)); CHKERRQ(ierr);
    /* compute Qd = Q*d */
    if (it < it2){
      for (i = 0; i < it; i++){
        tempQ = Q[ipt[i]];
        for (j = 0; j < dim; j++) Qd[j] += (tempQ[j] * d[ipt[i]]);
      }
    } else { /* compute Qd = Q*y-t */
      for (i = 0; i < it2; i++){
        tempQ = Q[ipt2[i]];
        for (j = 0; j < dim; j++) Qd[j] += (tempQ[j] * y[ipt2[i]]);
      }
      for (j = 0; j < dim; j++) Qd[j] -= t[j];
    }

    /* ak = inner{d_{k}}{d_{k}} */
    ak = 0.0;
    for (i = 0; i < dim; i++) ak += d[i] * d[i];

    bk = 0.0;
    for (i = 0; i < dim; i++) bk += d[i]*Qd[i];

    if (bk > EPS*ak && gd < 0.0)  lamnew = -gd/bk;
    else lamnew = 1.0;

    /* fv is computing f(x_{k} + d_{k}) */
    fv = 0.0;
    for (i = 0; i < dim; i++){
      xplus[i] = x[i] + d[i];
      tplus[i] = t[i] + Qd[i];
      fv      += xplus[i] * (0.5*tplus[i] + f[i]);
    }

    /* fr is fref */
    if ((innerIter == 1 && fv >= fv0) || (innerIter > 1 && fv >= fr)){
      lscount++;
      fv = 0.0;
      for (i = 0; i < dim; i++){
        xplus[i] = x[i] + lamnew*d[i];
        tplus[i] = t[i] + lamnew*Qd[i];
        fv      += xplus[i] * (0.5*tplus[i] + f[i]);
      }
    }

    for (i = 0; i < dim; i++){
      sk[i] = xplus[i] - x[i];
      yk[i] = tplus[i] - t[i];
      x[i]  = xplus[i];
      t[i]  = tplus[i];
      g[i]  = t[i] + f[i];
    }

    /* update the line search control parameters */
    if (fv < fbest){
      fbest = fv;
      fc    = fv;
      llast = 0;
    } else {
      fc = (fc > fv ? fc : fv);
      llast++;
      if (llast == L){
        fr    = fc;
        fc    = fv;
        llast = 0;
      }
    }

    ak = bk = 0.0;
    for (i = 0; i < dim; i++){
      ak += sk[i] * sk[i];
      bk += sk[i] * yk[i];
    }

    if (bk <= EPS*ak) alpha = ALPHA_MAX;
    else {
      if (bkold < EPS*akold) alpha = ak/bk;
      else alpha = (akold+ak)/(bkold+bk);

      if (alpha > ALPHA_MAX) alpha = ALPHA_MAX;
      else if (alpha < ALPHA_MIN) alpha = ALPHA_MIN;
    }

    akold = ak;
    bkold = bk;

    /*** stopping criterion based on KKT conditions ***/
    /* at optimal, gradient of lagrangian w.r.t. x is zero */

    bk = 0.0;
    for (i = 0; i < dim; i++) bk +=  x[i] * x[i];

    if (PetscSqrtReal(ak) < tol*10 * PetscSqrtReal(bk)){
      it     = 0;
      luv    = 0;
      kktlam = 0.0;
      for (i = 0; i < dim; i++){
        /* x[i] is active hence lagrange multipliers for box constraints
                are zero. The lagrange multiplier for ineq. const. is then
                defined as below
        */
        if ((x[i] > DELTAsv) && (x[i] < c-DELTAsv)){
          ipt[it++] = i;
          kktlam    = kktlam - a[i]*g[i];
        } else  uv[luv++] = i;
      }

      if (it == 0 && PetscSqrtReal(ak) < tol*0.5 * PetscSqrtReal(bk)) return 0;
      else {
        kktlam = kktlam/it;
        info   = 1;
        for (i = 0; i < it; i++) {
          if (fabs(a[ipt[i]] * g[ipt[i]] + kktlam) > tol) {
            info = 0;
            break;
          }
        }
        if (info == 1)  {
          for (i = 0; i < luv; i++)  {
            if (x[uv[i]] <= DELTAsv){
              /* x[i] == lower bound, hence, lagrange multiplier (say, beta) for lower bound may
                     not be zero. So, the gradient without beta is > 0
              */
              if (g[uv[i]] + kktlam*a[uv[i]] < -tol){
                info = 0;
                break;
              }
            } else {
              /* x[i] == upper bound, hence, lagrange multiplier (say, eta) for upper bound may
                     not be zero. So, the gradient without eta is < 0
              */
              if (g[uv[i]] + kktlam*a[uv[i]] > tol) {
                info = 0;
                break;
              }
            }
          }
        }

        if (info == 1) return 0;
      }
    }
  }
  return 0;
}