void gfc3d_init_workspace(GlobalFrictionContactProblem* problem)
{
  assert(problem);
  assert(problem->M);

  if (!problem->workspace)
  {
    problem->workspace = (GFC3D_workspace*) malloc(sizeof(GFC3D_workspace));
    problem->workspace->factorized_M = NULL;
    problem->workspace->globalVelocity = NULL;
  }

  if (!problem->workspace->factorized_M)
  {
    problem->workspace->factorized_M = createNumericsMatrix(problem->M->storageType,
                                               problem->M->size0,
                                               problem->M->size1);
    NM_copy(problem->M, problem->workspace->factorized_M);
  }

  if (!problem->workspace->globalVelocity)
  {
    problem->workspace->globalVelocity = (double*)malloc(problem->M->size1 * sizeof(double));
  }
}
int main(void)
{
  NumericsOptions NO;
  setDefaultNumericsOptions(&NO);
  NO.verboseMode = 1; // turn verbose mode to off by default

  int total_info = 0;

  double q[] = { -1, 1, 3, -1, 1, 3, -1, 1, 3};
  double mu[] = {0.1, 0.1, 0.1};

  double Wdata[81] = {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};

  NumericsMatrix* tmpM = createNumericsMatrixFromData(NM_DENSE, 9, 9, Wdata);
  NumericsMatrix* W = createNumericsMatrix(NM_SPARSE, 9, 9);
  NM_copy_to_sparse(tmpM, W);

  int solvers_to_test[] = {SICONOS_FRICTION_3D_NSGS,
                           SICONOS_FRICTION_3D_NSN_AC,
                           SICONOS_FRICTION_3D_NSN_FB,
                           SICONOS_FRICTION_3D_NSN_NM,
                           SICONOS_FRICTION_3D_SOCLCP,
                           SICONOS_FRICTION_3D_PROX};

  for (size_t s = 0; s < sizeof(solvers_to_test); ++s)
  {
    int solver_id = solvers_to_test[s];

    FrictionContactProblem* FC = frictionContactProblem_new(3, 3, W, q, mu);
    double r[9] = {0.};
    double u[9] = {0.};

    SolverOptions SO;;
    fc3d_setDefaultSolverOptions(&SO, solver_id);
    int info = fc3d_driver(FC, r, u, &SO, &NO);

    if (info)
    {
      fprintf(stderr, "Solver %s failed with error %d\n", idToName(solver_id), info);
      total_info = 1;
    }
    FC->M = NULL;
    FC->q = NULL;
    FC->mu = NULL;
    deleteSolverOptions(&SO);
    freeFrictionContactProblem(FC);
    free(FC);
  }

  freeNumericsMatrix(W);
  tmpM->matrix0 = NULL;
  freeNumericsMatrix(tmpM);
  free(W);
  free(tmpM);

  return total_info;
}
void fc3d_nonsmooth_Newton_solvers_solve(fc3d_nonsmooth_Newton_solvers* equation,
                                      double* reaction,
                                      double* velocity,
                                      int* info,
                                      SolverOptions* options)
{


  assert(equation);

  FrictionContactProblem* problem = equation->problem;

  assert(problem);
  assert(reaction);
  assert(velocity);
  assert(info);
  assert(options);

  assert(problem->dimension == 3);

  assert(options->iparam);
  assert(options->dparam);

  assert(problem->q);
  assert(problem->mu);
  assert(problem->M);

  assert(problem->M->matrix0 || problem->M->matrix1 || problem->M->matrix2);

  assert(!options->iparam[4]); // only host

  unsigned int problemSize = 3 * problem->numberOfContacts;

  unsigned int iter = 0;
  unsigned int itermax = options->iparam[0];
  unsigned int erritermax = options->iparam[7];

  int nzmax;

  if (problem->M->storageType == NM_DENSE)
  {
    nzmax = problemSize * problemSize;
  }
  else
  {
    nzmax = options->iparam[3];
  }

  assert(itermax > 0);
  assert(nzmax > 0);

  double tolerance = options->dparam[0];
  assert(tolerance > 0);
  
  if (verbose > 0)
    printf("------------------------ FC3D - _nonsmooth_Newton_solversSolve - Start with tolerance = %g\n", tolerance);

  unsigned int _3problemSize = 3 * problemSize;
  double normq = cblas_dnrm2(problemSize , problem->q , 1);
 
  void *buffer;

  if (!options->dWork)
  {
    buffer = malloc((11 * problemSize) * sizeof(double)); // F(1),
                                                          // tmp1(1),
                                                          // tmp2(1),
                                                          // tmp3(1),
                                                          // A(3),
                                                          // B(3), rho
  }
  else
  {
    buffer = options->dWork;
  }
  double *F = (double *) buffer;
  double *tmp1 = (double *) F + problemSize;
  double *tmp2 = (double *) tmp1 + problemSize;
  double *tmp3 = (double *) tmp2 + problemSize;
  double *Ax = tmp3 + problemSize;
  double *Bx = Ax + _3problemSize;
  double *rho = Bx + _3problemSize;

  NumericsMatrix *AWpB, *AWpB_backup;
  if (!options->dWork)
  {
    AWpB = createNumericsMatrix(problem->M->storageType,
        problem->M->size0, problem->M->size1);

    AWpB_backup = createNumericsMatrix(problem->M->storageType,
        problem->M->size0, problem->M->size1);
  }
  else
  {
    AWpB = (NumericsMatrix*) (rho + problemSize);
    AWpB_backup = (NumericsMatrix*) (AWpB + sizeof(NumericsMatrix*));
  }

  /* just for allocations */
  NM_copy(problem->M, AWpB);

  if (problem->M->storageType != NM_DENSE)
  {
    switch(options->iparam[13])
    {
      case 0:
        {
          NM_linearSolverParams(AWpB)->solver = NS_CS_LUSOL;
          break;
        }
      case 1:
        {
          NM_linearSolverParams(AWpB)->solver = NS_MUMPS;

#ifdef HAVE_MPI

          assert (options->solverData);

          if ((MPI_Comm) options->solverData == MPI_COMM_NULL)
          {
            options->solverData = NM_MPI_com(MPI_COMM_NULL);
          }
          else
          {
            NM_MPI_com((MPI_Comm) options->solverData);
          }

#endif
          break;
        }
      default:
        {
          numerics_error("fc3d_nonsmooth_Newton_solvers_solve", "Unknown linear solver.\n");
        }
    }
  }

  // compute rho here
  for (unsigned int i = 0; i < problemSize; ++i) rho[i] = options->dparam[3];

  // velocity <- M*reaction + qfree
  cblas_dcopy(problemSize, problem->q, 1, velocity, 1);
  NM_gemv(1., problem->M, reaction, 1., velocity);
  
  double linear_solver_residual=0.0;
  
  while (iter++ < itermax)
  {

    equation->function(equation->data,
                       problemSize,
                       reaction, velocity, equation->problem->mu,
                       rho,
                       F, Ax, Bx);

    // AW + B
    computeAWpB(Ax, problem->M, Bx, AWpB);

    cblas_dcopy_msan(problemSize, F, 1, tmp1, 1);
    cblas_dscal(problemSize, -1., tmp1, 1);

    /* Solve: AWpB X = -F */
    NM_copy(AWpB, AWpB_backup);
    int lsi = NM_gesv(AWpB, tmp1);

    /* NM_copy needed here */
    NM_copy(AWpB_backup, AWpB);

    if (lsi)
    {
      if (verbose > 0)
      {
        numerics_warning("fc3d_nonsmooth_Newton_solvers_solve",
                         "warning! linear solver exit with code = %d\n", lsi);
      }
    }

    if (verbose > 0)
    {
      cblas_dcopy_msan(problemSize, F, 1, tmp3, 1);
      NM_gemv(1., AWpB, tmp1, 1., tmp3);
      linear_solver_residual = cblas_dnrm2(problemSize, tmp3, 1);
      /* fprintf(stderr, "fc3d esolve: linear equation residual = %g\n", */
      /*         cblas_dnrm2(problemSize, tmp3, 1)); */
      /* for the component wise scaled residual: cf mumps &
       * http://www.netlib.org/lapack/lug/node81.html */
    }
    // line search
    double alpha = 1;
    int info_ls = 0;

    cblas_dcopy_msan(problemSize, tmp1, 1, tmp3, 1);

    switch (options->iparam[11])
    {
    case -1:
      /* without line search */
      info_ls = 1;
      break;

    case 0:
      /* Goldstein Price */
      info_ls = globalLineSearchGP(equation, reaction, velocity, problem->mu, rho, F, Ax, Bx, problem->M, problem->q, AWpB, tmp1, tmp2, &alpha, options->iparam[12]);
      break;
    case 1:
      /* FBLSA */
      info_ls = frictionContactFBLSA(equation, reaction, velocity, problem->mu, rho, F, Ax, Bx,
                                     problem->M, problem->q, AWpB, tmp1, tmp2, &alpha, options->iparam[12]);
      break;
    default:
      {
        numerics_error("fc3d_nonsmooth_Newton_solvers_solve",
                       "Unknown line search option.\n");
      }
    }

    if (!info_ls)
      // tmp2 should contains the reaction iterate of the line search
      //  for GP this should be the same as cblas_daxpy(problemSize, alpha, tmp1, 1, reaction, 1);
      cblas_dcopy(problemSize, tmp2, 1, reaction, 1);
    else
      cblas_daxpy(problemSize, 1., tmp3, 1., reaction, 1);


    // velocity <- M*reaction + qfree
    cblas_dcopy(problemSize, problem->q, 1, velocity, 1);
    NM_gemv(1., problem->M, reaction, 1., velocity);

    options->dparam[1] = INFINITY;

    if (!(iter % erritermax))
    {

      fc3d_compute_error(problem, reaction, velocity,
//      fc3d_FischerBurmeister_compute_error(problem, reaction, velocity,
                         tolerance, options, normq, &(options->dparam[1]));

      DEBUG_EXPR_WE(equation->function(equation->data, problemSize,
                                       reaction, velocity, equation->problem->mu, rho,
                                       F, NULL, NULL));


      DEBUG_EXPR_WE(assert((cblas_dnrm2(problemSize, F, 1)
                            / (1 + cblas_dnrm2(problemSize, problem->q, 1)))
                           <= (10 * options->dparam[1] + 1e-15)));

    }

    if (verbose > 0)
    {
      equation->function(equation->data, problemSize,
                         reaction, velocity, equation->problem->mu, rho,
                         F, NULL, NULL);

      printf("   ---- fc3d_nonsmooth_Newton_solvers_solve: iteration %d : , linear solver residual =%g, residual=%g, ||F||=%g\n", iter, linear_solver_residual, options->dparam[1],cblas_dnrm2(problemSize, F, 1));
    }

    if (options->callback)
    {
      options->callback->collectStatsIteration(options->callback->env, problemSize, reaction, velocity,
                                      options->dparam[1], NULL);
    }

    if (isnan(options->dparam[1]))
    {
       if (verbose > 0)
       {
         printf("            fc3d_nonsmooth_Newton_solvers_solve: iteration %d : computed residual is not a number, stop.\n", iter);
       }
       info[0] = 2;
       break;
    }

    if (options->dparam[1] < tolerance)
    {
      info[0] = 0;
      break;
    }

  }

  if (verbose > 0)
  {
    if (!info[0])
      printf("------------------------ FC3D - NSN - convergence after %d iterations, residual : %g < %g \n",  iter, options->dparam[1],tolerance);
    else
    {
      printf("------------------------ FC3D - NSN - no convergence after %d iterations, residual : %g  < %g \n",  iter, options->dparam[1], tolerance);
    }
  }

  options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;

  if (!options->dWork)
  {
    assert(buffer);
    free(buffer);
    options->dWork = NULL;
  }
  else
  {
    assert(buffer == options->dWork);
  }

  if (!options->dWork)
  {
    freeNumericsMatrix(AWpB);
    freeNumericsMatrix(AWpB_backup);

    free(AWpB);
    free(AWpB_backup);
  }
  if (verbose > 0)
    printf("------------------------ FC3D - NSN - End\n");

}
/* Alart & Curnier solver for sparse global problem */
void gfc3d_nonsmooth_Newton_AlartCurnier(
  GlobalFrictionContactProblem* problem,
  double *reaction,
  double *velocity,
  double *globalVelocity,
  int *info,
  SolverOptions* options)
{

  assert(problem);
  assert(reaction);
  assert(velocity);
  assert(info);
  assert(options);

  assert(problem->dimension == 3);

  assert(options->iparam);
  assert(options->dparam);

  assert(problem->q);
  assert(problem->mu);
  assert(problem->M);
  assert(problem->H);

  assert(!problem->M->matrix0);
//  assert(problem->M->matrix1);

  assert(!options->iparam[4]); // only host

  /* M is square */
  assert(problem->M->size0 == problem->M->size1);

  assert(problem->M->size0 == problem->H->size0);

  unsigned int iter = 0;
  unsigned int itermax = options->iparam[0];
  unsigned int erritermax = options->iparam[7];

  if (erritermax == 0)
  {
    /* output a warning here */
    erritermax = 1;
  }

  assert(itermax > 0);
  assert(options->iparam[3] > 0);

  double tolerance = options->dparam[0];
  assert(tolerance > 0);

  if (verbose > 0)
    printf("------------------------ GFC3D - _nonsmooth_Newton_AlartCurnier - Start with tolerance = %g\n", tolerance);


  /* sparse triplet storage */
  NM_triplet(problem->M);
  NM_triplet(problem->H);

  unsigned int ACProblemSize = sizeOfPsi(NM_triplet(problem->M),
                                         NM_triplet(problem->H));

  unsigned int globalProblemSize = (unsigned)NM_triplet(problem->M)->m;

  unsigned int localProblemSize = problem->H->size1;

  assert((int)localProblemSize == problem->numberOfContacts * problem->dimension);

  assert((int)globalProblemSize == problem->H->size0); /* size(velocity) ==
                                                   * Htrans*globalVelocity */


  AlartCurnierFun3x3Ptr computeACFun3x3 = NULL;

  switch (options->iparam[10])
  {
  case 0:
  {
    computeACFun3x3 = &computeAlartCurnierSTD;
    break;
  }
  case 1:
  {
    computeACFun3x3 = &computeAlartCurnierJeanMoreau;
    break;
  };
  case 2:
  {
    computeACFun3x3 = &fc3d_AlartCurnierFunctionGenerated;
    break;
  }
  case 3:
  {
    computeACFun3x3 = &fc3d_AlartCurnierJeanMoreauFunctionGenerated;
    break;
  }
  }

  if(options->iparam[9] == 0)
  {
    /* allocate memory */
    assert(options->dWork == NULL);
    assert(options->iWork == NULL);
    options->dWork = (double *) malloc(
                       (localProblemSize + /* F */
                        3 * localProblemSize + /* A */
                        3 * localProblemSize + /* B */
                        localProblemSize + /* rho */
                        ACProblemSize + /* psi */
                        ACProblemSize + /* rhs */
                        ACProblemSize + /* tmp2 */
                        ACProblemSize + /* tmp3 */
                        ACProblemSize   /* solution */) *  sizeof(double));

    /* XXX big hack here */
    options->iWork = (int *) malloc(
                       (3 * localProblemSize + /* iA */
                        3 * localProblemSize + /* iB */
                        3 * localProblemSize + /* pA */
                        3 * localProblemSize)  /* pB */
                       * sizeof(csi));

    options->iparam[9] = 1;

  }

  assert(options->dWork != NULL);
  assert(options->iWork != NULL);

  double *F = options->dWork;
  double *A = F +   localProblemSize;
  double *B = A +   3 * localProblemSize;
  double *rho = B + 3 * localProblemSize;

  double * psi = rho + localProblemSize;
  double * rhs = psi + ACProblemSize;
  double * tmp2 = rhs + ACProblemSize;
  double * tmp3 = tmp2 + ACProblemSize;
  double * solution = tmp3 + ACProblemSize;

  /* XXX big hack --xhub*/
  csi * iA = (csi *)options->iWork;
  csi * iB = iA + 3 * localProblemSize;
  csi * pA = iB + 3 * localProblemSize;
  csi * pB = pA + 3 * localProblemSize;

  CSparseMatrix A_;
  CSparseMatrix B_;
  CSparseMatrix *J;

  A_.p = pA;
  B_.p = pB;
  A_.i = iA;
  B_.i = iB;

  init3x3DiagBlocks(problem->numberOfContacts, A, &A_);
  init3x3DiagBlocks(problem->numberOfContacts, B, &B_);

  J = cs_spalloc(NM_triplet(problem->M)->n + A_.m + B_.m,
                 NM_triplet(problem->M)->n + A_.m + B_.m,
                 NM_triplet(problem->M)->nzmax + 2*NM_triplet(problem->H)->nzmax +
                 2*A_.n + A_.nzmax + B_.nzmax, 1, 1);

  assert(A_.n == problem->H->size1);
  assert(A_.nz == problem->numberOfContacts * 9);
  assert(B_.n == problem->H->size1);
  assert(B_.nz == problem->numberOfContacts * 9);

  fc3d_AlartCurnierFunction(
    localProblemSize,
    computeACFun3x3,
    reaction, velocity,
    problem->mu, rho,
    F, A, B);

  csi Astart = initACPsiJacobian(NM_triplet(problem->M),
                                 NM_triplet(problem->H),
                                 &A_, &B_, J);

  assert(Astart > 0);

  assert(A_.m == A_.n);
  assert(B_.m == B_.n);

  assert(A_.m == problem->H->size1);

  // compute rho here
  for(unsigned int i = 0; i < localProblemSize; ++i) rho[i] = 1.;

  // direction
  for(unsigned int i = 0; i < ACProblemSize; ++i) rhs[i] = 0.;



  // quick hack to make things work
  // need to use the functions from NumericsMatrix --xhub


  NumericsMatrix *AA_work = createNumericsMatrix(NM_SPARSE,  (int)J->m, (int)J->n);

  NumericsSparseMatrix* SM = newNumericsSparseMatrix();
  SM->triplet = J;
  NumericsMatrix *AA = createNumericsMatrixFromData(NM_SPARSE,  (int)J->m, (int)J->n, SM);

  info[0] = 1;

  /* update local velocity from global velocity */
  /* an assertion ? */
  cblas_dcopy(localProblemSize, problem->b, 1, velocity, 1);
  NM_tgemv(1., problem->H, globalVelocity, 1, velocity);
  double linear_solver_residual=0.0;
  while(iter++ < itermax)
  {

    /* compute psi */
    ACPsi(problem, computeACFun3x3, globalVelocity, reaction, velocity, rho, psi);

    /* compute A & B */
    fc3d_AlartCurnierFunction(localProblemSize,
                              computeACFun3x3,
                              reaction, velocity,
                              problem->mu, rho,
                              F, A, B);
    /* update J */
    updateACPsiJacobian(NM_triplet(problem->M),
                        NM_triplet(problem->H),
                        &A_, &B_, J, Astart);

    /* rhs = -psi */
    cblas_dcopy(ACProblemSize, psi, 1, rhs, 1);
    cblas_dscal(ACProblemSize, -1., rhs, 1);

    /* get compress column storage for linear ops */
    CSparseMatrix* Jcsc = cs_compress(J);

    /* Solve: J X = -psi */

    /* Solve: AWpB X = -F */
    NM_copy(AA, AA_work);

    int info_solver = NM_gesv(AA_work, rhs);
    if (info_solver > 0)
    {
      fprintf(stderr, "------------------------ GFC3D - NSN_AC - solver failed info = %d\n", info_solver);
      break;
      info[0] = 2;
      CHECK_RETURN(!cs_check_triplet(NM_triplet(AA_work)));
    }

    /* Check the quality of the solution */
    if (verbose > 0)
    {
      cblas_dcopy_msan(ACProblemSize, psi, 1, tmp3, 1);
      NM_gemv(1., AA, rhs, 1., tmp3);
      linear_solver_residual = cblas_dnrm2(ACProblemSize, tmp3, 1);
      /* fprintf(stderr, "fc3d esolve: linear equation residual = %g\n", */
      /*         cblas_dnrm2(problemSize, tmp3, 1)); */
      /* for the component wise scaled residual: cf mumps &
       * http://www.netlib.org/lapack/lug/node81.html */
    }

    /* line search */
    double alpha = 1;

    /* set current solution */
    for(unsigned int i = 0; i < globalProblemSize; ++i)
    {
      solution[i] = globalVelocity[i];
    }
    for(unsigned int i = 0; i < localProblemSize; ++i)
    {
      solution[i+globalProblemSize] = velocity[i];
      solution[i+globalProblemSize+localProblemSize] = reaction[i];
    }

    DEBUG_EXPR_WE(
      for(unsigned int i = 0; i < globalProblemSize; ++i)
      {
        printf("globalVelocity[%i] = %6.4e\n",i,globalVelocity[i]);
      }
      for(unsigned int i = 0; i < localProblemSize; ++i)
      {
        printf("velocity[%i] = %6.4e\t",i,velocity[i]);
        printf("reaction[%i] = %6.4e\n",i,reaction[i]);
      }
      );


    int info_ls = _globalLineSearchSparseGP(problem,
                                            computeACFun3x3,
                                            solution,
                                            rhs,
                                            globalVelocity,
                                            reaction, velocity,
                                            problem->mu, rho, F, psi, Jcsc,
                                            tmp2, &alpha, 100);


    cs_spfree(Jcsc);
    if(!info_ls)
    {
      cblas_daxpy(ACProblemSize, alpha, rhs, 1, solution, 1);
    }
    else
    {
      cblas_daxpy(ACProblemSize, 1, rhs, 1., solution, 1);
    }

    for(unsigned int e = 0 ; e < globalProblemSize; ++e)
    {
      globalVelocity[e] = solution[e];
    }

    for(unsigned int e = 0 ; e < localProblemSize; ++e)
    {
      velocity[e] = solution[e+globalProblemSize];
    }

    for(unsigned int e = 0; e < localProblemSize; ++e)
    {
      reaction[e] = solution[e+globalProblemSize+localProblemSize];
    }

    options->dparam[1] = INFINITY;

    if(!(iter % erritermax))
    {

      gfc3d_compute_error(problem,
                          reaction, velocity, globalVelocity,
                          tolerance,
                          &(options->dparam[1]));
    }

    if(verbose > 0)
      printf("------------------------ GFC3D - NSN_AC - iteration %d, residual = %g, linear solver residual = %g, tolerance = %g \n", iter, options->dparam[1],linear_solver_residual, tolerance);

    if(options->dparam[1] < tolerance)
    {
      info[0] = 0;
      break;
    }


  }