/*
 * MigFlow - Copyright (C) <2010-2018>
 * <Universite catholique de Louvain (UCL), Belgium
 *  Universite de Montpellier, France>
 * 	
 * List of the contributors to the development of MigFlow: see AUTHORS file.
 * Description and complete License: see LICENSE file.
 * 	
 * This program (MigFlow) is free software: 
 * you can redistribute it and/or modify it under the terms of the GNU Lesser General 
 * Public License as published by the Free Software Foundation, either version
 * 3 of the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program (see COPYING and COPYING.LESSER files).  If not, 
 * see <http://www.gnu.org/licenses/>.
 */

#include "scontact.h" 
#include "quadtree.h"
#include "vector.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <string.h>
#include <time.h>

typedef struct _Particle Particle;
typedef struct _Contact Contact;
typedef struct _Disk Disk;
typedef struct _Segment Segment;
typedef struct _Triangle Triangle;

struct _ParticleProblem{
  Particle *particles;
  Contact *contacts;
  double *position, *velocity, *externalForces;
  Disk *disks;
  char **tagname, **materialName;
  int *diskTag, *segmentTag;
  int *diskMaterial, *segmentMaterial;
  int *particleMaterial;
  Segment *segments;
#if FRICTION_ENABLED
  double *omega;
  double friction_relaxation;
  double *mu;
#endif
#if DIMENSION == 3
  Triangle *triangles;
  int *triangleTag;
  int *triangleMaterial;
#endif
  int use_queue;
};

#if DIMENSION == 3
typedef enum {PARTICLE_PARTICLE=0, PARTICLE_DISK, PARTICLE_SEGMENT, PARTICLE_TRIANGLE} ContactType;
#else
typedef enum {PARTICLE_PARTICLE=0, PARTICLE_DISK, PARTICLE_SEGMENT} ContactType;
#endif

static double dot(const double *x, const double *y) {
  #if DIMENSION == 3
  return x[0] * y[0] +  x[1] * y[1] + x[2] * y[2];
  #else
  return x[0] * y[0] +  x[1] * y[1];
  #endif
}

/* Particle */
struct _Particle{
  double r;
  double m;
};

struct _Contact {
  size_t o0, o1;
  double a0, a1;
  double D;
  double dv;
#if FRICTION_ENABLED
  double In0, In1;
  double ct;
#endif
  ContactType type;
  double n[DIMENSION];
};

static void particleBoundingBox(const Particle *p, const double x[DIMENSION], double pmin[DIMENSION], double pmax[DIMENSION]){
  for (int i = 0; i < DIMENSION; ++i) {
    pmin[i] = x[i] - p->r;
    pmax[i] = x[i] + p->r;
  }
}

static int particleInitContact(size_t id0, Particle *p0, double *x0, size_t id1, Particle *p1, double *x1, double alert, Contact *c) {
  double nnorm = 0;
  c->dv = 0;
  c->o0 = id0;
  c->o1 = id1;
  for (int k = 0; k < DIMENSION; ++k) {
    c->n[k] = x0[k] - x1[k];
    nnorm += c->n[k] * c->n[k];
  }
  nnorm = sqrt(nnorm);
  for (int i = 0; i < DIMENSION; ++i)
    c->n[i] /= -nnorm;
  c->D = fmax(0., nnorm - (p0->r + p1->r));
  c->a0 = p1->m / (p0->m + p1->m);
  c->a1 = p0->m / (p0->m + p1->m);
#ifdef FRICTION_ENABLED
  c->ct = 0;
  c->In0 = c->a0*2/(p0->r);
  c->In1 = c->a1*2/(p1->r);
#endif
  c->type = PARTICLE_PARTICLE;
  return c->D < alert;
}

struct _Disk {
  double x[DIMENSION];
  double v[DIMENSION];
  double r;
#ifdef FRICTION_ENABLED
  double vt;
#endif
};

static void diskBoundingBox(const Disk *d, double *pmin, double *pmax) {
  const double r = fabs(d->r);
  for (int i = 0; i < DIMENSION; ++i) {
    pmin[i] = d->x[i] - r;
    pmax[i] = d->x[i] + r;
  }
}


size_t particleProblemAddBoundaryDiskTagId(ParticleProblem *p, const double x[DIMENSION], double r, int tag, int materialTag) {
  Disk *d = vectorPush(&p->disks);
  *vectorPush(&p->diskTag) = tag;
  *vectorPush(&p->diskMaterial) = materialTag;
  d->r = r;
#ifdef FRICTION_ENABLED
  d->vt = 0;
#endif
  for (int i = 0; i < DIMENSION; ++i) {
    d->x[i] = x[i];
    d->v[i] = 0.;
  }
  return vectorSize(p->disks) - 1;
}

size_t particleProblemAddBoundaryDisk(ParticleProblem *p, const double x[DIMENSION], double r, const char *tag, const char *materialTag) {
  return particleProblemAddBoundaryDiskTagId(p,x,r,particleProblemGetTagId(p,tag),particleProblemGetMaterialTagId(p,materialTag));
}

static int diskInitContact(size_t id, const Disk *d, size_t particle, const Particle *p, double *x, double alert, Contact *c) {
  double nnorm = 0;
  c->dv = 0;
  c->o1 = particle;
  c->o0 = id;
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] = d->x[i] - x[i];
    nnorm += c->n[i] * c->n[i];
  }
  nnorm = sqrt(nnorm);
  c->D = (nnorm - fabs(p->r + d->r)) * (d->r < 0 ? -1 : 1);
  if (c->D < 0) c->D = 0;
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] /= nnorm * (d->r < 0 ? -1 : 1);
  }
  c->a0 = 0.;
  c->a1 = 1.;
#ifdef FRICTION_ENABLED
  c->ct = 0;
  c->In0 = 0;
  c->In1 = c->a1*2/(p->r);
#endif
  c->type = PARTICLE_DISK;
  return c->D < alert;
}

struct _Segment{
  double p[2][DIMENSION];
  double v[DIMENSION];
#ifdef FRICTION_ENABLED
  double vt;
#endif
};

static void coordAdd(double *x, double a, const double *y) { // x += a * y
  x[0] += a * y[0];
  x[1] += a * y[1];
#if DIMENSION == 3
  x[2] += a * y[2];
#endif
}

static void segmentBoundingBox(const Segment *s, double *pmin, double *pmax) {
  for (int i = 0; i < DIMENSION; ++i) {
    pmin[i] = fmin(s->p[0][i], s->p[1][i]);
    pmax[i] = fmax(s->p[0][i], s->p[1][i]);
  }
}

static int segmentProjectionIsInside(const Segment *s, double *x) {
  double alpha = 0, beta = 0;
  for (int i = 0; i < DIMENSION; ++i) {
    const double d = s->p[0][i] - s->p[1][i];
    alpha += (x[i] - s->p[0][i]) * d;
    beta  += (x[i] - s->p[1][i]) * d;
  }
  return (alpha < 0 && beta > 0);
}

