/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2020 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 <math.h>                          // for fabs
#include <stdio.h>                         // for printf, fclose, fopen, snp...
#include <stdlib.h>                        // for malloc, free, abs
#include <string.h>                        // for strncpy, NULL, memcpy
#include "CSparseMatrix_internal.h"                 // for CSparseMatrix, CS_INT
#include "GlobalFrictionContactProblem.h"  // for GlobalFrictionContactProblem
#include "NonSmoothDrivers.h"              // for gfc3d_driver, gfc3d_LmgcDr...
#include "NumericsFwd.h"                   // for NumericsMatrix, GlobalFric...
#include "NumericsMatrix.h"                // for NumericsMatrix, NM_clear
#include "NumericsSparseMatrix.h"          // for NSM_new, NumericsSparseMatrix
#include "SolverOptions.h"                 // for SolverOptions, SICONOS_IPA...
#include "debug.h"                         // for DEBUG_PRINTF
#include "numerics_verbose.h"              // for verbose

#include "SiconosConfig.h" // for WITH_FCLIB  // IWYU pragma: keep

#ifdef WITH_FCLIB
// avoid a conflict with old csparse.h
#define _CS_H
#include <sys/stat.h>                      // for stat, mkdir
static int gfccounter =-1;

#endif

//#define USE_NM_DENSE


static double * alloc_memory_double(unsigned int size, double *p)
{
  double * r = (double *) malloc(size * sizeof(double));
  memcpy(r, p, size * sizeof(double));
  return r;
}

static CS_INT * alloc_memory_csi(unsigned int size, unsigned int *p)
{
  CS_INT * r = (CS_INT *) malloc(size * sizeof(CS_INT));
  for(unsigned int i=0; i<size; ++i)
  {
    r[i] = (CS_INT) p[i];
  }
  return r;
}

int globalFrictionContact_fclib_write(
  GlobalFrictionContactProblem* problem,
  char * title,
  char * description,
  char * mathInfo,
  const char *path);

