#include "src/complementarity/impls/ssls/ssls.h"
// Context for ASXLS
//   -- active-set	- reduced matrices formed
//			  - inherit properties of original system
//   -- semismooth (S)  - function not differentiable
//                      - merit function continuously differentiable
//                      - Fischer-Burmeister reformulation of complementarity
//                        - Billups composition for two finite bounds
//   -- infeasible (I)  - iterates not guaranteed to remain within bounds
//   -- feasible (F)    - iterates guaranteed to remain within bounds
//   -- linesearch (LS) - Armijo rule on direction
//
// Many other reformulations are possible and combinations of 
// feasible/infeasible and linesearch/trust region are possible.
//
// Basic theory 
//   Fischer-Burmeister reformulation is semismooth with a continuously
//   differentiable merit function and strongly semismooth if the F has
//   lipschitz continuous derivatives.
//
//   Every accumulation point generated by the algorithm is a stationary
//   point for the merit function.  Stationary points of the merit function
//   are solutions of the complementarity problem if
//     a.  the stationary point has a BD-regular subdifferential, or
//     b.  the Schur complement F'/F'_ff is a P_0-matrix where ff is the
//         index set corresponding to the free variables.
//
//   If one of the accumulation points has a BD-regular subdifferential then
//     a.  the entire sequence converges to this accumulation point at
//         a local q-superlinear rate
//     b.  if in addition the reformulation is strongly semismooth near
//         this accumulation point, then the algorithm converges at a
//         local q-quadratic rate.
//
// The theory for the feasible version follows from the feasible descent
// algorithm framework.
//
// References:
//   Billups, "Algorithms for Complementarity Problems and Generalized
//     Equations," Ph.D thesis, University of Wisconsin - Madison, 1995.
//   De Luca, Facchinei, Kanzow, "A Semismooth Equation Approach to the
//     Solution of Nonlinear Complementarity Problems," Mathematical
//     Programming, 75, pages 407-439, 1996.
//   Ferris, Kanzow, Munson, "Feasible Descent Algorithms for Mixed
//     Complementarity Problems," Mathematical Programming, 86,
//     pages 475-497, 1999.
//   Fischer, "A Special Newton-type Optimization Method," Optimization,
//     24, pages 269-284, 1992
//   Munson, Facchinei, Ferris, Fischer, Kanzow, "The Semismooth Algorithm
//     for Large Scale Complementarity Problems," Technical Report 99-06,
//     University of Wisconsin - Madison, 1999.