static int segmentInitContact(size_t id, const Segment *s, size_t particle, const Particle *p, double *x, double alert, Contact *c) {
  c->o0 = id;
  c->o1 = particle;
  c->dv = 0;
  double t[DIMENSION];
  double nt2 = 0;
  double dt = 0;
  for (int i = 0; i <DIMENSION; ++i){
    t[i] = s->p[1][i] - s->p[0][i];
    dt += t[i] * (s->p[0][i] - x[i]);
    nt2 += t[i] * t[i];
  }
  double nn2 = 0;
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] = s->p[0][i] - x[i] - t[i] / nt2 *  dt;
    nn2 += c->n[i] * c->n[i];
  }
  const double nnorm = sqrt(nn2);
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] /= nnorm;
  }
  c->D = nnorm - p->r;
  if (c->D < 0 && segmentProjectionIsInside(s, x)) c->D = 0;
  c->a0 = 0.;
  c->a1 = 1.;
#ifdef FRICTION_ENABLED
  c->ct = 0;
  c->In0 = 0;
  c->In1 = c->a1*2/(p->r);
#endif
  c->type = PARTICLE_SEGMENT;
  return c->D >= 0  && c->D < alert;
}

#if DIMENSION == 3
struct _Triangle {
  double p[3][DIMENSION];
  double v[DIMENSION];
};

void particleProblemAddBoundaryTriangleTagId(ParticleProblem *p, const double x0[3], const double x1[3], const double x2[3], int tag, int materialTag) {
  Triangle *t = vectorPush(&p->triangles);
  *vectorPush(&p->triangleTag) = tag;
  *vectorPush(&p->triangleMaterial) = materialTag;
  for (int i = 0; i < DIMENSION; ++i) {
    t->p[0][i] = x0[i];
    t->p[1][i] = x1[i];
    t->p[2][i] = x2[i];
    t->v[i] = 0.;
  }
}

void particleProblemAddBoundaryTriangle(ParticleProblem *p, const double x0[3], const double x1[3], const double x2[3], const char *tag, const char *material) {
  return particleProblemAddBoundaryTriangleTagId(p, x0, x1, x2, particleProblemGetTagId(p, tag), particleProblemGetMaterialTagId(p, material));
}

static void triangleBoundingBox(const Triangle *t, double *pmin, double *pmax) {
  for (int i = 0; i < 3; ++i) {
    pmin[i] = fmin(fmin(t->p[0][i], t->p[1][i]), t->p[2][i]);
    pmax[i] = fmax(fmax(t->p[0][i], t->p[1][i]), t->p[2][i]);
  }
}

static void _cross (const double *a, const double *b, double *c) {
  c[0] = a[1] * b[2] - a[2] * b[1];
  c[1] = a[2] * b[0] - a[0] * b[2];
  c[2] = a[0] * b[1] - a[1] * b[0];
}

static int triangleProjectionIsInside(const Triangle *t, const double *x) {
  double d0[3] = {t->p[1][0] - t->p[0][0], t->p[1][1] - t->p[0][1], t->p[1][2] - t->p[0][2]};
  double d1[3] = {t->p[2][0] - t->p[0][0], t->p[2][1] - t->p[0][1], t->p[2][2] - t->p[0][2]};
  double n[3];
  _cross(d0, d1, n);
  double xp[3];
  double dp[3] = {t->p[0][0] - x[0], t->p[0][1] - x[1], t->p[0][2] - x[2]};
  const double nd = dot(n, dp);
  for (int i = 0; i < 3; ++i) 
    xp[i] = x[i] + n[i] * nd;
  double dx[3][3] = 
    {{xp[0] - t->p[0][0], xp[1] - t->p[0][1], xp[2] - t->p[0][2]},
    {xp[0] - t->p[1][0], xp[1] - t->p[1][1], xp[2] - t->p[1][2]},
    {xp[0] - t->p[2][0], xp[1] - t->p[2][1], xp[2] - t->p[2][2]}};
  double alpha[3];
  for (int i = 0; i < 3; ++i) {
    double d[3];
    _cross(dx[(i + 1) % 3], dx[(i + 2) %3], d);
    alpha[i] = 0.5 * dot(n, d);
  }
  return ((alpha[0] <= 0 && alpha[1] <= 0 &&  alpha[2]<= 0) || (alpha[0] >= 0 && alpha[1] >= 0 && alpha[2] >= 0));
}

static int triangleInitContact(size_t id, const Triangle *t, size_t particle, const Particle *p, double *x, double alert, Contact *c) {
  c->o0 = id;
  c->o1 = particle;
  c->dv = 0;
  double d0[3] = {t->p[1][0] - t->p[0][0], t->p[1][1] - t->p[0][1], t->p[1][2] - t->p[0][2]};
  double d1[3] = {t->p[2][0] - t->p[0][0], t->p[2][1] - t->p[0][1], t->p[2][2] - t->p[0][2]};
  double N[3];
  _cross(d0, d1, N);
  const double nn = sqrt(dot(N, N));
  for (int i = 0; i < 3; ++i)
    c->n[i] = N[i] / nn;
  double dd[3] = {t->p[0][0] - x[0], t->p[0][1] - x[1], t->p[0][2] - x[2]};
  c->D = dot(c->n, dd);
  if (c->D < 0) {
    for (int i = 0; i <3; ++i)
      c->n[i] = -c->n[i];
    c->D = -c->D;
  }
  c->D -= p->r;
  c->a0 = 0.;
  c->a1 = 1.;
#ifdef FRICTION_ENABLED
  c->ct = 0;
  c->In0 = 0;
  c->In1 = c->a1*2/(p->r);
#endif
  if (c->D < 0)
    c->D = 0;
  c->type = PARTICLE_TRIANGLE;
  return (c->D > 0 || triangleProjectionIsInside(t, x)) && c->D < alert;
}

#endif

#ifdef FRICTION_ENABLED
static double particleProblemGetMu(const ParticleProblem *p, int mat0, int mat1) {
  return p->mu[mat0*particleProblemNMaterial(p)+mat1];
}
#endif

static int contactParticleParticleSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
  double vn = c->dv;
  for (int i = 0; i<DIMENSION; ++i)
    vn += (p->velocity[c->o0*DIMENSION+i] - p->velocity[c->o1*DIMENSION+i]) * c->n[i];
  double dp = fmax(0., vn - c->D/dt);
#ifdef FRICTION_ENABLED
  double t[2];
  t[0] = c->n[1];
  t[1] = -c->n[0];
  double ct = 0 ;
  double vt = 
    c->ct*p->particles[c->o0].r*c->In0+
    c->ct*p->particles[c->o1].r*c->In1+
    (p->velocity[c->o0*2+0]-p->velocity[c->o1*2+0])*t[0]+
    (p->velocity[c->o0*2+1]-p->velocity[c->o1*2+1])*t[1]+
    p->omega[c->o0]*p->particles[c->o0].r +
    p->omega[c->o1]*p->particles[c->o1].r;
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o0],p->particleMaterial[c->o1]);
  if (fabs(vt)> 3*mu*dp) vt *= 3*mu*dp/fabs(vt);
  ct = vt/3;
  if (dp == 0.)
    ct = 0;
  ct -= c->ct;
  p->omega[c->o0] -= c->In0*ct;
  p->omega[c->o1] -= c->In1*ct;
  c->ct += ct;
