/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#include <float.h>                               // for DBL_EPSILON
#include <math.h>                                // for fabs, NAN
#include <stdio.h>                               // for printf, NULL
#include <stdlib.h>                              // for calloc, free, malloc
#include "SiconosBlas.h"                               // for cblas_daxpy, cblas_d...
#include "NumericsFwd.h"                         // for SolverOptions, Varia...
#include "SolverOptions.h"                       // for SolverOptions, SICON...
#include "VI_cst.h"                              // for SICONOS_VI_IPARAM_ER...
#include "VariationalInequality.h"               // for VariationalInequality
#include "VariationalInequality_Solvers.h"       // for variationalInequalit...
#include "VariationalInequality_computeError.h"  // for variationalInequalit...
#include "siconos_debug.h"                               // for DEBUG_PRINTF, DEBUG_...
#include "numerics_verbose.h"                    // for verbose, numerics_error

#ifdef DEBUG_MESSAGES
#include "NumericsVector.h"
#endif

static int determine_convergence(double error, double *tolerance, int iter,
                                 SolverOptions *options,
                                 VariationalInequality* problem,
                                 double *z, double *w, double rho)

{
  int hasNotConverged = 1;
  if(error < *tolerance)
  {
    if(verbose > 0)
      printf("--------------- VI - Extra Gradient (EG) - Iteration %i "
             "Residual = %14.7e < %7.3e\n", iter, error, *tolerance);
    double absolute_error =0.0;
    variationalInequality_computeError(problem, z, w, *tolerance, options, &absolute_error);
    if(verbose > 0)
      printf("--------------  VI - Extra Gradient (EG)- Full error criterion =  %e\n", absolute_error);


    hasNotConverged = 0;


    if(options->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION]==SICONOS_VI_ERROR_EVALUATION_LIGHT_WITH_FULL_FINAL)
    {
      if(absolute_error > options->dparam[SICONOS_DPARAM_TOL])
      {

        if(verbose > 0)
        {
          printf("error = %e ", error);
          printf("absolute_error = %e ", absolute_error);
          printf("options->dparam[SICONOS_DPARAM_TOL]= %e\n", options->dparam[SICONOS_DPARAM_TOL]);
        }

        *tolerance = error/absolute_error*options->dparam[SICONOS_DPARAM_TOL];
        if(verbose > 0)
          printf("--------------  VI - Extra Gradient (EG)- We modify the required incremental precision to reach accuracy to %e\n", *tolerance);
        hasNotConverged = 1;
      }
      else
      {
        if(verbose > 0)
          printf("-------------- VI - Extra Gradient (EG) - The incremental precision is sufficient to reach accuracy to %e\n", *tolerance);
      }
    }
  }
  else
  {
    if(verbose > 0)
      printf("--------------- VI - Extra Gradient (EG) - Iteration %i rho = %14.7e error = %14.7e > %10.5e \n", iter, rho, error, *tolerance);
  }
  return hasNotConverged;
}