/*------------------------------------------------------------*/
#undef __FUNCT__  
#define __FUNCT__ "TaoSolve_ASILS"
static int TaoSolve_ASILS(TAO_SOLVER tao, void *solver)
{
  TAO_SSLS *asls = (TAO_SSLS *)solver;
  TaoVec *x, *l, *u, *ff, *dpsi, *d, *w, *t1, *t2, *da, *db;
  TaoMat *J,*Jsub;
  TaoVec *dxfree,*r1, *r2, *r3;
  TaoIndexSet *FreeVariableSet,*FixedVariableSet;
  //  TaoLinearSolver *lsolver;
  double psi, ndpsi, normd, innerd, t=0;
  double delta, rho;
  int iter=0, info, nn, nf;
  TaoTerminateReason reason;
  TaoTruth flag;

  TaoFunctionBegin;

  // Assume that Setup has been called!
  // Set the structure for the Jacobian and create a linear solver.

  delta = asls->delta;
  rho = asls->rho;

  info = TaoGetSolution(tao, &x); CHKERRQ(info);
  info = TaoGetVariableBounds(tao, &l, &u); CHKERRQ(info);
  info = TaoEvaluateVariableBounds(tao,l,u); CHKERRQ(info);
  info = TaoGetJacobian(tao, &J); CHKERRQ(info);

  info = x->GetDimension(&nn); CHKERRQ(info);

  info = x->CreateIndexSet(&FreeVariableSet); CHKERRQ(info);
  info = x->CreateIndexSet(&FixedVariableSet); CHKERRQ(info);
  info = J->CreateReducedMatrix(FreeVariableSet,FreeVariableSet,&Jsub); CHKERRQ(info);
  info = x->Clone(&dxfree); CHKERRQ(info);
  info = x->Clone(&r1); CHKERRQ(info);
  info = x->Clone(&r2); CHKERRQ(info);
  info = x->Clone(&r3); CHKERRQ(info);

  ff = asls->ff;
  dpsi = asls->dpsi;
  d = asls->d;
  w = asls->w;
  t1 = asls->t1;
  t2 = asls->t2;
  da = asls->da;
  db = asls->db;

  info = TaoSetMeritFunction(tao, Tao_SSLS_Function, Tao_ASLS_FunctionGradient,
                             TAO_NULL, TAO_NULL, asls); CHKERRQ(info);

  // Calculate the function value and fischer function value at the
  // current iterate

  info = TaoComputeMeritFunctionGradient(tao, x, &psi, dpsi); CHKERRQ(info);
  info = dpsi->Norm2(&ndpsi); CHKERRQ(info);

  while (1) {

    // Check the termination criteria

    info = TaoMonitor(tao, iter++, asls->merit, ndpsi, 0.0, t, &reason); CHKERRQ(info);
    if (TAO_CONTINUE_ITERATING != reason) break;

    // We are going to solve a linear system of equations.  We need to
    // set the tolerances for the solve so that we maintain an asymptotic
    // rate of convergence that is superlinear.
    // Note: these tolerances are for the reduced system.  We really need
    // to make sure that the full system satisfies the full-space conditions.
    
    // This rule gives superlinear asymptotic convergence
    // asls->atol = TaoMin(0.5, asls->merit*sqrt(asls->merit));
    // asls->rtol = 0.0;

    // This rule gives quadratic asymptotic convergence
    // asls->atol = TaoMin(0.5, asls->merit*asls->merit);
    // asls->rtol = 0.0;

    // Calculate a free and fixed set of variables.  The fixed set of
    // variables are those for the d_b is approximately equal to zero.
    // The definition of approximately changes as we approach the solution
    // to the problem.

    // No one rule is guaranteed to work in all cases.  The following 
    // definition is based on the norm of the Jacobian matrix.  If the 
    // norm is large, the tolerance becomes smaller.

    info = J->Norm1(&(asls->identifier)); CHKERRQ(info);
    // asls->identifier = asls->atol / (1 + asls->identifier);
    asls->identifier = TaoMin(asls->merit, 1e-2) / (1 + asls->identifier);
    // asls->identifier = 1e-4 / (1 + asls->identifier);
    // asls->identifier = 1e-4;
    // asls->identifier = 1e-8;

    info = t1->SetToConstant(-asls->identifier); CHKERRQ(info);
    info = t2->SetToConstant( asls->identifier); CHKERRQ(info);

    info = FixedVariableSet->WhichBetweenOrEqual(t1, db, t2); CHKERRQ(info);
    info = FreeVariableSet->ComplementOf(FixedVariableSet); CHKERRQ(info);

    info = FixedVariableSet->GetSize(&nf);
    printf("Fixed size: %d\n", nf);

    // We now have our partition.  Now calculate the direction in the
    // fixed variable space.

    info = r1->SetReducedVec(ff, FixedVariableSet); CHKERRQ(info);
    info = r2->SetReducedVec(da, FixedVariableSet); CHKERRQ(info);
    info = r1->PointwiseDivide(r1, r2); CHKERRQ(info);

    info = d->SetToZero(); CHKERRQ(info);
    info = d->ReducedXPY(r1, FixedVariableSet); CHKERRQ(info);

    // Our direction in the FixedVariableSet is fixed.  Calculate the
    // information needed for the step in the FreeVariableSet.  To
    // do this, we need to know the diagonal perturbation and the
    // right hand side.

    info = r1->SetReducedVec(da, FreeVariableSet); CHKERRQ(info);
    info = r2->SetReducedVec(ff, FreeVariableSet); CHKERRQ(info);
    info = r3->SetReducedVec(db, FreeVariableSet); CHKERRQ(info);

    info = r1->PointwiseDivide(r1, r3); CHKERRQ(info);
    info = r2->PointwiseDivide(r2, r3); CHKERRQ(info);

    // r1 is the diagonal perturbation
    // r2 is the right hand side
    // r3 is no longer needed

    // Now need to modify r2 for our direction choice in the fixed
    // variable set:  calculate t1 = J*d, take the reduced vector
    // of t1 and modify r2.

    info = J->Multiply(d, t1); CHKERRQ(info);
    info = r3->SetReducedVec(t1, FreeVariableSet); CHKERRQ(info);
    info = r2->Axpy(-1.0, r3);

    // Calculate the reduced problem matrix and the direction

    info = Jsub->SetReducedMatrix(J, FreeVariableSet, FreeVariableSet); CHKERRQ(info);
    info = Jsub->AddDiagonal(r1); CHKERRQ(info);
    info = dxfree->SetReducedVec(d, FreeVariableSet);CHKERRQ(info);
    info = dxfree->SetToZero(); CHKERRQ(info);

    // Set the convergence for the reduced problem solver.
    // info = TaoGetLinearSolver(tao, &lsolver); CHKERRQ(info);
    // info = lsolver->SetTolerances(0.1*asls->rtol, 0.1*asls->atol, 1e30, 10*nn); CHKERRQ(info);

    // Calculate the reduced direction.  (Really negative of Newton
    // direction.  Therefore, rest of the code uses -d.)

    info = TaoPreLinearSolve(tao, Jsub); CHKERRQ(info);
    info = TaoLinearSolve(tao, Jsub, r2, dxfree, &flag); CHKERRQ(info);

    // Add the direction in the free variables back into the real direction.

    info = d->ReducedXPY(dxfree, FreeVariableSet);CHKERRQ(info);

    // Calculate the norm of the relative residual in the real space

    // info = t1->PointwiseMultiply(da, d); CHKERRQ(info);
    // info = J->Multiply(d, t2); CHKERRQ(info);
    // info = t2->PointwiseMultiply(db, t2); CHKERRQ(info);
    // info = t1->Axpy( 1.0, t2); CHKERRQ(info);
    // info = t1->Axpy(-1.0, ff); CHKERRQ(info);
    // info = t1->Norm2(&two_norm); CHKERRQ(info);

    // if (two_norm >= asls->atol) {
    //   printf("Bad absolute residual: actual: %5.4e needed: %5.4e\n",
    //          two_norm, asls->atol);
    // }

    // Check the real direction for descent and if not, use the negative
    // gradient direction.

    info = d->Norm2(&normd); CHKERRQ(info);
    info = d->Dot(dpsi, &innerd); CHKERRQ(info);

    if (innerd <= delta*pow(normd, rho)) {
       printf("Gradient direction: %5.4e.\n", innerd);
       info = PetscLogInfo((tao, "TaoSolve_ASILS: %d: newton direction not descent\n", iter)); CHKERRQ(info);

       info = d->CopyFrom(dpsi); CHKERRQ(info);
       info = d->Dot(dpsi, &innerd); CHKERRQ(info);
    }

    info = d->Negate(); CHKERRQ(info);
    innerd = -innerd;

    // We now have a correct descent direction.  Apply a linesearch to
    // find the new iterate.

    t = 1;
    info = TaoLineSearchApply(tao, x, dpsi, d, w,
                              &psi, &t, &tao->lsflag); CHKERRQ(info);
    info = dpsi->Norm2(&ndpsi); CHKERRQ(info);
  }

  info = TaoMatDestroy(Jsub);CHKERRQ(info);
  info = TaoVecDestroy(dxfree);CHKERRQ(info);
  info = TaoVecDestroy(r1);CHKERRQ(info);
  info = TaoVecDestroy(r2);CHKERRQ(info);
  info = TaoVecDestroy(r3);CHKERRQ(info);
  info = TaoIndexSetDestroy(FreeVariableSet);CHKERRQ(info);
  info = TaoIndexSetDestroy(FixedVariableSet);CHKERRQ(info);

  TaoFunctionReturn(0);
}