#endif
  dp -= c->dv;
  coordAdd(&p->velocity[c->o0*DIMENSION], -dp*c->a0, c->n);
  coordAdd(&p->velocity[c->o1*DIMENSION], dp*c->a1, c->n);
  c->dv += dp;
  if(fabs(dp) > tol/dt) return 0;
#ifdef FRICTION_ENABLED
  if(fabs(ct) > tol/dt) return 0;
#endif
  return 1;
}

static double contactParticleBndSolve(Contact *c, const double *v, double vlocfree[DIMENSION], double *vn, double dt) {
  for (int i = 0; i < DIMENSION; ++i)
    vlocfree[i] = v[i] + c->n[i] * c->dv;
  *vn = dot(vlocfree, c->n);
  return fmax(0, *vn - c->D/dt);
}

static int contactParticleDiskSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
  double vlocfree[DIMENSION], vn;
  double vloc[DIMENSION];
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->disks[c->o0].v[i];
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
  
#ifdef FRICTION_ENABLED
  double t[2];
  t[0] = c->n[1];
  t[1] = -c->n[0];
  double vt = 
    c->ct*p->particles[c->o1].r*c->In1+
    p->velocity[c->o1 * DIMENSION]*t[0]+
    p->velocity[c->o1 * DIMENSION+1]*t[1]+
    p->omega[c->o1]*p->particles[c->o1].r+
    p->disks[c->o0].vt;
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->diskMaterial[c->o0]);
  double ct = 0;
  if (fabs(vt)> 3*mu*dp) vt *= 3*mu*dp/fabs(vt);
  ct = vt/3;
  if (dp == 0.)
    ct = 0;
  ct -= c->ct;
  p->omega[c->o1] -= c->In1*ct;
  c->ct += ct;
#endif
  dp -=c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -dp, c->n);
  c->dv += dp;
  if(fabs(dp) > tol/dt) return 0;
#ifdef FRICTION_ENABLED
  if(fabs(ct) > tol/dt) return 0;
#endif
  return 1;
}

static int contactParticleSegmentSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
  double r = p->particles[c->o1].r;
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->segments[c->o0].v[i];
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
  for (int i = 0; i < DIMENSION; ++i)
    xn[i] = p->position[c->o1*DIMENSION + i] + r*c->n[i] + vlocfree[i]*c->D / vn;
  if (!segmentProjectionIsInside(&p->segments[c->o0], xn))
    dp = 0;
#ifdef FRICTION_ENABLED
  double t[2];
  t[0] = c->n[1];
  t[1] = -c->n[0];
  double vt = 
    c->ct*p->particles[c->o1].r*c->In1+
    p->velocity[c->o1*DIMENSION]*t[0]+
    p->velocity[c->o1*DIMENSION +1]*t[1]+
    p->omega[c->o1]*p->particles[c->o1].r+
    p->segments[c->o0].vt;
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->segmentMaterial[c->o0]);
  double ct = 0;
  if (fabs(vt)> 3*mu*dp) vt *= 3*mu*dp/fabs(vt);
  ct = vt/3;
  if (dp == 0.)
    ct = 0;
  ct -= c->ct;
  p->omega[c->o1] -= c->In1*ct;
  c->ct += ct;
#endif
  dp -= c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -dp, c->n);
  c->dv += dp;
#if FRICTION_ENABLED
#endif
  if(fabs(dp) > tol/dt) return 0;
#if FRICTION_ENABLED
  if(fabs(ct) > tol/dt) return 0;
#endif
  return 1;
}

#if DIMENSION == 3
static int contactParticleTriangleSolve(Contact *c, ParticleProblem *p,double dt, double tol) {
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
  double r = p->particles[c->o1].r;
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = p->velocity[c->o1*DIMENSION+i] - p->triangles[c->o0].v[i];
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt) - c->dv;
  for (int i = 0; i < DIMENSION; ++i)
    xn[i] = p->position[c->o1*DIMENSION+i] + r*c->n[i] + vlocfree[i]*c->D/vn;
  if (!triangleProjectionIsInside(&p->triangles[c->o0], xn))
    dp = 0;
  dp -= c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -dp, c->n);
  c->dv += dp;
  return (fabs(dp) < tol/dt);
}
#endif



Contact *findContactSorted(Contact *c, Contact *v, size_t *i) {
  while (*i < vectorSize(v) && (v[*i].type < c->type || v[*i].o0 < c->o0 || (v[*i].o0 == c->o0 && v[*i].o1 < c->o1)))
    (*i)++;
  return (*i < vectorSize(v) && v[*i].type == c->type && v[*i].o0 == c->o0 && v[*i].o1 == c->o1) ? &v[*i] : NULL;
}

static void addAlert(double alert, double *rmin, double *rmax) {
  rmin[0] -= alert;
  rmin[1] -= alert;
  rmax[0] += alert;
  rmax[1] += alert;
#if DIMENSION == 3
  rmin[2] -= alert;
  rmax[2] += alert;
#endif
}

static void bbadd(double *amin, double *amax, const double *bmin, const double *bmax)
{
  for (int i = 0; i < DIMENSION; ++i) {
    if (bmin[i] < amin[i])
      amin[i] = bmin[i];
    if (bmax[i] > amax[i])
      amax[i] = bmax[i];
  }
}