static
double compute_error(VariationalInequality* problem,
                     double *z, double *w, double norm_z_z_k,
                     double tolerance,
                     SolverOptions * options)
{

  double error;
  if(options->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION]==SICONOS_VI_ERROR_EVALUATION_FULL)
    variationalInequality_computeError(problem, z, w, tolerance, options, &error);
  else if(options->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION]==SICONOS_VI_ERROR_EVALUATION_LIGHT ||
          options->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION]==SICONOS_VI_ERROR_EVALUATION_LIGHT_WITH_FULL_FINAL)
  {
    /* cblas_dcopy(n , x , 1 , xtmp, 1); */
    /* cblas_daxpy(n, -1.0, x_k , 1, xtmp , 1) ; */
    /* error = cblas_dnorm2(n,xtmp,1); */
    double norm_z = cblas_dnrm2(problem->size, z, 1);
    error = norm_z_z_k;
    if(fabs(norm_z) > DBL_EPSILON)
      error /= norm_z;
  }
  else
  {
    return NAN;
    // unreachable code => comment.
    // numerics_error("compute_error(VariationalInequality* problem, ...)", "unknown error evaluation strategy");
  }
  return error;
}
void variationalInequality_ExtraGradient(VariationalInequality* problem, double *x, double *w, int* info, SolverOptions* options)
{
  //verbose=1;
  DEBUG_BEGIN("variationalInequality_ExtraGradient(VariationalInequality* problem, ...)\n")
  /* /\* int and double parameters *\/ */
  int* iparam = options->iparam;
  double* dparam = options->dparam;
  /* Number of contacts */
  int n = problem->size;
  /* Maximum number of iterations */
  int itermax = iparam[SICONOS_IPARAM_MAX_ITER];
  /* Tolerance */
  double tolerance = dparam[SICONOS_DPARAM_TOL];

  DEBUG_EXPR(NV_display(x,n););
  DEBUG_EXPR(NV_display(w,n););
  /*****  Fixed point iterations *****/
  int iter = 0; /* Current iteration number */
  double error = 1.; /* Current error */
  int hasNotConverged = 1;


  double * xtmp = (double *)calloc(n,sizeof(double));
  double * wtmp = (double *)calloc(n,sizeof(double));

  double rho = 0.0, rho_k =0.0;
  int isVariable = 0;

  if(dparam[SICONOS_VI_DPARAM_RHO] > 0.0)
  {
    rho = dparam[SICONOS_VI_DPARAM_RHO];
    if(verbose > 0)
    {
      printf("--------------- VI - Extra Gradient (EG) - Fixed stepsize with  rho = %14.7e \n", rho);
    }
  }
  else
  {
    /* Variable step in iterations*/
    isVariable = 1;
    rho = -dparam[SICONOS_VI_DPARAM_RHO];
    if(verbose > 0)
    {
      printf("--------------- VI - Extra Gradient (EG) - Variable stepsize with starting rho = %14.7e \n", rho);
    }

  }

  /* Variable for Line_search */
  int success =0;
  DEBUG_EXPR_WE(double error_k;);
  double light_error_sum =0.0;
  int ls_iter = 0;
  int ls_itermax = 10;
  double tau=dparam[SICONOS_VI_DPARAM_LS_TAU],
         tauinv=dparam[SICONOS_VI_DPARAM_LS_TAUINV],
         L= dparam[SICONOS_VI_DPARAM_LS_L], Lmin = dparam[SICONOS_VI_DPARAM_LS_LMIN];
  double a1=0.0, a2=0.0;
  double * x_k =0;
  double * w_k =0;

  if(isVariable)
  {
    x_k = (double *)malloc(n * sizeof(double));
    w_k = (double *)malloc(n * sizeof(double));
  }
  /* memcpy(x,x_k,n * sizeof(double)); */
  /* memcpy(w,w_k,n * sizeof(double)); */
  //isVariable=0;
  if(!isVariable)
  {
    /*   double minusrho  = -1.0*rho; */
    while((iter < itermax) && (hasNotConverged > 0))
    {
      ++iter;

      /* xtmp <- x  */
      cblas_dcopy(n, x, 1, xtmp, 1);


      /* wtmp <- F(xtmp) */
      problem->F(problem, n, xtmp,wtmp);

      /* xtmp <- xtmp - F(xtmp) */
      cblas_daxpy(n, -1.0, wtmp, 1, xtmp, 1) ;

      /* wtmp <-  ProjectionOnX(xtmp) */
      problem->ProjectionOnX(problem,xtmp,wtmp);

      /* x <- x - wtmp */
      cblas_daxpy(n, -1.0, wtmp, 1, x, 1) ;

      /* x <-  ProjectionOnX(x) */
      cblas_dcopy(n, xtmp, 1, x, 1);

      problem->ProjectionOnX(problem,xtmp,x);


      /* problem->F(problem,x,w); */
      /* cblas_daxpy(n, -1.0, w , 1, x , 1) ; */
      /* cblas_dcopy(n , x , 1 , xtmp, 1); */
      /* problem->ProjectionOnX(problem,xtmp,x); */

      /* **** Criterium convergence **** */
      if(options->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION] == SICONOS_VI_ERROR_EVALUATION_LIGHT||
          options->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION] == SICONOS_VI_ERROR_EVALUATION_LIGHT_WITH_FULL_FINAL)
      {
        cblas_dcopy(n, xtmp, 1,x, 1) ;
        cblas_daxpy(n, -1.0, x_k, 1, xtmp, 1) ;
        light_error_sum = cblas_dnrm2(n,xtmp,1);
      }
      error = compute_error(problem, x, w, light_error_sum,  tolerance, options);
      hasNotConverged = determine_convergence(error, &tolerance, iter, options,
                                              problem, x, w, rho);


      if(options->callback)
      {
        options->callback->collectStatsIteration(options->callback->env, n,
            x, w,
            error, NULL);
      }




    }
  }

  if(isVariable)
  {
    if(iparam[SICONOS_VI_IPARAM_LINESEARCH_METHOD]==SICONOS_VI_LS_ARMIJO)  /* Armijo rule with Khotbotov ratio (default)   */
    {
      while((iter < itermax) && (hasNotConverged > 0))
      {
        ++iter;


        /* Store the error */
        DEBUG_EXPR_WE(error_k = error;);

        /* x_k <-- x store the x at the beginning of the iteration */
        cblas_dcopy(n, x, 1, x_k, 1);
        DEBUG_EXPR(NV_display(x_k,n););
        problem->F(problem, n, x, w_k);

        ls_iter = 0 ;
        success =0;
        rho_k=rho / tau;

        while(!success && (ls_iter < ls_itermax))
        {
          /* if (iparam[SICONOS_VI_IPARAM_DECREASE_RHO] && ls_iter !=0) rho_k = rho_k * tau * min(1.0,a2/(rho_k*a1)); */
          /* else */ rho_k = rho_k * tau ;

          /* x <- x_k  for the std approach*/
          if(iparam[SICONOS_VI_IPARAM_ACTIVATE_UPDATE]==0) cblas_dcopy(n, x_k, 1, x, 1) ;

          /* x <- x - rho_k*  w_k */
          cblas_daxpy(n, -rho_k, w_k, 1, x, 1) ;

          /* xtmp <-  ProjectionOnX(x) */
          problem->ProjectionOnX(problem,x,xtmp);
          problem->F(problem,n,xtmp,w);

          DEBUG_EXPR_WE(for(int i =0; i< 5 ; i++)
        {
          printf("xtmp[%i]=%12.8e\t",i,xtmp[i]);
            printf("w[%i]=F[%i]=%12.8e\n",i,i,w[i]);
          });
          /* velocitytmp <- velocity */
          /* cblas_dcopy(n, w, 1, wtmp , 1) ; */

          /* velocity <- velocity - velocity_k   */
          cblas_daxpy(n, -1.0, w_k, 1, w, 1) ;

          /* a1 =  ||w - w_k|| */
          a1 = cblas_dnrm2(n, w, 1);
          DEBUG_PRINTF("a1 = %12.8e\n", a1);

          /* xtmp <- x */
          cblas_dcopy(n, xtmp, 1,x, 1) ;

          /* xtmp <- x - x_k   */
          cblas_daxpy(n, -1.0, x_k, 1, xtmp, 1) ;

          /* a2 =  || x - x_k || */
          a2 = cblas_dnrm2(n, xtmp, 1) ;
          DEBUG_PRINTF("a2 = %12.8e\n", a2);

          DEBUG_PRINTF("test rho_k*a1 < L * a2 = %e < %e\n", rho_k*a1, L * a2) ;
          success = (rho_k*a1 < L * a2)?1:0;

          /* printf("rho_k = %12.8e\t", rho_k); */
          /* printf("a1 = %12.8e\t", a1); */
          /* printf("a2 = %12.8e\t", a2); */
          /* printf("norm x = %12.8e\t",cblas_dnrm2(n, x, 1) ); */
          /* printf("success = %i\n", success); */

          ls_iter++;
        }

        /* velocitytmp <- q  */
        /* cblas_dcopy(n , q , 1 , velocitytmp, 1); */
        /* NM_gemv(alpha, M, reaction, beta, velocitytmp); */

        problem->F(problem, n, x,wtmp);

        /* x <- x - rho_k*  wtmp */
        cblas_daxpy(n, -rho_k, wtmp, 1, x, 1) ;

        /* wtmp <-  ProjectionOnX(xtmp) */
        cblas_dcopy(n, x, 1, xtmp, 1);
        problem->ProjectionOnX(problem,xtmp,x);

        DEBUG_EXPR(NV_display(x,n););
        DEBUG_EXPR(NV_display(w,n););

        /* **** Criterium convergence **** */
        error = compute_error(problem, x, w, a1,  tolerance, options);
        hasNotConverged = determine_convergence(error, &tolerance, iter, options,
                                                problem, x, w, rho);

        DEBUG_PRINTF("error = %12.8e\t error_k = %12.8e\n",error,error_k);
        /*Update rho*/
        if((rho_k*a1 < Lmin * a2))
        {
          rho =rho_k*tauinv;
        }
        else
          rho =rho_k;
      }
    }// end iparam[SICONOS_VI_IPARAM_LINESEARCH_METHOD]==0

    if(iparam[SICONOS_VI_IPARAM_LINESEARCH_METHOD]==SICONOS_VI_LS_SOLODOV)  /* Armijo rule with Solodov.Tseng ratio */
    {
      while((iter < itermax) && (hasNotConverged > 0))
      {
        ++iter;


        /* Store the error */
        DEBUG_EXPR_WE(error_k = error;);

        /* x_k <-- x store the x at the beginning of the iteration */
        cblas_dcopy(n, x, 1, x_k, 1);

        problem->F(problem, n, x, w_k);

        ls_iter = 0 ;
        success =0;
        rho_k=rho / tau;

        while(!success && (ls_iter < ls_itermax))
        {

          /* if (iparam[SICONOS_VI_IPARAM_DECREASE_RHO] && ls_iter !=0) rho_k = rho_k * tau * min(1.0,a2*a2/(rho_k*a1)); */
          /* else */ rho_k = rho_k * tau ;

          /* x <- x_k  for the std approach*/
          if(iparam[SICONOS_VI_IPARAM_ACTIVATE_UPDATE]==0) cblas_dcopy(n, x_k, 1, x, 1) ;

          /* x <- x - rho_k*  w_k */
          cblas_daxpy(n, -rho_k, w_k, 1, x, 1) ;

          /* xtmp <-  ProjectionOnX(x) */
          problem->ProjectionOnX(problem,x,xtmp);
          problem->F(problem,n,xtmp,w);

          DEBUG_EXPR_WE(for(int i =0; i< 5 ; i++)
        {
          printf("xtmp[%i]=%12.8e\t",i,xtmp[i]);
            printf("w[%i]=F[%i]=%12.8e\n",i,i,w[i]);
          });
          /* wtmp <- w */
          /* cblas_dcopy(n, w, 1, wtmp , 1) ; */

          /* w <- w - w_k   */
          cblas_daxpy(n, -1.0, w_k, 1, w, 1) ;

          /* xtmp <- x - x_k   */
          cblas_dcopy(n, xtmp, 1,x, 1) ;
          cblas_daxpy(n, -1.0, x_k, 1, xtmp, 1) ;

          /* a1 =  (w - w_k)^T(x - x_k) */
          a1 = cblas_ddot(n, xtmp, 1, w, 1);
          DEBUG_PRINTF("a1 = %12.8e\n", a1);

          /* a2 =  || x - x_k || */
          a2 = cblas_dnrm2(n, xtmp, 1) ;
          DEBUG_PRINTF("a2 = %12.8e\n", a2);

          DEBUG_PRINTF("test rho_k*a1 < L * a2 * a2 = %e < %e\n", rho_k*a1, L * a2 * a2) ;
          success = (rho_k*a1 < L * a2 * a2)?1:0;

          /* printf("rho_k = %12.8e\t", rho_k); */
          /* printf("a1 = %12.8e\t", a1); */
          /* printf("a2 = %12.8e\t", a2); */
          /* printf("norm x = %12.8e\t",cblas_dnrm2(n, x, 1) ); */
          /* printf("success = %i\n", success); */

          ls_iter++;
        }



        /* velocitytmp <- q  */
        /* cblas_dcopy(n , q , 1 , velocitytmp, 1); */
        /* NM_gemv(alpha, M, reaction, beta, velocitytmp); */

        problem->F(problem, n, x,wtmp);

        /* x <- x - rho_k*  wtmp */
        cblas_daxpy(n, -rho_k, wtmp, 1, x, 1) ;

        /* wtmp <-  ProjectionOnX(xtmp) */
        cblas_dcopy(n, x, 1, xtmp, 1);
        problem->ProjectionOnX(problem,xtmp,x);
        DEBUG_EXPR_WE(for(int i =0; i< 5 ; i++)
      {
        printf("x[%i]=%12.8e\t",i,x[i]);
          printf("w[%i]=F[%i]=%12.8e\n",i,i,w[i]);
        }
                     );


        /* **** Criterium convergence **** */
        error = compute_error(problem, x, w, a1,  tolerance, options);
        hasNotConverged = determine_convergence(error, &tolerance, iter, options,
                                                problem, x, w, rho);





        DEBUG_PRINTF("error = %12.8e\t error_k = %12.8e\n",error,error_k);
        /*Update rho*/
        if((rho_k*a1 < Lmin * a2*a2))
        {
          rho =rho_k*tauinv;
        }
        else
          rho =rho_k;

      }

    }// end iparam[SICONOS_VI_IPARAM_LINESEARCH_METHOD]==SICONOS_VI_LS_SOLODOV
    /* we return the negative value of rho for multiple call to the solver */
    dparam[SICONOS_VI_DPARAM_RHO] = -rho;

    free(x_k);
    free(w_k);
  }

  *info = hasNotConverged;
  if(verbose > 0)
  {
    printf("--------------- VI - Extra Gradient (EG) - #Iteration %i Final Residual = %14.7e\n", iter, error);
  }
  dparam[SICONOS_DPARAM_TOL] = tolerance;
  dparam[SICONOS_DPARAM_RESIDU] = error;
  iparam[SICONOS_IPARAM_ITER_DONE] = iter;
  free(xtmp);
  free(wtmp);
  DEBUG_END("variationalInequality_ExtraGradient(VariationalInequality* problem, ...)\n")

}