int gfc3d_LmgcDriver(double *reaction,
                     double *velocity,
                     double *globalVelocity,
                     double *q,
                     double *b,
                     double *mu,
                     double *Mdata,
                     unsigned int nzM,
                     unsigned int *rowM,
                     unsigned int *colM,
                     double* Hdata,
                     unsigned int nzH,
                     unsigned int *rowH,
                     unsigned int *colH,
                     unsigned int n,
                     unsigned int nc,
                     int solver_id,
                     int isize,
                     int *iparam,
                     int dsize,
                     double *dparam,
                     int verbose_in,
                     int outputFile,
                     int freq_output)
{


  verbose = verbose_in;

  /* NumericsMatrix M, H; */
  NumericsMatrix * M =NM_new();
  M->storageType = 2; /* sparse */
  M->size0 = n;
  M->size1 = n;


  NumericsMatrix * H =NM_new();
  H->storageType = 2;
  H->size0 = M->size0;
  H->size1 = 3 * nc;

  NumericsSparseMatrix * SM =NSM_new();
  M->matrix2 = SM;
  SM->triplet = (CSparseMatrix *)malloc(sizeof(CSparseMatrix));
  CSparseMatrix * _M = SM->triplet;
  SM->origin = NSM_TRIPLET;

  CS_INT * _colM = alloc_memory_csi(nzM, colM);
  CS_INT * _rowM = alloc_memory_csi(nzM, rowM);

  _M->nzmax = nzM;
  _M->nz = nzM;
  _M->m = M->size0;
  _M->n = M->size1;
  _M->p = (CS_INT *) _colM;
  _M->i = (CS_INT *) _rowM;
  double * _Mdata = alloc_memory_double(nzM, Mdata);
  _M->x = _Mdata;

  DEBUG_PRINTF("_M->n=%lli\t",_M->n);
  DEBUG_PRINTF("_M->m=%lli\n",_M->m);

  NumericsSparseMatrix * SH =NSM_new();
  H->matrix2 = SH;
  SH->triplet = (CSparseMatrix *)malloc(sizeof(CSparseMatrix));
  CSparseMatrix * _H = SH->triplet;
  SH->origin = NSM_TRIPLET;

  CS_INT * _colH = alloc_memory_csi(nzH, colH);
  CS_INT * _rowH = alloc_memory_csi(nzH, rowH);

  _H->nzmax = nzH;
  _H->nz = nzH;
  _H->m = H->size0;
  _H->n = H->size1;

  _H->p = _colH;
  _H->i =  _rowH;
  double * _Hdata = alloc_memory_double(nzH, Hdata);
  _H->x = _Hdata;

  for(int i=0; i< _M->nz; ++i)
  {
    _M->p[i] --;
    _M->i[i] --;
    /* DEBUG_PRINTF("%d -> %d,%d\n", i, _M->p[i], _M->i[i]); */

  }

  for(int i=0; i< _H->nz; ++i)
  {
    _H->p[i] --;
    _H->i[i] --;
    /* DEBUG_PRINTF("%d -> %d,%d\n", i, _H->p[i], _H->i[i]); */
  }



#ifdef USE_NM_DENSE
  assert(M);
  assert(H);

  NumericsMatrix *MMtmp = NM_new();
  NumericsMatrix *HHtmp = NM_new();

  NM_copy(M,MMtmp);
  NM_copy(H,HHtmp);

  NM_clearSparse(M);
  NM_clearSparse(H);

  M = NM_create(NM_DENSE, H->size0, H->size0);
  H = NM_create(NM_DENSE, H->size0, H->size1);

  NM_to_dense(MMtmp,M);
  NM_to_dense(HHtmp,H);

  /* NM_display(M); */
  /* NM_display(H); */

#endif



  GlobalFrictionContactProblem * problem =(GlobalFrictionContactProblem*)malloc(sizeof(GlobalFrictionContactProblem));

  problem->dimension = 3;
  problem->numberOfContacts = nc;
  problem->env = NULL;

  problem->M = M;
  problem->H = H;
  problem->q = q;
  problem->b = b;
  problem->mu = mu;

  SolverOptions * numerics_solver_options = solver_options_create(solver_id);
  int iSize_min = isize < numerics_solver_options->iSize ? isize : numerics_solver_options->iSize;
  DEBUG_PRINTF("iSize_min = %i", iSize_min);
  for(int i = 0; i < iSize_min; ++i)
    if(abs(iparam[i])>0)
      numerics_solver_options->iparam[i] = iparam[i];

  int dSize_min = dsize <  numerics_solver_options->dSize ? dsize : numerics_solver_options->dSize;
  for(int i=0; i < dSize_min; ++i)
    if(fabs(dparam[i]) > 0)
      numerics_solver_options->dparam[i] = dparam[i];

  /* solver_options_print(&numerics_solver_options); */
  /* FILE * file  =  fopen("toto.dat", "w"); */
  /* globalFrictionContact_printInFile(problem, file); */
  /* fclose(file); */
  int rinfo =  gfc3d_driver(problem,
                            reaction,
                            velocity,
                            globalVelocity,
                            numerics_solver_options);


  iparam[SICONOS_IPARAM_ITER_DONE] = numerics_solver_options->iparam[SICONOS_IPARAM_ITER_DONE];
  dparam[SICONOS_DPARAM_TOL] = numerics_solver_options->dparam[SICONOS_DPARAM_TOL];

  /* FILE * file1  =  fopen("tutu.dat", "w"); */
  /* globalFrictionContact_printInFile(problem, file1); */
  /* fclose(file1); */
  if(outputFile == 1)
  {
    /* dump in C format */
  }
  else if(outputFile == 2)
  {
    /* dump in Numerics .dat format */
  }
  else if(outputFile == 3)
  {
#ifdef WITH_FCLIB
    gfccounter++;
    struct stat st = {};
    if(stat("./fclib-hdf5/", &st) == -1)
    {
      mkdir("./fclib-hdf5/", 0700);
    }
    printf("################################## gfcccounter = %i\n", gfccounter);
    if(gfccounter % freq_output == 0)
    {
      char fname[256];
      snprintf(fname, sizeof(fname), "./fclib-hdf5/LMGC_GFC3D-i%.5d-%i-%.5d.hdf5", numerics_solver_options->iparam[SICONOS_IPARAM_ITER_DONE], (int)nc, gfccounter);
      printf("Dump ./fclib-hdf5/LMGC_GFC3D-i%.5d-%i-%.5d.hdf5.\n", numerics_solver_options->iparam[SICONOS_IPARAM_ITER_DONE], (int)nc, gfccounter);
      /* printf("ndof = %i.\n", ndof); */

      FILE * foutput  =  fopen(fname, "w");
      int n = 100;
      char * title = (char *)malloc(n * sizeof(char));
      strncpy(title, "LMGC dump in hdf5", n);
      char * description = (char *)malloc(n * sizeof(char));

      snprintf(description, n, "Rewriting in hdf5 through siconos of %s in FCLIB format", fname);
      char * mathInfo = (char *)malloc(n * sizeof(char));
      strncpy(mathInfo, "unknown", n);

      globalFrictionContact_fclib_write(problem,
                                        title,
                                        description,
                                        mathInfo,
                                        fname);


      fclose(foutput);
    }
#else
    printf("Fclib is not available ...\n");
#endif

  }

  solver_options_delete(numerics_solver_options);
  numerics_solver_options = NULL;
  NM_clear(M);
  NM_clear(H);
  free(M);
  free(H);
  free(problem);

  /* free(_colM); */
  /* free(_colH); */

  /* free(_rowM); */
  /* free(_rowH); */

  return rinfo;
}