static void _particleProblemBoundingBox(ParticleProblem *p, double *bbmin, double *bbmax) {
  double amin[DIMENSION], amax[DIMENSION];
  for (int i = 0; i < DIMENSION; ++i) {
    bbmin[i] = DBL_MAX;
    bbmax[i] = -DBL_MAX;
  }
  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
    particleBoundingBox(&p->particles[i], &p->position[i * DIMENSION], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
  for (size_t i = 0; i < vectorSize(p->disks); ++i) {
    diskBoundingBox(&p->disks[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
  for (size_t i = 0; i < vectorSize(p->segments); ++i) {
    segmentBoundingBox(&p->segments[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
#if DIMENSION == 3
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
    triangleBoundingBox(&p->triangles[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
#endif
}

static void _bboxtol(double *bbmin, double *bbmax) {
  double lmax = 0;
  for (int i = 0; i < DIMENSION; ++i) {
    lmax = fmax(lmax, bbmax[i] - bbmin[i]);
  }
  double tol = 1e-8 * lmax;
  for (int i = 0; i < DIMENSION; ++i) {
    bbmax[i] += tol;
    bbmin[i] -= tol;
  }
}


double volTet(double x0[3], double x1[3], double x2[3], double x3[3]) {
  // x1 * (y2 * (z3 - z4) - z2 * (y3 - y4) + y3 * z4 - y4 * z3)
  //- y1 * (x2 * (z3 - z4) - z2 * (x3 - x4) + x3 * z4 - x4 * z3)
  //+ z1 * (x2 * (y3 - y4) - y2 * (x3 - x4) + x3 * y4 - x4 * y3)
  //- (x2 * (y3 * z4 - y4 * z3) - y2 * (x3 * z4 - x4 * z3) + z2 * (x3 * y4 - x4 * y3))
  return  x0[0] * (x1[1] * (x2[2] - x3[2]) - x1[2] * (x2[1] - x3[1]) + x2[1] * x3[2] - x3[1] * x2[2])
    - x0[1] * (x1[0] * (x2[2] - x3[2]) - x1[2] * (x2[0] - x3[0]) + x2[0] * x3[2] - x3[0] * x2[2])
    + x0[2] * (x1[0] * (x2[1] - x3[1]) - x1[1] * (x2[0] - x3[0]) + x2[0] * x3[1] - x3[0] * x2[1])
    - (x1[0] * (x2[1] * x3[2] - x3[1] * x2[2]) - x1[1] * (x2[0] * x3[2] - x3[0] * x2[2]) + x1[2] * (x2[0] * x3[1] - x3[0] * x2[1]));
}

void ParticleToMesh(size_t nParticles, double *particles, int nElements, double *elements, int *elid, double *Xi)
{
  double bbmin[DIMENSION], bbmax[DIMENSION];
  int N = DIMENSION + 1;
  for (int i = 0; i < DIMENSION; ++i) {
    bbmin[i] = elements[i];
    bbmax[i] = elements[i];
  }
  for (int i = 0; i < N * nElements; ++i) {
    for (int d = 0; d < DIMENSION; ++d) {
      bbmin[d] = fmin(bbmin[d], elements[DIMENSION * i + d]);
      bbmax[d] = fmax(bbmax[d], elements[DIMENSION * i + d]);
    }
  }
  _bboxtol(bbmin, bbmax);
  Cell *tree = cellNew(bbmin, bbmax);
  double amin[DIMENSION], amax[DIMENSION];
  for (int i = 0; i < nElements; ++i) {
    double *el = elements + (DIMENSION * N) * i;
    for (int d = 0; d < DIMENSION; ++d) {
      amin[d] = el[d];
      amax[d] = el[d];
    }
    for (int v = 1; v < N; ++v) {//only simplices
      for (int d = 0; d < DIMENSION; ++d) {
        amin[d] = fmin(amin[d], el[v * DIMENSION + d]);
        amax[d] = fmax(amax[d], el[v * DIMENSION + d]);
      }
    }
    _bboxtol(amin, amax);
    cellAdd(tree, amin, amax, i, NULL);
  }
  int *found = NULL;
  vectorPushN(&found, 100);
  vectorClear(found);
  for (size_t i = 0; i < nParticles; ++i) {
    double *x = &particles[i * DIMENSION];
    elid[i] = -1;
    cellSearch(tree, x, x, &found);
    for (size_t j = 0; j < vectorSize(found); ++j) {
      double *el = &elements[found[j] * N * DIMENSION];
      if (DIMENSION == 2)  {
        double *X[3] = {el, el + DIMENSION, el + DIMENSION * 2};
        double dx = x[0] - X[0][0], dy = x[1] - X[0][1];
        double DX[2] = {X[1][0] - X[0][0], X[2][0] - X[0][0]};
        double DY[2] = {X[1][1] - X[0][1], X[2][1] - X[0][1]};
        double det = DX[1] * DY[0] - DX[0] * DY[1];
        double xi  = (DX[1] * dy - DY[1] * dx) / det;
        double eta = -(DX[0] * dy - DY[0] * dx) / det;
        if (xi > -1e-8 && eta > -1e-8 && 1 - xi - eta > -1e-8) {
          Xi[i * DIMENSION + 0] = xi;
          Xi[i * DIMENSION + 1] = eta;
          elid[i] = found[j];
          break;
        }
      }
      else {
        double *X[4] = {el, el + DIMENSION, el + DIMENSION * 2, el + DIMENSION * 3};
        double v = volTet(X[0], X[1], X[2], X[3]);
        double v0 = volTet(x, X[1], X[2], X[3]);
        if (v0 * v < -1e-8)
          continue;
        double v1 = volTet(X[0], x, X[2], X[3]);
        if (v1 * v < -1e-8)
          continue;
        double v2 = volTet(X[0], X[1], x, X[3]);
        if (v2 * v < -1e-8)
          continue;
        double v3 = volTet(X[0], X[1], X[2], x);
        if (v3 * v < -1e-8)
          continue;
        Xi[i * 3 + 0] = v1 / v;
        Xi[i * 3 + 1] = v2 / v;
        Xi[i * 3 + 2] = v3 / v;
        elid[i] = found[j];
        break;
      }
    }
    /*if (elid[i] == -1) {
      printf(" PARTICLE %i OUTSIDE DOMAIN N2 search!!!\n", i);
      double toll = -1000;
      for (size_t j = 0; j < nElements; ++j) {
        double *el = &elements[j * N * DIMENSION];
        double *X[3] = {el, el + DIMENSION, el + DIMENSION * 2};
        double dx = x[0] - X[0][0], dy = x[1] - X[0][1];
        double DX[2] = {X[1][0] - X[0][0], X[2][0] - X[0][0]};
        double DY[2] = {X[1][1] - X[0][1], X[2][1] - X[0][1]};
        double det = DX[1] * DY[0] - DX[0] * DY[1];
        double xi  = (DX[1] * dy - DY[1] * dx) / det;
        double eta = -(DX[0] * dy - DY[0] * dx) / det;
        toll = fmax(toll, fmin(-xi, fmin(-eta, xi + eta -1)));
        if (xi > -1e-8 && eta > -1e-8 && 1 - xi - eta > -1e-8) {
          Xi[i * DIMENSION + 0] = xi;
          Xi[i * DIMENSION + 1] = eta;
          elid[i] = j;
          break;
        }
      }*/
      if (elid[i] == -1) {
        //printf(" PARTICLE %lu OUTSIDE DOMAIN!!! %g %g\n", i, x[0], x[1]);
        //exit(1);
      }
    //}
  }
  vectorFree(found);
  cellFree(tree);
}

int listcmp(const void *i0, const void *i1) {
  const u_int32_t j0 = *(u_int32_t*)i0, j1 = *(u_int32_t*)i1;
  return j0 - j1;
}

static int intersect(const double *amin, const double *amax, const double *bmin, const double *bmax) {
  if (amin[0] > bmax[0] || amax[0] < bmin[0]) return 0;
  if (amin[1] > bmax[1] || amax[1] < bmin[1]) return 0;
  #if DIMENSION == 3
  if (amin[2] > bmax[2] || amax[2] < bmin[2]) return 0;
  #endif
  return 1;
}

static void _particleProblemGenContacts2(ParticleProblem *p, const double alert)
{
  clock_t tic = clock();
  u_int32_t *list = NULL;
  double bbmin[DIMENSION], bbmax[DIMENSION];
  _particleProblemBoundingBox(p,bbmin,bbmax);
  addAlert(alert,bbmin, bbmax);
  u_int16_t N[DIMENSION];
  double delta = alert*6;
  for (int i = 0; i < DIMENSION; ++i) {
    N[i] = (bbmax[i]-bbmin[i])/delta;
  }

  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
    double amin[DIMENSION], amax[DIMENSION];
    particleBoundingBox(&p->particles[i], &p->position[i*DIMENSION],amin,amax);
    addAlert(alert/2,amin,amax);
    u_int16_t from[DIMENSION],to[DIMENSION];
    for (int i = 0; i < DIMENSION; ++i){
      from[i] = (amin[i]-bbmin[i])/delta;
      to[i] = (amax[i]-bbmin[i])/delta;
    }
#if DIMENSION == 2
    for (int j = from[0]; j <= to[0]; ++j) {
      for (int k = from[1]; k <= to[1]; ++k) {
        u_int32_t bid = j*N[1]+k;
        *vectorPush(&list) = bid;
        *vectorPush(&list) = i;
      }
    }
#else
    for (int j = from[0]; j <= to[0]; ++j) {
      for (int k = from[1]; k <= to[1]; ++k) {
        for (int l = from[2]; l <= to[2]; ++l) {
          u_int32_t bid = (j*N[1]+k)*N[2]+l;
          *vectorPush(&list) = bid;
          *vectorPush(&list) = i;
        }
      }
    }
#endif
  }
  clock_t tic1 = clock();
  qsort(list, vectorSize(list)/2, 2*sizeof(u_int32_t), listcmp);
  clock_t tic2 = clock();
  int ntot = 0;
  Contact *cc=NULL;
  vectorPushN(&cc, 10000000);
  vectorClear(cc);
  double amin[DIMENSION], amax[DIMENSION];
  double bmin[DIMENSION], bmax[DIMENSION];
  for (int start = 0; start < vectorSize(list);) {
    int end = start;
    for (; end< vectorSize(list);end+=2) {
      if (list[end] != list[start])
        break;
    }
    for (int i = start; i < end; i+= 2) {
      int ii = list[i+1];
      Particle *p0 = &p->particles[ii];
      double *x_i = &p->position[ii*DIMENSION];
      for (int j = i; j < end; j+= 2) {
        int jj = list[j+1];
        Particle *p1 = &p->particles[jj];
        double *x_j = &p->position[jj*DIMENSION];
        Contact c;
        c.dv = 0;
        c.o0 = ii; c.o1 = jj;
        double nnorm = 0;
        for (int k = 0; k < DIMENSION; ++k) {
          c.n[k] = x_i[k] - x_j[k];
          nnorm += c.n[k]*c.n[k];
        }
        double dd =p0->r+p1->r+alert;
        if (nnorm > dd*dd)
          continue;
        nnorm = sqrt(nnorm);
        for (int i = 0; i < DIMENSION; ++i)
          c.n[i] /= -nnorm;
        c.D = fmax(0., nnorm - (p0->r + p1->r));
        c.a0 = p1->m / (p0->m + p1->m);
        c.a1 = p0->m / (p0->m + p1->m);
        c.type = PARTICLE_PARTICLE;
        ntot++;
        *vectorPush(&cc) = c;
        /*else if ((cold = findContactSorted(c, oldContacts, &iold))) {
          coordAdd(&p->velocity[c->o0 * DIMENSION], -cold->dv * c->a0, c->n);
          coordAdd(&p->velocity[c->o1 * DIMENSION], cold->dv * c->a1, c->n);
          c->dv = cold->dv;
        }*/
      }
    }
    start = end;
  }
  clock_t tic3 = clock();
  printf("time new %lu : %lu %lu %lu\n", tic3-tic, tic3-tic2,tic2-tic1, tic1-tic);
  printf("%lu / %i (%lu%%)\n", vectorSize(cc), ntot, vectorSize(cc)*100/ntot);
  vectorFree(cc);
}

static void _particleProblemGenContacts(ParticleProblem *p, const double alert)
{
  //_particleProblemGenContacts2(p,alert);
  clock_t tic = clock();
  size_t iold = 0;
  double bbmin[DIMENSION], bbmax[DIMENSION];
  _particleProblemBoundingBox(p, bbmin, bbmax);
  Cell *tree = cellNew(bbmin, bbmax);
  double amin[DIMENSION], amax[DIMENSION];
  int *found = NULL;
  vectorPushN(&found, 100);
  vectorClear(found);
  // Particles
  Contact *oldContacts = vectorDup(p->contacts), *cold;
  vectorClear(p->contacts);
  int ntot = 0;
  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
    particleBoundingBox(&p->particles[i], &p->position[i * DIMENSION], amin, amax);
    addAlert(alert/2, amin, amax);
    vectorClear(found);
    cellAdd(tree, amin, amax, i, &found);
    for (size_t j = 0; j < vectorSize(found); ++j) {
      ntot++;
      Contact *c= vectorPush(&p->contacts);
      if(!particleInitContact(i, &p->particles[i], &p->position[i * DIMENSION], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c))
        vectorPop(p->contacts);
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
        coordAdd(&p->velocity[c->o0 * DIMENSION], -cold->dv * c->a0, c->n);
        coordAdd(&p->velocity[c->o1 * DIMENSION], cold->dv * c->a1, c->n);
        c->dv = cold->dv;
#if FRICTION_ENABLED
        c->ct = cold->ct;
        p->omega[c->o0] -= c->In0*(c->ct);
        p->omega[c->o1] -= c->In1*(c->ct);
#endif
      }
    }
  }
  //printf("time old %i\n", clock()-tic);
  //exit(0);
  // Disks
  for (size_t i = 0; i < vectorSize(p->disks); ++i) {
    diskBoundingBox(&p->disks[i], amin, amax);
    addAlert(alert/2, amin, amax);
    vectorClear(found);
    cellSearch(tree, amin, amax, &found);
    for (size_t j = 0; j < vectorSize(found); ++j) {
      Contact *c = vectorPush(&p->contacts);
      if(!diskInitContact(i, &p->disks[i], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c))
        vectorPop(p->contacts);
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
        c->dv = cold->dv;
#if FRICTION_ENABLED
        c->ct = cold->ct;
        p->omega[c->o1] -= c->In1*(c->ct);
#endif
      }
    }
  }
  // Segments
  for (size_t i = 0; i < vectorSize(p->segments); ++i) {
    segmentBoundingBox(&p->segments[i], amin, amax);
    addAlert(alert/2, amin, amax);
    vectorClear(found);
    cellSearch(tree, amin, amax, &found);
    for (size_t j = 0; j < vectorSize(found); ++j) {
      Contact *c = vectorPush(&p->contacts);
      if (!segmentInitContact(i, &p->segments[i], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c))
        vectorPop(p->contacts);
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
        c->dv = cold->dv;
#if FRICTION_ENABLED
        c->ct = cold->ct;
        p->omega[c->o1] -= c->In1*(c->ct);
#endif
      }
    }
  }
  // Triangles 
  #if DIMENSION == 3
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
    triangleBoundingBox(&p->triangles[i], amin, amax);
    addAlert(alert/2, amin, amax);
    vectorClear(found);
    cellSearch(tree, amin, amax, &found);
    for (int j = 0; j < vectorSize(found); ++j) {
      Contact *c =  vectorPush(&p->contacts);
      if (!triangleInitContact(i, &p->triangles[i], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c))
        vectorPop(p->contacts);
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
        c->dv = cold->dv;
      }
    }
  }
  #endif
  vectorFree(oldContacts);
  vectorFree(found);
  cellFree(tree);
}

typedef struct {
  size_t *data;
  size_t *b, *e;
  size_t size;
}Fifo;

static Fifo *fifoNew(size_t maxSize) {
  Fifo *f = malloc(sizeof(Fifo));
  f->b = f->e = f->data = malloc(sizeof(size_t)*(maxSize+1));
  f->size = maxSize+1;
  return f;
}

static void fifoPush(Fifo *f, size_t v) {
  *(f->e++) = v;
  if (f->e == f->data + f->size) f->e = f->data;
}

#define FIFO_EMPTY ((size_t)-1)

static size_t fifoPop(Fifo *f) {
  if (f->b == f->e) return FIFO_EMPTY;
  size_t v = *(f->b++);
  if (f->b == f->data + f->size) f->b = f->data;
  return v;
}

static void fifoFree(Fifo *f) {
  free(f->data);
  free(f);
}

static void _particleProblemSolveContacts(ParticleProblem *p, double dt, double tol) {
  int converged = 0;
  while (! converged) {
    converged = 1;
    for (int ic = vectorSize(p->contacts)-1; ic >= 0;  --ic) {
      Contact *c = &p->contacts[ic];
      int conv;
      switch (c->type) {
        case PARTICLE_PARTICLE :
          conv = contactParticleParticleSolve(c, p, dt,tol);
          break;
        case PARTICLE_DISK :
          conv = contactParticleDiskSolve(c, p, dt, tol);
          break;
        case PARTICLE_SEGMENT :
          conv = contactParticleSegmentSolve(c, p, dt, tol);
          break;
#if DIMENSION == 3
        case PARTICLE_TRIANGLE :
          converged = contactParticleTriangleSolve(c, p, dt, tol);
          break;
#endif
      }
      if (!conv) {
        converged = 0;
      }
    }
  }
}

static void _particleProblemSolveContactsQueue(ParticleProblem *p, double dt, double tol) {
  size_t *nContactByParticle = malloc(sizeof(size_t)*(vectorSize(p->particles)));
  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
    nContactByParticle[i] = 0;
  }
  for (size_t i = 0; i < vectorSize(p->contacts); ++i) {
    Contact *c = &p->contacts[i];
    if(c->type == PARTICLE_PARTICLE)
      nContactByParticle[c->o0]++;
    nContactByParticle[c->o1]++;
  }
  size_t *contactByParticleP = malloc(sizeof(size_t)*(vectorSize(p->particles)+1));
  contactByParticleP[0] = 0;
  for (size_t i = 0; i < vectorSize(p->particles); ++i){
    contactByParticleP[i+1] = contactByParticleP[i] + nContactByParticle[i];
    nContactByParticle[i] = 0;
  }
  size_t *contactByParticle = malloc(contactByParticleP[vectorSize(p->particles)]*sizeof(size_t));
  for (size_t i = 0; i < vectorSize(p->contacts); ++i) {
    Contact *c = &p->contacts[i];
    if (c->type == PARTICLE_PARTICLE)
      contactByParticle[contactByParticleP[c->o0]+(nContactByParticle[c->o0]++)] = i;
    contactByParticle[contactByParticleP[c->o1]+(nContactByParticle[c->o1]++)] = i;
  }
  free(nContactByParticle);

  Fifo *queue = fifoNew(vectorSize(p->contacts));
  int *activeContact = malloc(sizeof(int)*vectorSize(p->contacts));
  for (size_t i = 0; i < vectorSize(p->contacts); ++i) {
    fifoPush(queue, i);
    activeContact[i] = 1;
  }
  for (size_t ic = fifoPop(queue); ic != FIFO_EMPTY; ic=fifoPop(queue)){
    Contact *c = &p->contacts[ic];
    int converged;
    switch (c->type) {
    case PARTICLE_PARTICLE :
      converged = contactParticleParticleSolve(c, p, dt, tol);
      break;
    case PARTICLE_DISK :
      converged = contactParticleDiskSolve(c, p, dt, tol);
      break;
    case PARTICLE_SEGMENT :
      converged = contactParticleSegmentSolve(c, p, dt, tol);
      break;
#if DIMENSION == 3
    case PARTICLE_TRIANGLE :
      converged = contactParticleTriangleSolve(c, p, dt, tol);
      break;
#endif
    }
    if (!converged) {
      if(c->type == PARTICLE_PARTICLE) {
        for (size_t j = contactByParticleP[c->o0]; j < contactByParticleP[c->o0+1]; ++j) {
          if (!activeContact[contactByParticle[j]]){
            activeContact[contactByParticle[j]] = 1;
            fifoPush(queue, contactByParticle[j]);
          }
        }
      }
      for (size_t j = contactByParticleP[c->o1]; j < contactByParticleP[c->o1+1]; ++j) {
        if (!activeContact[contactByParticle[j]]){
          activeContact[contactByParticle[j]] = 1;
          fifoPush(queue, contactByParticle[j]);
        }
      }
    }
    activeContact[ic] = 0;
  }

  fifoFree(queue);
  free(activeContact);
  free(contactByParticleP);
  free(contactByParticle);
}

double particleProblemMaxDt(const ParticleProblem *p, double alert) {
  double VM2 = 0;
  double FM2 = 0;
  for (size_t j = 0; j < vectorSize(p->particles); ++j) {
    VM2 = fmax(VM2, dot(&p->velocity[j * DIMENSION], &p->velocity[j * DIMENSION]));
    FM2 = fmax(FM2, dot(&p->externalForces[j * DIMENSION], &p->externalForces[j * DIMENSION]) / (p->particles[j].m * p->particles[j].m));
  }
  const double VM = sqrt(VM2);
  const double FM = sqrt(FM2);
  const double q = VM + sqrt(VM2 + 4 * FM * alert);
  return alert / q;
}

void particleProblemSolve(ParticleProblem *p, double alert, double dt, double tol, int maxit)
{
  _particleProblemGenContacts(p, alert);
  if (p->use_queue)
    _particleProblemSolveContactsQueue(p,dt,tol);
  else
    _particleProblemSolveContacts(p,dt,tol);
}

static void particleProblemApplyCtTranslation(ParticleProblem *p)
{
#if FRICTION_ENABLED
  for (int i = 0; i < vectorSize(p->contacts); ++i) {
    const Contact *c = p->contacts+i;
    double t[2] = {c->n[1], -c->n[0]};
    double ct = c->ct;
    if(c->type == PARTICLE_PARTICLE) {
      coordAdd(&p->velocity[c->o0 * DIMENSION], -ct*c->a0, t);
      coordAdd(&p->velocity[c->o0 * DIMENSION], ct*c->a1, t);
    }
    else {
      coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*c->a1, t);
    }
  }
#endif
}