void variationalInequality_ExtraGradient_set_default(SolverOptions* options)
{
  options->iparam[SICONOS_VI_IPARAM_LINESEARCH_METHOD] = SICONOS_VI_LS_ARMIJO;
  /* options->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION]=SICONOS_VI_ERROR_EVALUATION_FULL; */
  options->iparam[SICONOS_VI_IPARAM_DECREASE_RHO] = 0; // use rho_k * tau * min(1.0,a2/(rho_k*a1)) to decrease rho; commented in the code
  options->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION] = SICONOS_VI_ERROR_EVALUATION_LIGHT_WITH_FULL_FINAL;
  options->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION_FREQUENCY]=0; // Rk FP : set but not used
  options->iparam[SICONOS_VI_IPARAM_ACTIVATE_UPDATE] = 0; // activate the update in the loop (0:false default choice)
  options->dparam[SICONOS_VI_DPARAM_RHO] = -1.0; // in-out parameter
  options->dparam[SICONOS_VI_DPARAM_LS_TAU] = 2/3.0;  /* tau */
  options->dparam[SICONOS_VI_DPARAM_LS_TAUINV] = 3.0/2.0;  /*tauinv */
  options->dparam[SICONOS_VI_DPARAM_LS_L] = 0.9;  /* L */
  options->dparam[SICONOS_VI_DPARAM_LS_LMIN] = 0.3;  /* Lmin */
}
