/* 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 "RollingFrictionContactProblem.h"
#include <assert.h>            // for assert
#include <math.h>              // for fabs
#include <stdio.h>             // for printf, fprintf, fscanf, NULL, fclose
#include <stdlib.h>            // for free, malloc, exit, EXIT_FAILURE
#include <sys/errno.h>         // for errno
#include "NumericsMatrix.h"    // for NM_vector_display, NM_display, NM_clear
#include "numerics_verbose.h"  // for CHECK_IO, numerics_printf_verbose
//#define DEBUG_STDOUT
//#define DEBUG_MESSAGES
#include "debug.h"             // for DEBUG_PRINT, DEBUG_PRINTF

void rollingFrictionContact_display(RollingFrictionContactProblem* problem)
{

  assert(problem);
  int n = problem->dimension * problem->numberOfContacts;
  printf("RollingFrictionContact Display :\n-------------\n");
  printf("dimension :%d \n", problem->dimension);
  printf("numberOfContacts:%d \n", problem->numberOfContacts);

  if(problem->M)
  {
    printf("M matrix:\n");
    NM_display(problem->M);
  }
  else
    printf("No M matrix:\n");

  if(problem->q)
  {
    printf("q vector:\n");
    NM_vector_display(problem->q,n);
  }
  else
    printf("No q vector:\n");

  if(problem->mu)
  {
    printf("mu vector:\n");
    NM_vector_display(problem->mu,problem->numberOfContacts);
  }
  else
    printf("No mu vector:\n");

  if(problem->mu_r)
  {
    printf("mu_R vector:\n");
    NM_vector_display(problem->mu_r,problem->numberOfContacts);
  }
  else
    printf("No mu_R vector:\n");

}





int rollingFrictionContact_printInFile(RollingFrictionContactProblem*  problem, FILE* file)
{
  if(! problem)
  {
    fprintf(stderr, "Numerics, RollingFrictionContactProblem printInFile failed, NULL input.\n");
    exit(EXIT_FAILURE);
  }
  int i;

  int d  = problem->dimension;
  fprintf(file, "%d\n", d);
  int nc = problem->numberOfContacts;
  fprintf(file, "%d\n", nc);
  NM_write_in_file(problem->M, file);
  for(i = 0; i < problem->M->size1; i++)
  {
    fprintf(file, "%32.24e ", problem->q[i]);
  }
  fprintf(file, "\n");
  for(i = 0; i < nc; i++)
  {
    fprintf(file, "%32.24e ", problem->mu[i]);
  }
  fprintf(file, "\n");
  for(i = 0; i < nc; i++)
  {
    fprintf(file, "%32.24e ", problem->mu_r[i]);
  }
  fprintf(file, "\n");
  return 0;
}

int rollingFrictionContact_printInFilename(RollingFrictionContactProblem* problem, char* filename)
{
  int info = 0;
  FILE * file = fopen(filename, "w");

  if(!file)
  {
    return errno;
  }

  info = rollingFrictionContact_printInFile(problem, file);

  fclose(file);
  return info;
}

RollingFrictionContactProblem* rollingFrictionContact_newFromFile(FILE* file)
{
  RollingFrictionContactProblem* problem = rollingFrictionContactProblem_new();
  assert(file);
  DEBUG_PRINT("Start -- int rollingFrictionContact_newFromFile(RollingFrictionContactProblem* problem, FILE* file)\n");
  int nc = 0, d = 0;
  int i;
  CHECK_IO(fscanf(file, "%d\n", &d));
  problem->dimension = d;
  DEBUG_PRINTF("problem->dimension = %i \n",problem->dimension);
  CHECK_IO(fscanf(file, "%d\n", &nc));
  problem->numberOfContacts = nc;
  problem->M =  NM_new_from_file(file);

  problem->q = (double *) malloc(problem->M->size1 * sizeof(double));
  for(i = 0; i < problem->M->size1; i++)
  {
    CHECK_IO(fscanf(file, "%lf ", &(problem->q[i])));
  }

  problem->mu = (double *) malloc(nc * sizeof(double));
  for(i = 0; i < nc; i++)
  {
    CHECK_IO(fscanf(file, "%lf ", &(problem->mu[i])));
  }
  problem->mu_r = (double *) malloc(nc * sizeof(double));
  for(i = 0; i < nc; i++)
  {
    CHECK_IO(fscanf(file, "%lf ", &(problem->mu_r[i])));
  }
  DEBUG_PRINT("End --  int rollingFrictionContact_newFromFile(RollingFrictionContactProblem* problem, FILE* file)\n");

  return problem;
}

RollingFrictionContactProblem* rollingFrictionContact_new_from_filename(const char* filename)
{
  RollingFrictionContactProblem* problem = NULL;
  FILE * file = fopen(filename, "r");
  if(!file)
    numerics_error("RollingFrictionContactProblem", "Can not open file ", filename);

  problem = rollingFrictionContact_newFromFile(file);
  fclose(file);
  return problem;
}

void rollingFrictionContactProblem_free(RollingFrictionContactProblem* problem)
{
  assert(problem);
  if(problem->M)
  {
    NM_clear(problem->M);
    free(problem->M);
    problem->M = NULL;
  }

  if(problem->mu)
  {
    free(problem->mu);
    problem->mu = NULL;
  }
  if(problem->mu_r)
  {
    free(problem->mu_r);
    problem->mu_r = NULL;
  }

  if(problem->q)
  {
    free(problem->q);
    problem->q = NULL;
  }

  free(problem);

}

RollingFrictionContactProblem* rollingFrictionContactProblem_new(void)
{
  RollingFrictionContactProblem* fcp = (RollingFrictionContactProblem*) malloc(sizeof(RollingFrictionContactProblem));
  fcp->dimension = 0;
  fcp->numberOfContacts = 0;
  fcp->M = NULL;
  fcp->q = NULL;
  fcp->mu = NULL;
  fcp->mu_r = NULL;

  return fcp;
}

RollingFrictionContactProblem* rollingFrictionContactProblem_new_with_data(int dim, int nc, NumericsMatrix* M, double* q, double* mu, double* mu_r)
{
  RollingFrictionContactProblem* fcp = (RollingFrictionContactProblem*) malloc(sizeof(RollingFrictionContactProblem));

  fcp->dimension = dim;
  fcp->numberOfContacts = nc;
  fcp->M = M;
  fcp->q = q;
  fcp->mu = mu;
  fcp->mu_r = mu_r;

  return fcp;
}

//#define SN_SBM_TO_DENSE

void rollingFrictionContactProblem_compute_statistics(RollingFrictionContactProblem* problem,
    double * reaction,
    double * velocity,

    double tol,
    int do_print)
{
  /* NumericsMatrix* M = problem->M; */
  /* double* q = problem->q; */
  double* mu = problem->mu;

  /* Number of contacts */
  int nc = problem->numberOfContacts;
  /* int n = problem->M->size0; */
  /* int m = 3 * nc; */


  int contact =0;

  int take_off_count=0;
  int closed_count=0;

  int sticking_count=0;
  int sliding_count=0;
  int ambiguous_take_off_count=0;
  int ambiguous_closed_count=0;

  for(contact =0; contact < nc; contact ++)
  {
    int pos=0;
    pos=contact*3;
    if(velocity[pos] > tol)
    {
      take_off_count++;
      if(fabs(reaction[pos]) > tol)
        ambiguous_take_off_count++;
    }
    else if(reaction[pos] > tol)
    {
      closed_count++;
      if(fabs(velocity[pos]) > tol)
        ambiguous_closed_count++;

      double criteria = reaction[pos+1]*reaction[pos+1] + reaction[pos+2]*reaction[pos+2] - mu[contact]*mu[contact]*reaction[pos]*reaction[pos];
      if(criteria < 0)
      {
        sticking_count++;
      }
      else
      {
        sliding_count++;
      }
    }
  }

  numerics_printf_verbose(do_print, "---- FC3D - STAT  - Number of contact = %i\t,  take off = %i\t, closed = %i\t, sliding = %i\t,sticking = %i\t, ambiguous take off = %i\t, ambiguous closed = %i",nc,take_off_count,closed_count,sliding_count,sticking_count, ambiguous_take_off_count,ambiguous_closed_count);

}