void particleProblemIterate(ParticleProblem *p, double alert, double dt, double tol, int maxit)
{
  int r = 0;
  for (size_t j = 0; j < vectorSize(p->particles); ++j)
    for (size_t i = 0; i < DIMENSION; ++i)
      p->velocity[j * DIMENSION + i] += p->externalForces[j * DIMENSION + i] * dt / p->particles[j].m;
  particleProblemSolve(p, alert, dt, tol, maxit);
  for (size_t i = 0; i < vectorSize(p->position); ++i){
    p->position[i] += p->velocity[i] * dt;
  }
  for (size_t i = 0; i < vectorSize(p->disks); ++i)
    for (int j = 0; j < DIMENSION; ++j)
      p->disks[i].x[j] += p->disks[i].v[j] * dt;
  for (size_t i = 0; i < vectorSize(p->segments); ++i)
    for (int j = 0; j < DIMENSION; ++j) {
      p->segments[i].p[0][j] += p->segments[i].v[j] * dt;
      p->segments[i].p[1][j] += p->segments[i].v[j] * dt;
    }
  #if DIMENSION == 3
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
    for (int j = 0; j < DIMENSION; ++j) {
      p->triangles[i].p[0][j] += p->triangles[i].v[j] * dt;
      p->triangles[i].p[1][j] += p->triangles[i].v[j] * dt;
      p->triangles[i].p[2][j] += p->triangles[i].v[j] * dt;
    }
  }
  #endif
  particleProblemApplyCtTranslation(p);
}