/* int gfc3d_LmgcDriver_SBM(double *reaction, */
/*                                        double *velocity, */
/*                                        double *globalVelocity, */
/*                                        double *q, */
/*                                        double *b, */
/*                                        double *mu, */
/*                                        double *Mdata, */
/*                                        unsigned int nzM, */
/*                                        unsigned int *rowM, */
/*                                        unsigned int *colM, */
/*                                        double* Hdata, */
/*                                        unsigned int nzH, */
/*                                        unsigned int *rowH, */
/*                                        unsigned int *colH, */
/*                                        unsigned int n, */
/*                                        unsigned int nc, */
/*                                        int solver_id, */
/*                                        int isize, */
/*                                        int *iparam, */
/*                                        int dsize, */
/*                                        double *dparam, */
/*                                        int verbose, */
/*                                        int outputFile) */
/* { */
/*   GlobalFrictionContactProblem problem; */
/*   NumericsMatrix M, H; */

/*   CSparseMatrix _M, _H; */

/*   unsigned int * _colM = alloc_memory_int(nzM, colM); */
/*   unsigned int * _rowM = alloc_memory_int(nzM, rowM); */

/*   unsigned int * _colH = alloc_memory_int(nzH, colH); */
/*   unsigned int * _rowH = alloc_memory_int(nzH, rowH); */


/*   M.matrix0 = NULL; */
/*   M.matrix1 = NULL; */
/*   M.matrix2 = NULL; */
/*   M.internalData = NULL; */

/*   M.storageType = 2; /\* csc *\/ */
/*   M.size0 = n; */
/*   M.size1 = n; */

/*   _M.nzmax = nzM; */
/*   _M.nz = nzM; */
/*   _M.m = M.size0; */
/*   _M.n = M.size1; */
/*   _M.p = (int *) _colM; */
/*   _M.i = (int *) _rowM; */
/*   _M.x = Mdata; */


/*   H.matrix0 = NULL; */
/*   H.matrix1 = NULL; */
/*   H.matrix2 = NULL; */
/*   H.internalData = NULL; */

/*   H.storageType = 2; */
/*   H.size0 = M.size0; */
/*   H.size1 = 3 * nc; */

/*   _H.nzmax = nzH; */
/*   _H.nz = nzH; */
/*   _H.m = H.size0; */
/*   _H.n = H.size1; */

/*   _H.p = (int *) _colH; */
/*   _H.i = (int *) _rowH; */
/*   _H.x = Hdata; */

/*   M.matrix2->triplet = &_M; */
/*   H.matrix2->triplet = &_H; */

/*   for (int i=0; i< _M.nz; ++i) */
/*   { */
/*     _M.p[i] --; */
/*     _M.i[i] --; */
/*   } */

/*   for (int i=0; i< _H.nz; ++i) */
/*   { */
/*     _H.p[i] --; */

/*     _H.i[i] --; */

/*     DEBUG_PRINTF("%d -> %d,%d\n", i, _H.p[i], _H.i[i]); */

/*   } */