/* ---------------------------------------------------------- */
EXTERN_C_BEGIN
#undef __FUNCT__  
#define __FUNCT__ "TaoCreate_ASILS"
int TaoCreate_ASILS(TAO_SOLVER tao)
{
  TAO_SSLS *asls;
  int        info;

  TaoFunctionBegin;

  info = TaoNew(TAO_SSLS,&asls); CHKERRQ(info);
  info = PetscLogObjectMemory(tao, sizeof(TAO_SSLS)); CHKERRQ(info);

  asls->delta = 1e-10;
  asls->rho = 2.1;

  asls->identifier = 1e-5;

  info=TaoSetTaoSolveRoutine(tao,TaoSolve_ASILS,(void*)asls); CHKERRQ(info);
  info=TaoSetTaoSetUpDownRoutines(tao,TaoSetUp_SSLS,TaoSetDown_SSLS); CHKERRQ(info);
  info=TaoSetTaoOptionsRoutine(tao,TaoSetOptions_SSLS); CHKERRQ(info);
  info=TaoSetTaoViewRoutine(tao,TaoView_SSLS); CHKERRQ(info);

  info = TaoCreateArmijoLineSearch(tao); CHKERRQ(info);

  info = TaoSetMaximumIterates(tao,2000); CHKERRQ(info);
  info = TaoSetMaximumFunctionEvaluations(tao,4000); CHKERRQ(info);

  info = TaoSetTolerances(tao,0,0,0,0); CHKERRQ(info);
  info = TaoSetGradientTolerances(tao,1.0e-16,0.0,0.0); CHKERRQ(info);
  info = TaoSetFunctionLowerBound(tao,1.0e-8); CHKERRQ(info);

  TaoFunctionReturn(0);
}
EXTERN_C_END