size_t particleProblemAddBoundarySegmentTagId(ParticleProblem *p, const double x0[DIMENSION], const double x1[DIMENSION], int tag, int materialTag) {
  Segment *s = vectorPush(&p->segments);
  *vectorPush(&p->segmentTag) = tag;
  *vectorPush(&p->segmentMaterial) = materialTag;
  for (int i = 0; i < DIMENSION; ++i) {
    s->p[0][i] = x0[i];
    s->p[1][i] = x1[i];
    s->v[i] = 0.;
  }
#ifdef FRICTION_ENABLED
  s->vt = 0.0;
#endif
  return vectorSize(p->segments) - 1;
}

size_t particleProblemAddBoundarySegment(ParticleProblem *p, const double x0[DIMENSION], const double x1[DIMENSION], const char *tag, const char *material) {
  return particleProblemAddBoundarySegmentTagId(p, x0, x1, particleProblemGetTagId(p, tag),particleProblemGetMaterialTagId(p, material));
}

void particleProblemAddParticleMaterialTagId(ParticleProblem *p, const double x[DIMENSION], double r, double m, int tag) {
  size_t n = vectorSize(p->particles);
  Particle *particle = vectorPush(&p->particles);
  particle->r = r;
  particle->m = m;
  vectorPushN(&p->position, DIMENSION);
  vectorPushN(&p->velocity, DIMENSION);
  vectorPushN(&p->externalForces, DIMENSION);
  vectorPushN(&p->particleMaterial,1);
  for (int i = 0; i < DIMENSION; ++i) {
    p->position[n * DIMENSION +  i] = x[i];
    p->velocity[n * DIMENSION +  i] = 0;
    p->externalForces[n * DIMENSION + i] = 0;
  }
#ifdef FRICTION_ENABLED
  vectorPushN(&p->omega,1);
  p->omega[n] = 0.0;
#endif
  p->particleMaterial[n] = tag;
}