/*   problem.dimension = 3; */
/*   problem.numberOfContacts = nc; */

/*   problem.M = &M; */
/*   problem.H = &H; */
/*   problem.q = q; */
/*   problem.b = b; */
/*   problem.mu = mu; */

/*   SolverOptions numerics_solver_options; */

/*   gfc3d_setDefaultSolverOptions(&numerics_solver_options, solver_id); */

/*   assert(isize <= numerics_solver_options->iSize); */
/*   assert(dsize <= numerics_solver_options->dSize); */

/*   for (int i=0; i<isize; ++i) */
/*     numerics_solver_options->iparam[i] = iparam[i]; */

/*   for (int i=0; i<dsize; ++i) */
/*     numerics_solver_options->dparam[i] = dparam[i]; */

/*   int rinfo = gfc3d_driver(&problem, */
/*                                              reaction, */
/*                                              velocity, */
/*                                              globalVelocity, */
/*                                              &numerics_solver_options); */

/*   if(outputFile == 1) */
/*   { */
/*     /\* dump in C format *\/ */
/*   } */
/*   else if (outputFile == 2) */
/*   { */
/*     /\* dump in Numerics .dat format *\/ */
/*   } */
/*   else if (outputFile == 3) */
/*   { */
/* #ifdef WITH_FCLIB */
/*     char fname[256]; */
/*     sprintf(fname, "LMGC_GlobalFrictionContactProblem%.5d.hdf5", gfccounter++); */
/*     printf("Dump of LMGC_GlobalFrictionContactProblem%.5d.hdf5", gfccounter); */

/*     FILE * foutput  =  fopen(fname, "w"); */
/*     int n = 100; */
/*     char * title = (char *)malloc(n * sizeof(char *)); */
/*     strcpy(title, "LMGC dump in hdf5"); */
/*     char * description = (char *)malloc(n * sizeof(char *)); */

/*     strcat(description, "Rewriting in hdf5 through siconos of  "); */
/*     strcat(description, fname); */
/*     strcat(description, " in FCLIB format"); */
/*     char * mathInfo = (char *)malloc(n * sizeof(char *)); */
/*     strcpy(mathInfo,  "unknown"); */

/*     globalFrictionContact_fclib_write(&problem, */
/*                                       title, */
/*                                       description, */
/*                                       mathInfo, */
/*                                       fname); */


/*     fclose(foutput); */
/* #else */
/*     printf("Fclib is not available ...\n"); */
/* #endif */

/*   } */


/*   NM_clear(&M); */
/*   NM_clear(&H); */

/*   free(_colM); */
/*   free(_colH); */

/*   free(_rowM); */
/*   free(_rowH); */

/*   return rinfo; */
/* } */



/* int gfc3d_LmgcDriver(double *reaction, */
/*                                        double *velocity, */
/*                                        double *globalVelocity, */
/*                                        double *q, */
/*                                        double *b, */
/*                                        double *mu, */
/*                                        double *Mdata, */
/*                                        unsigned int nzM, */
/*                                        unsigned int *rowM, */
/*                                        unsigned int *colM, */
/*                                        double* Hdata, */
/*                                        unsigned int nzH, */
/*                                        unsigned int *rowH, */
/*                                        unsigned int *colH, */
/*                                        unsigned int n, */
/*                                        unsigned int nc, */
/*                                        int solver_id, */
/*                                        int isize, */
/*                                        int *iparam, */
/*                                        int dsize, */
/*                                        double *dparam, */
/*                                        int verbose, */
/*                                        int outputFile) */
/* { */
/*   return gfc3d_LmgcDriver_CSC(reaction, */
/*                                                 velocity, */
/*                                                 globalVelocity, */
/*                                                 q, */
/*                                                 b, */
/*                                                 mu, */
/*                                                 Mdata, */
/*                                                 nzM, */
/*                                                 rowM, */
/*                                                 colM, */
/*                                                 Hdata, */
/*                                                 nzH, */
/*                                                 rowH, */
/*                                                 colH, */
/*                                                 n, */
/*                                                 nc, */
/*                                                 solver_id, */
/*                                                 isize, */
/*                                                 iparam, */
/*                                                 dsize, */
/*                                                 dparam, */
/*                                                 verbose, */
/*                                                 outputFile); */

/* } */
