/* 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 DECIMAL_DIG, DBL_EPSILON
#include <math.h>                   // for copysign
#include <stdio.h>                  // for printf, NULL
#include <stdlib.h>                 // for free
#include "NonSmoothDrivers.h"       // for variationalInequality_driver
#include "NumericsFwd.h"            // for VariationalInequality, SolverOptions
#include "NumericsMatrix.h"         // for NM_create_from_data, NM_DENSE
#include "SiconosSets.h"            // for box_constraints, SICONOS_SET_BOX
#include "SolverOptions.h"          // for SolverOptions, solver_options_delete
#include "VI_cst.h"                 // for SICONOS_VI_BOX_AVI_LSA
#include "VariationalInequality.h"  // for VariationalInequality, VI_get_env

#define N 2

#ifdef __cplusplus
#undef restrict
#define restrict __restrict
#endif

typedef struct
{
  int id;
  double* xk;
  double h;
  double theta;
  double gamma;
  double g;
  double kappa;
  unsigned int f_eval;
  unsigned int nabla_eval;
} data_ZI;

static void F_ZI(void* problem, int n, double* restrict l, double* restrict F)
{
  data_ZI* d = (data_ZI*) VI_get_env(problem);

  double xk0 = d->xk[0];
  double xk1 = d->xk[1];
  double l0 = l[0];
  double l1 = l[1];
  double invR = 1.0/(1.0 - d->kappa*l0*l1);
  F[1] = d->h*d->g*l0*invR;

  double v_gamma = xk1 + d->gamma*F[1];
  F[0] = -d->h*d->kappa*l0*l1*v_gamma;

  double v_theta = xk1 + d->theta*F[1];
  F[0] += xk0 + d->h*v_theta;
  F[1] += xk1;
  d->f_eval += 1;

}

static void nabla_F_ZI(void* problem, int n, double* restrict l, NumericsMatrix* restrict nabla_F)
{
  double* restrict nabla_F_dense = nabla_F->matrix0;
  data_ZI* d = (data_ZI*) VI_get_env(problem);

  double l0 = l[0];
  double l1 = l[1];
  double xk1 = d->xk[1];
  double invR = 1.0/(1.0 - d->kappa*l0*l1);
  double invR2 = invR*invR;
  double rr1 = d->g*l0*invR;

  double v_gamma = xk1 + d->gamma*d->h*rr1;

  nabla_F_dense[1] = d->h*d->g*invR2;
  nabla_F_dense[1 + 2] = d->h*(d->g*d->kappa*l0*l0)/invR2;
//  nabla_F_dense[0] = d->h*(-d->kappa*l1*v_gamma + d->gamma*d->h*(1.0-d->kappa*l0*l1)*nabla_F_dense[1]);
//  nabla_F_dense[0 + 2] = d->h*(-d->kappa*l0*v_gamma + d->gamma*d->h*(1.0-d->kappa*l0*l1)*nabla_F_dense[1 + 2]);
  nabla_F_dense[0] = d->h*(-d->kappa*l1*v_gamma + d->gamma*d->h*(1.0-d->kappa*l0*l1)*nabla_F_dense[1]);
  nabla_F_dense[0 + 2] = d->h*(-d->kappa*l0*v_gamma + d->gamma*d->h*(1.0-d->kappa*l0*l1)*nabla_F_dense[1 + 2]);
  d->nabla_eval += 1;
}


int main(void)
{
  int info = 0;

  /* Set solver options */
  SolverOptions * options = solver_options_create( @PROBLEM_COLLECTION@);

  /* Create a VariationalInequality */
  VariationalInequality* problem = variationalInequality_new(N);
  problem->F = &F_ZI;
  problem->compute_nabla_F = &nabla_F_ZI;

  double xk[] = {1., 10.0};
  double t = 0.0;
  double T = 1.0;

  double z[N] = {0.};
  double F[N] = {0.};
  double nablaF[N*N] = {0.};

  data_ZI sim_data;

  sim_data.id = -1;
  sim_data.xk = xk;
  sim_data.h = 1e-3;
  sim_data.theta = 1.0;
  sim_data.gamma = 1.0;
  sim_data.g = 9.81;
  sim_data.kappa = .7;

  options->dparam[SICONOS_DPARAM_TOL] = N*DBL_EPSILON;
  options->iparam[SICONOS_IPARAM_PREALLOC] = 1;
  options->iparam[SICONOS_IPARAM_MAX_ITER] = 250;
  
//  options->dparam[SICONOS_VI_DPARAM_RHO] = 2;
//  options->dparam[SICONOS_VI_DPARAM_LS_TAU] = 10;
  
  problem->env = &sim_data;
  problem->nabla_F = NM_create_from_data(NM_DENSE, N, N, nablaF);

  box_constraints box;
  double lb[] = {-1.0, -1.0};
  double ub[] = {1.0, 1.0};
  box.id = SICONOS_SET_BOX;
  box.lb = lb;
  box.ub = ub;
  problem->set = &box;

  unsigned k = 0;

  while ((t <= T) && info == 0)
  {
    k++;
    info = variationalInequality_driver(problem, z, F, options);
    if (info > 0)
    {
      printf("VI_ZI1 info = %d\n", info);
      /* do some magic shit to force the algo to fix a solution */
      z[0] = copysign(1.0, -xk[0]);
      z[1] = copysign(1.0, -xk[1]);

      info = variationalInequality_driver(problem, z, F, options);

      if (info > 0)
      {
        printf("step %d, iter = %d, error = %le\n", k, options->iparam[SICONOS_IPARAM_ITER_DONE], options->dparam[SICONOS_DPARAM_RESIDU]);
        printf("VI_ZI1 with solver @PROBLEM_COLLECTION@ failed completly\n");
        printf("VI_ZI1 failure at xk[0] =  %.*e; xk[1] =  %.*e\n", DECIMAL_DIG, xk[0], DECIMAL_DIG, xk[1]);
      }

    }
    xk[0] = F[0];
    xk[1] = F[1];
    t = k*sim_data.h;
  }
  solver_options_delete(options);
  options = NULL;
  free(problem->nabla_F);
  problem->nabla_F = NULL;
  free(problem);
  problem = NULL;
  return info;
}