void particleProblemAddParticle(ParticleProblem *p, const double x[DIMENSION], double r, double m, const char *material) {
  particleProblemAddParticleMaterialTagId(p,x,r,m,particleProblemGetMaterialTagId(p,material));
}

#ifdef FRICTION_ENABLED
void particleProblemSetFrictionCoefficient(ParticleProblem *p, double mu, const char *mat0, const char *mat1) {
  int imat0 = particleProblemGetMaterialTagId(p,mat0);
  int imat1 = particleProblemGetMaterialTagId(p,mat1);
  int n = particleProblemNMaterial(p);
  p->mu[imat0*n+imat1] = p->mu[imat1*n+imat0] = mu;
}

void particleProblemSetBoundaryTangentVelocity(ParticleProblem *p, const char *tag, double v){ 
  int ttag = (int)particleProblemGetTagId(p,tag);
  int d = vectorSize(p->diskTag);
  int s = vectorSize(p->segmentTag);
  int i;
  for(i=0;i<d;i++){
    if(p->diskTag[i]==ttag){
      p->disks[i].vt = v;
    }
  }
  for(i=0;i<s;i++){
    if(p->segmentTag[i]==ttag){
      p->segments[i].vt = v;
    }
  }
}
#endif

void particleProblemClearParticles(ParticleProblem *p) {
  vectorClear(p->position);
  vectorClear(p->particles);
  vectorClear(p->externalForces);
  vectorClear(p->velocity);
#ifdef FRICTION_ENABLED
  vectorClear(p->omega);
  vectorClear(p->particleMaterial);
#endif
}

void particleProblemAddParticles(ParticleProblem *p, int n_particles, double *position, double *mass, double *radius, double *velocity, double *omega, int *tag) {
  size_t N = vectorSize(p->particles);
  vectorPushN(&p->position,n_particles*DIMENSION);
  vectorPushN(&p->particles,n_particles);
  vectorPushN(&p->velocity, n_particles*DIMENSION);
  vectorPushN(&p->externalForces,n_particles*DIMENSION);
#ifdef FRICTION_ENABLED
  vectorPushN(&p->omega,n_particles);
#endif
  vectorPushN(&p->particleMaterial,n_particles);
  for (int i = 0; i < n_particles; ++i) {
    for (int j = 0; j < DIMENSION; ++j) {
      p->position[(i+N)*DIMENSION+j] = position[i*DIMENSION+j];
      p->velocity[(i+N)*DIMENSION+j] = velocity[i*DIMENSION+j];
    }
    p->particles[i+N].m = mass[i];
    p->particles[i+N].r = radius[i];
#ifdef FRICTION_ENABLED
    p->omega[i+N] = omega[i];
#endif
    p->particleMaterial[i+N] = tag[i];
  }
}

void particleProblemClearBoundaries(ParticleProblem *p) {
  vectorClear(p->disks);
  vectorClear(p->segments);
#if DIMENSION == 3
  vectorClear(p->triangles);
#endif
}

void particleProblemSetContacts(ParticleProblem *p, size_t n, size_t *o, double *v, int *types) {
  vectorClear(p->contacts);
  vectorPushN(&p->contacts, n);
  for (int j = 0; j < n; ++j){
    p->contacts[j].o0 = o[j*2+0];
    p->contacts[j].o1 = o[j*2+1];
    p->contacts[j].dv = v[j*2+0];
#ifdef FRICTION_ENABLED
    p->contacts[j].ct = v[j*2+1];
#endif
    p->contacts[j].type = types[j];
  }
}

int *particleProblemDiskTag(ParticleProblem *p) {return p->diskTag;}
int *particleProblemDiskMaterial(ParticleProblem *p) {return p->diskMaterial;}
int *particleProblemSegmentTag(ParticleProblem *p) {return p->segmentTag;};
int *particleProblemSegmentMaterial(ParticleProblem *p) {return p->segmentMaterial;};
int *particleProblemParticleMaterial(ParticleProblem *p) {return p->particleMaterial;}
#if DIMENSION == 3
int *particleProblemTriangleTag(ParticleProblem *p) {return p->triangleTag;};
int *particleProblemTriangleMaterial(ParticleProblem *p) {return p->triangleMaterial;};
double *particleProblemTriangle(ParticleProblem *p) {return (double*)&p->triangles[0];}
#endif
double *particleProblemVelocity(ParticleProblem *p) {return &p->velocity[0];}
double *particleProblemDisk(ParticleProblem *p) {
  return (double*)&p->disks[0];
}
double *particleProblemSegment(ParticleProblem *p) {return (double*)&p->segments[0];}
double *particleProblemParticle(ParticleProblem *p) {return (double*)&p->particles[0];}
double *particleProblemPosition(ParticleProblem *p){return &p->position[0];}
double *particleProblemExternalForces(ParticleProblem *p){return &p->externalForces[0];}

void particleProblemDelete(ParticleProblem *p) {
  vectorFree(p->particles);
  vectorFree(p->disks);
  vectorFree(p->diskTag);
  vectorFree(p->diskMaterial);
  vectorFree(p->segments);
  vectorFree(p->segmentMaterial);
  vectorFree(p->segmentTag);
  vectorFree(p->contacts);
  vectorFree(p->position);
  vectorFree(p->particleMaterial);
  vectorFree(p->velocity);
  vectorFree(p->externalForces);
#ifdef FRICTION_ENABLED
  vectorFree(p->mu);
  vectorFree(p->omega);
#endif
  #if DIMENSION == 3
  vectorFree(p->triangles);
  vectorFree(p->triangleTag);
  vectorFree(p->triangleMaterial);
  #endif
  for (size_t i = 0; i < vectorSize(p->tagname); ++i)
    free(p->tagname[i]);
  vectorFree(p->tagname);
  for (size_t i = 0; i < vectorSize(p->materialName); ++i)
    free(p->materialName[i]);
  vectorFree(p->materialName);
  free(p);
}

size_t particleProblemGetTagId(ParticleProblem *p, const char *tag) {
  for (size_t i = 0; i < vectorSize(p->tagname); ++i)
    if (!strcmp(tag, p->tagname[i]))
      return i;
  char *dup = malloc(sizeof(char) * (strlen(tag)+1));
  strcpy(dup, tag);
  *vectorPush(&p->tagname) = dup;
  return vectorSize(p->tagname) - 1;
}

size_t particleProblemGetMaterialTagId(ParticleProblem *p, const char *tag) {
  for (size_t i = 0; i < vectorSize(p->materialName); ++i)
    if (!strcmp(tag, p->materialName[i]))
      return i;
  char *dup = malloc(sizeof(char) * (strlen(tag)+1));
  strcpy(dup, tag);
  *vectorPush(&p->materialName) = dup;
#ifdef FRICTION_ENABLED
  int n = vectorSize(p->materialName);
  double *oldmu = p->mu;
  p->mu = NULL;
  vectorPushN(&p->mu, n*n);
  for (int i = 0; i < n; ++i){
    for (int j = 0; j < n; ++j) {
      p->mu[i] = (i==n-1 || j== n-1) ? 0 : oldmu[i*(n-1)+j];
    }
  }
  vectorFree(oldmu);
#endif
  return vectorSize(p->materialName) - 1;
}

const char *particleProblemGetMaterialTagName(ParticleProblem *p, size_t id) {
  if (id >= vectorSize(p->materialName))
    return NULL;
  return p->materialName[id];
}

const char *particleProblemGetTagName(ParticleProblem *p, size_t id) {
  if (id >= vectorSize(p->tagname))
    return NULL;
  return p->tagname[id];
}

ParticleProblem *particleProblemNew() {
  ParticleProblem *p = (ParticleProblem*)malloc(sizeof(ParticleProblem));
  p->use_queue = 1;
  p->particles = NULL;
  p->contacts = NULL;
  p->tagname = NULL;
  p->materialName = NULL;
  p->disks = NULL;
  p->diskTag = NULL;
  p->diskMaterial = NULL;
  p->segments = NULL;
  p->segmentTag = NULL;
  p->segmentMaterial = NULL;
  p->particleMaterial = NULL;
  p->position = NULL;
  p->velocity = NULL;
#ifdef FRICTION_ENABLED
  p->omega = NULL;
  p->mu = NULL;
#endif
  p->externalForces = NULL;
#if DIMENSION == 3
  p->triangles = NULL;
  p->triangleTag = NULL;
  p->triangleMaterial = NULL;
#endif
  return p;
}

unsigned long int particleProblemNParticle(ParticleProblem *p) {
  return vectorSize(p->particles);
}

size_t particleProblemContactN(ParticleProblem *p, int ctype){
  size_t n = 0;
  for (size_t i = 0; i < vectorSize(p->contacts); ++i)
    if (p->contacts[i].type == ctype) n+=1;
  return n;
}

void particleProblemContact(ParticleProblem *p, int ctype, size_t *o, double *v){
  size_t n = 0;
  for (size_t i = 0; i < vectorSize(p->contacts); ++i){
    Contact *c = &p->contacts[i];
    if (c->type == ctype) {
      v[2*n+0] = c->dv;
#ifdef FRICTION_ENABLED
      v[2*n+1] = c->ct;
#else
      v[2*n+1] = 0;
#endif
      o[2*n+0] = c->o0;
      o[2*n+1] = c->o1;
      n+=1;
    }
  }
}

void particleProblemSetUseQueue(ParticleProblem *p, int use_queue) {
  p->use_queue = use_queue;
}

#ifdef FRICTION_ENABLED
double *particleProblemOmega(ParticleProblem *p) {return &p->omega[0];}
#endif

int particleProblemNTag(const ParticleProblem *p) {
  return vectorSize(p->tagname);
}

int particleProblemNMaterial(const ParticleProblem *p) {
  return vectorSize(p->materialName);
}