#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;

static int check_word_in_file(FILE *f, const char *w) {
  char word[256];
  if(fscanf(f,"%255s", word) != 1){
    printf("Invalid mesh file (\"%s\" expected).\n", w);
    return -1;
  }
  if(strcmp(word, w) != 0){
    printf("Invalid mesh file (\"%s\" expected).\n", w);
    return -1;
  }
  return 0;
}

struct _ParticleProblem{
  int jacobi;
  double relax;
  Particle *particles;
  Contact *contacts;
  double *position, *velocity, *externalForces;
  Disk *disks;
  char **tagname;
  int *diskTag, *segmentTag;
  Segment *segments;
  #if DIMENSION == 3
  Triangle *triangles;
  int *triangleTag;
  #endif
};

#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
}

void coordRead(FILE *f, double *x) {
  for (int i = 0; i < DIMENSION; ++i)
    fscanf(f, "%le", &x[i]);
}

void coordWrite(FILE *f, double *x) {
  for (int i = 0; i < DIMENSION; ++i)
    fprintf(f, " %.16g", x[i]);
}

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

struct _Contact {
  size_t o0, o1;
  double a0, a1;
  double D;
  double dv;
  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);
  c->type = PARTICLE_PARTICLE;
  return c->D < alert;
}

struct _Disk {
  double x[DIMENSION];
  double v[DIMENSION];
  double r;
};

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;
  }
}

static size_t particleProblemAddBoundaryDiskTagId(ParticleProblem *p, const double x[DIMENSION], double r, size_t tag) {
  Disk *d = vectorPush(&p->disks);
  *vectorPush(&p->diskTag) = tag;
  d->r = r;
  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) {
  return particleProblemAddBoundaryDiskTagId(p, x, r, particleProblemGetTagId(p, tag));
}

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.;
  c->type = PARTICLE_DISK;
  return c->D < alert;
}

struct _Segment{
  double p[2][DIMENSION];
  double v[DIMENSION];
};

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.;
  c->type = PARTICLE_SEGMENT;
  return c->D >= 0  && c->D < alert;
}

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

static void particleProblemAddBoundaryTriangleTagId(ParticleProblem *p, const double x0[3], const double x1[3], const double x2[3], size_t tag) {
  Triangle *t = vectorPush(&p->triangles);
  *vectorPush(&p->triangleTag) = tag;
  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) {
  return particleProblemAddBoundaryTriangleTagId(p, x0, x1, x2, particleProblemGetTagId(p, tag));
}

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.;
  if (c->D < 0)
    c->D = 0;
  c->type = PARTICLE_TRIANGLE;
  return (c->D > 0 || triangleProjectionIsInside(t, x)) && c->D < alert;
}

#endif

static double contactParticleParticleSolve(Contact *c, const double *v0, const double *v1, double dt) {
  #if DIMENSION == 3
  double vn = (v0[0] - v1[0]) * c->n[0] + (v0[1] - v1[1]) * c->n[1] + (v0[2] - v1[2]) * c->n[2] + c->dv;
  #else 
  double vn = (v0[0] - v1[0]) * c->n[0] + (v0[1] - v1[1]) * c->n[1] + c->dv;
  #endif
  return fmax(0., vn - c->D/dt);
}

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 double contactParticleDiskSolve(Contact *c, const double *v, Disk *d, double dt) {

  double vlocfree[DIMENSION], vn;
  double vloc[DIMENSION];
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = v[i] - d->v[i];
  return contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
}

static double contactParticleSegmentSolve(Contact *c, const double *v, const double *x, double r, Segment *s, double dt) {
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = v[i] - s->v[i];
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
  for (int i = 0; i < DIMENSION; ++i)
    xn[i] = x[i] + r*c->n[i] + vlocfree[i]*c->D / vn;
  if (!segmentProjectionIsInside(s, xn))
    dp = 0;
  return dp;
}

#if DIMENSION == 3
static double contactParticleTriangleSolve(Contact *c, const double *v, const double *x, double r, Triangle *t, double dt) {
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = v[i] - t->v[i];
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
  for (int i = 0; i < DIMENSION; ++i)
    xn[i] = x[i] + r*c->n[i] + vlocfree[i]*c->D/vn;
  if (!triangleProjectionIsInside(t, xn))
    dp = 0;
  return dp;
}
#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
}

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 %i : %i %i %i\n", tic3-tic, tic3-tic2,tic2-tic1, tic1-tic);
  printf("%i / %i (%i%%)\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;
      }
    }
  }
  //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;
      }
    }
  }
  // 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;
      }
    }
  }
  // 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 _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];
    double dp;
    switch (c->type) {
      case PARTICLE_PARTICLE :
        dp = contactParticleParticleSolve(c, &p->velocity[c->o0 * DIMENSION], &p->velocity[c->o1 * DIMENSION], dt);
        break;
      case PARTICLE_DISK :
        dp = contactParticleDiskSolve(c, &p->velocity[c->o1*DIMENSION], &p->disks[c->o0], dt);
        break;
      case PARTICLE_SEGMENT :
        dp = contactParticleSegmentSolve(c, &p->velocity[c->o1*DIMENSION], &p->position[c->o1*DIMENSION], p->particles[c->o1].r,&p->segments[c->o0], dt);
        break;
#if DIMENSION == 3
      case PARTICLE_TRIANGLE :
        dp = contactParticleTriangleSolve(c, &p->velocity[c->o1*DIMENSION], &p->position[c->o1*DIMENSION], p->particles[c->o1].r,&p->triangles[c->o0], dt);
        break;
#endif
    }
    dp -= c->dv;
    if (c->type == PARTICLE_PARTICLE) {
      coordAdd(&p->velocity[c->o0 * DIMENSION], -dp*c->a0, c->n);
      coordAdd(&p->velocity[c->o1 * DIMENSION], dp*c->a1, c->n);
    }
    else {
      coordAdd(&p->velocity[c->o1 * DIMENSION], -dp, c->n);
    }
    if (fabs(dp) > tol/dt) {
      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]);
        }
      }
    }
    c->dv += dp;
    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);
  _particleProblemSolveContactsQueue(p,dt,tol);
}

void particleProblemIterate(ParticleProblem *p, double alert, double dt, double tol, int maxit)
{
  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
}

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

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

void particleProblemAddParticle(ParticleProblem *p, const double x[DIMENSION], double r, double m) {
  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);
  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;
  }
}

static void _writeContactVtkSingle(ParticleProblem *p, FILE *f, int ctype, const char *cname)
{
  size_t n = 0;
  for (size_t i = 0; i < vectorSize(p->contacts); ++i) {
    if (p->contacts[i].type == ctype)
      n++;
  }
  fprintf(f, "<DataArray Name=\"%s\" NumberOfTuples=\"%zu\" NumberOfComponents=\"1\" type=\"Float32\" format=\"ascii\">\n",cname, n);
  for (size_t i = 0; i < vectorSize(p->contacts); ++i) {
    if (p->contacts[i].type == ctype) {
      fprintf(f, "%.2g ", p->contacts[i].dv);
    }
  }
  fprintf(f, "\n</DataArray>\n");
  fprintf(f, "<DataArray Name=\"%s_idx\" NumberOfTuples=\"%zu\" NumberOfComponents=\"2\" type=\"UInt64\" format=\"ascii\">\n",cname,n);
  for (size_t i = 0; i < vectorSize(p->contacts); ++i) {
    if (p->contacts[i].type == ctype) {
      fprintf(f, "%zu %zu ", p->contacts[i].o0, p->contacts[i].o1);
    }
  }
  fprintf(f, "\n</DataArray>\n");
}

static int particleProblemWriteContactVtp(ParticleProblem *p, const char *dirname, int id) {
  char *filename = malloc((strlen(dirname)+50)*sizeof(char));
  sprintf(filename,"%s/contacts_%05d.vtp",dirname, id);
  FILE *f = fopen(filename, "w");
  if (!f){
    printf("cannot open file %s\n", filename);
    return -1;
  }
/*  size_t n_contacts[DIMENSION+2] = {0};*/
/*  for (size_t i = 0; i < vectorSize(p->contacts); ++i) {*/
/*    n_contacts[p->contacts[i].type]++;*/
/*  }*/
  fprintf(f, "<VTKFile type=\"PolyData\">\n");
  fprintf(f, "<PolyData>\n");
  fprintf(f, "<FieldData>\n");
  _writeContactVtkSingle(p,f,PARTICLE_PARTICLE,"particle_particle");
  _writeContactVtkSingle(p,f,PARTICLE_DISK,"particle_disk");
  _writeContactVtkSingle(p,f,PARTICLE_SEGMENT,"particle_segment");
#if DIMENSION == 3
  _writeContactVtkSingle(p,f,PARTICLE_TRIANGLE,"particle_triangle");
#endif
  fprintf(f, "</FieldData>\n");
  fprintf(f, "<Piece NumberOfPoints=\"1\" NumberOfCells=\"0\">\n");
  fprintf(f, "<Points>\n<DataArray NumberOfComponents=\"3\" type=\"Float32\" format=\"ascii\">\n");
  fprintf(f, "0 0 0\n");
  fprintf(f, "</DataArray>\n");
  fprintf(f, "</Points>\n");
  fprintf(f, "</Piece>\n");
  fprintf(f, "</PolyData>\n");
  fprintf(f, "</VTKFile>\n");
  fclose(f);
  free(filename);
  return 0;
}

static int particleProblemWriteParticleVtp(ParticleProblem *p, const char *dirname, int id) {
  char *filename = malloc((strlen(dirname)+50)*sizeof(char));
  sprintf(filename,"%s/particles_%05d.vtp",dirname, id);
  FILE *f = fopen(filename, "w");
  if (!f){
    printf("cannot open file %s\n", filename);
    return -1;
  }
  size_t n_pp_contacts = 0;
  for (size_t i = 0; i < vectorSize(p->contacts); ++i) {
    if (p->contacts[i].type == PARTICLE_PARTICLE)
      n_pp_contacts ++;
  }
  fprintf(f, "<VTKFile type=\"PolyData\">\n");
  fprintf(f, "<PolyData>\n");
  fprintf(f, "<Piece NumberOfPoints=\"%lu\" NumberOfCells=\"0\">\n", vectorSize(p->particles));
  fprintf(f, "<PointData Scalars=\"Radius\" Vectors=\"Velocity\">\n");
  fprintf(f, "<DataArray Name=\"Radius\" NumberOfComponents = \"1\" type=\"Float32\" format=\"ascii\">\n");
  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
    fprintf(f, "%.16g ", p->particles[i].r);
  }
  fprintf(f, "\n</DataArray>\n");
  fprintf(f, "<DataArray Name=\"Mass\" NumberOfComponents = \"1\" type=\"Float32\" format=\"ascii\">\n");
  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
    fprintf(f, "%.16g ", p->particles[i].m);
  }
  fprintf(f, "\n</DataArray>\n");
  fprintf(f, "<DataArray Name=\"Velocity\" NumberOfComponents = \"3\" type=\"Float32\" format=\"ascii\">\n");
  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
#if DIMENSION == 2
    fprintf(f, "%.16g %.16g 0 ", p->velocity[i * 2], p->velocity[i * 2 + 1]);
#else
    fprintf(f, "%.16g %.16g %.16g ", p->velocity[i * 3], p->velocity[i * 3 + 1], p->velocity[i * 3 + 2]);
#endif
  }
  fprintf(f, "\n</DataArray>\n");
  fprintf(f, "</PointData>\n");
  fprintf(f, "<Points>\n<DataArray NumberOfComponents=\"3\" type=\"Float32\" format=\"ascii\">\n");
  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
#if DIMENSION == 2
    fprintf(f, "%.16g %.16g 0 ", p->position[i * 2], p->position[i * 2 + 1]);
#else
    fprintf(f, "%.16g %.16g %.16g ", p->position[i * 3], p->position[i * 3 + 1], p->position[i * 3 + 2]);
#endif
  }
  fprintf(f, "\n</DataArray>\n");
  fprintf(f, "</Points>\n");
  fprintf(f, "</Piece>\n");
  fprintf(f, "</PolyData>\n");
  fprintf(f, "</VTKFile>\n");
  fclose(f);
  free(filename);
  return 0;
}

static int particleProblemWriteBndVtu(ParticleProblem *p, const char *dirname, int id) {
  char *filename = malloc((strlen(dirname)+50)*sizeof(char));
  sprintf(filename,"%s/boundaries_%05d.vtu",dirname, id);
  size_t nd = vectorSize(p->disks);
  size_t ns = vectorSize(p->segments);
#if DIMENSION == 3
  size_t nt = vectorSize(p->triangles);
#else
  size_t nt = 0;
#endif
  FILE *f = fopen(filename, "w");
  fprintf(f, "<VTKFile type=\"UnstructuredGrid\">\n");
  fprintf(f, "<UnstructuredGrid>\n");
  fprintf(f, "<FieldData>\n");
  fprintf(f, "<DataArray Name=\"sizes\" NumberOfTuples=\"3\" NumberOfComponents=\"1\" type=\"Int32\" format=\"ascii\">\n");
  fprintf(f, "%i %i %i\n", (int)vectorSize(p->disks), (int)vectorSize(p->segments), 0);
  fprintf(f, "</DataArray>\n");
  fprintf(f, "</FieldData>\n");
  fprintf(f, "<Piece NumberOfPoints=\"%lu\" NumberOfCells=\"%lu\">\n", nt*3+ns*2+nd, nt+ns+nd);
  fprintf(f, "<Points>\n<DataArray NumberOfComponents=\"3\" type=\"Float32\" format=\"ascii\">\n");
  for (size_t i = 0; i < nd; ++i) {
    Disk *d = &p->disks[i];
    fprintf(f, "%.16g %.16g %.16g ", d->x[0], d->x[1], DIMENSION == 2 ? 0. : d->x[2]);
  }
  for (size_t i = 0; i < ns; ++i) {
    Segment *s = &p->segments[i];
    fprintf(f, "%.16g %.16g %.16g ", s->p[0][0], s->p[0][1], DIMENSION == 2 ? 0. : s->p[0][2]);
    fprintf(f, "%.16g %.16g %.16g ", s->p[1][0], s->p[1][1], DIMENSION == 2 ? 0. : s->p[1][2]);
  }
#if DIMENSION == 3
  for (int i = 0; i < nt; ++i) {
    Triangle *t = &p->triangles[i];
    fprintf(f, "%.16g %.16g %.16g ", t->p[0][0], t->p[0][1], t->p[0][2]);
    fprintf(f, "%.16g %.16g %.16g ", t->p[1][0], t->p[1][1], t->p[1][2]);
    fprintf(f, "%.16g %.16g %.16g ", t->p[2][0], t->p[2][1], t->p[2][2]);
  }
#endif
  fprintf(f, "\n</DataArray>\n");
  fprintf(f, "</Points>\n");
  fprintf(f, "<Cells>\n");
  fprintf(f, "<DataArray type=\"Int32\" Name=\"connectivity\">\n");
  for (size_t i = 0; i < nd; ++i) {
    fprintf(f, "%i ", (int)(i));
  }
  for (size_t i = 0; i < ns; ++i) {
    fprintf(f, "%i %i ", (int)(nd+i*2), (int)(nd+i*2+1));
  }
#if DIMENSION == 3
  for (size_t i = 0; i < nt; ++i) {
    fprintf(f, "%i %i %i ", (int)(nd+ns*2+i*3+0), (int)(nd+ns*2+i*3+1), (int)(nd+ns*2+i*3+2));
  }
#endif
  fprintf(f, "\n</DataArray>\n");
  fprintf(f, "<DataArray type=\"Int32\" Name=\"offsets\">\n");
  for (size_t i = 0; i < nd; ++i) {
    fprintf(f, "%i ", (int)(i+1));
  }
  for (size_t i = 0; i < ns; ++i) {
    fprintf(f, "%i ", (int)nd+(int)(i+1)*2);
  }
  for (size_t i = 0; i < nt; ++i) {
    fprintf(f, "%i ", (int)(nd+ns*2+(i+1)*3));
  }
  fprintf(f, "\n</DataArray>\n");
  fprintf(f, "<DataArray type=\"Int32\" Name=\"types\">\n");
  for (size_t i = 0; i < nd; ++i) {
    fprintf(f, "%i ", 1);
  }
  for (size_t i = 0; i < ns; ++i) {
    fprintf(f, "%i ", 3);
  }
  for (size_t i = 0; i < nt; ++i) {
    fprintf(f, "%i ", 5);
  }
  fprintf(f, "\n</DataArray>\n");
  fprintf(f, "</Cells>\n");
  fprintf(f, "</Piece>\n");
  fprintf(f, "</UnstructuredGrid>\n");
  fprintf(f, "</VTKFile>\n");
  fclose(f);
  free(filename);
  return 0;
}

int particleProblemWriteVtk(ParticleProblem *p, const char *dirname, int id) {
  if (particleProblemWriteParticleVtp(p,dirname,id) != 0)
    return -1;
  if (particleProblemWriteBndVtu(p,dirname,id) != 0)
    return -1;
  if (particleProblemWriteContactVtp(p,dirname,id) != 0)
    return -1;
  char *filename = malloc((strlen(dirname)+50)*sizeof(char));
  sprintf(filename,"%s/particles_problem_%05d.vtm",dirname, id);
  FILE *f = fopen(filename, "w");
  if (!f)
    return -1;
  fprintf(f, "<VTKFile type=\"vtkMultiBlockDataSet\" version=\"1.0\" byte_order=\"LittleEndian\">\n");
  fprintf(f, "<vtkMultiBlockDataSet>\n");
  fprintf(f, "<DataSet index=\"0\" file=\"particles_%05d.vtp\">\n",id);
  fprintf(f, "</DataSet>\n");
  fprintf(f, "<DataSet index=\"1\" file=\"boundaries_%05d.vtu\">\n",id);
  fprintf(f, "</DataSet>\n");
  fprintf(f, "<DataSet index=\"2\" file=\"contacts_%05d.vtp\">\n",id);
  fprintf(f, "</DataSet>\n");
  fprintf(f, "</vtkMultiBlockDataSet>\n");
  fprintf(f,"</VTKFile>\n");
  fclose(f);
  free(filename);

  return 0;
}

#include "mbxml.h"
int particleProblemImportVtk(ParticleProblem *p, const char *filename)
{
  MbXmlReader *r = mb_xml_reader_create(filename);
  MbXmlElement fileelement,gridelement,pieceelement;
  mb_xml_read_element(r,&fileelement);
  mb_xml_read_element(r,&gridelement);
  mb_xml_read_element(r,&pieceelement);
  MbXmlElement e;
  MBVtkDataArray a;
  size_t n_particles;
  for (size_t i = 0; i<pieceelement.n_attr; ++i) {
    if(!strcasecmp(pieceelement.attr[i].name,"NumberOfPoints")){
      sscanf(pieceelement.attr[i].value,"%lu", &n_particles);
      vectorClear(p->position);
      vectorClear(p->particles);
      vectorClear(p->externalForces);
      vectorClear(p->velocity);
      vectorPushN(&p->position,n_particles*DIMENSION);
      vectorPushN(&p->particles,n_particles);
      vectorPushN(&p->externalForces,n_particles*DIMENSION);
      vectorPushN(&p->velocity, n_particles*DIMENSION);
    }
  }
  while(mb_xml_read_element(r,&e)!=1) {
    if (!strcasecmp(e.name,"Points")){
      mb_read_vtk_data_array(r,&a);
      double *position = mb_vtk_data_array_to_double(&a,n_particles,3);
      for (size_t i = 0; i < n_particles; ++i)
        for (int j = 0; j < DIMENSION; ++j)
          p->position[DIMENSION*i+j] = position[i*3+j];
      free(position);
      mb_vtk_data_array_destroy(&a);
    }
    else if (!strcasecmp(e.name,"PointData")) {
      while(mb_read_vtk_data_array(r,&a) != 0){
        double *value = mb_vtk_data_array_to_double(&a,n_particles,-1);
        if(!strcasecmp(a.name,"Velocity")) {
          for (size_t i = 0; i < n_particles; ++i)
            for (int j = 0; j < DIMENSION; ++j)
              p->velocity[DIMENSION*i+j] = value[i*3+j];
        }
        else if(!strcasecmp(a.name,"Mass")) {
          for (size_t i = 0; i < n_particles; ++i)
            p->particles[i].m = value[i];
        }
        else if(!strcasecmp(a.name,"Radius")) {
          for (size_t i = 0; i < n_particles; ++i){
            p->particles[i].r = value[i];
          }
        }
        free(value);
        mb_vtk_data_array_destroy(&a);
      }
    }
    else {
      printf("unknown type in piece : \"%s\"\n", e.name);
      exit(1);
    }
    mb_xml_end_element(r,&e);
  }
  mb_xml_end_element(r,&pieceelement);
  mb_xml_end_element(r,&gridelement);
  mb_xml_end_element(r,&fileelement);
  mb_xml_reader_destroy(r);
  return 0;
}

int particleProblemImportBoundaryVtk(ParticleProblem *p, const char *filename)
{
  MbXmlReader *r = mb_xml_reader_create(filename);
  MbXmlElement fileelement,gridelement;
  mb_xml_read_element(r,&fileelement);
  mb_xml_read_element(r,&gridelement);
  MbXmlElement e;
  MBVtkDataArray a;
  while(mb_xml_read_element(r,&e)!=1) {
    if (!strcasecmp(e.name, "FieldData")) {
      while(mb_read_vtk_data_array(r,&a)) {
        mb_vtk_data_array_destroy(&a);
      }
    }
    else if(!strcasecmp(e.name,"Piece")) {
      int n_points, n_cells;
      for(size_t i = 0; i < e.n_attr; ++i) {
        if (!strcasecmp("NumberOfPoints",e.attr[i].name))
          sscanf(e.attr[i].value,"%i",&n_points);
        if (!strcasecmp("NumberOfCells",e.attr[i].name))
          sscanf(e.attr[i].value,"%i",&n_cells);
      }
      MbXmlElement f;
      for (size_t i = 0; i<e.n_attr; ++i) {
        MBVtkDataArray points={0}, connectivity={0}, offsets={0}, types={0};
        while(mb_xml_read_element(r,&f)!=1) {
          if (!strcasecmp(f.name,"Points"))
            mb_read_vtk_data_array(r,&points);
          else if (!strcasecmp(f.name,"Cells")) {
            while(mb_read_vtk_data_array(r,&a) != 0){
              if(!strcasecmp(a.name,"connectivity"))
                connectivity = a;
              else if (!strcasecmp(a.name,"types"))
                types = a;
              else if (!strcasecmp(a.name,"offsets"))
                offsets = a;
              else {
                printf("unexpected data array in \"%s\" : \"%s\"\n",filename, a.name);
                mb_vtk_data_array_destroy(&a);
              }
            }
            vectorClear(p->disks);
            vectorClear(p->segments);
#if DIMENSION == 3
            vectorClear(p->triangles);
#endif
            double *pts = mb_vtk_data_array_to_double(&points,n_points,3);
            int *off = mb_vtk_data_array_to_int(&offsets,n_cells,1);
            int *con = mb_vtk_data_array_to_int(&connectivity,off[n_cells-1],1);
            int *typ = mb_vtk_data_array_to_int(&types,n_cells,1);
            mb_vtk_data_array_destroy(&points);
            mb_vtk_data_array_destroy(&offsets);
            mb_vtk_data_array_destroy(&connectivity);
            mb_vtk_data_array_destroy(&types);
            for(size_t i = 0; i < n_cells; ++i) {
              int t = typ[i];
              int *nodes = con+(i==0 ? 0 : off[i-1]);
              if (t == 1){
                particleProblemAddBoundaryDisk(p,pts+nodes[0]*3,0.,"");
              }
              else if (t == 3)
                particleProblemAddBoundarySegment(p, pts+nodes[0]*3,pts+nodes[1]*3,"");
#if DIMENSION == 3
              else if (t == 5)
                particleProblemAddBoundaryTriangle(p, pts+nodes[0]*3,pts+nodes[1]*3,pts+nodes[2]*3,"");
#endif
              else
                printf("Unknown boundary object : %i\n", t);
            }
          }
          else {
            printf("unexpected element in \"%s\" : \"%s\"\n",filename, f.name);
            exit(1);
          }
          mb_xml_end_element(r,&f);
        }
      }
    }
    else {
      printf("unexpected element in \"%s\" : \"%s\"\n",filename, e.name);
      exit(1);
    }
    mb_xml_end_element(r,&e);
  }
  mb_xml_end_element(r,&gridelement);
  mb_xml_end_element(r,&fileelement);
  mb_xml_reader_destroy(r);
  return 0;
}

int particleProblemReadVtk(ParticleProblem *p, const char *dirname, int iter)
{
  char *f0 = malloc(sizeof(char)*(strlen(dirname)+30));
  char *f1 = malloc(sizeof(char)*(strlen(dirname)+30));
  sprintf(f0, "%s/particles_%05d.vtp",dirname,iter);
  sprintf(f1, "%s/boundaries_%05d.vtu",dirname,iter);
  particleProblemImportVtk(p,f0);
  particleProblemImportBoundaryVtk(p,f1);
  free(f0);
  free(f1);
  return 0;
}

int *particleProblemDiskTag(ParticleProblem *p) {return p->diskTag;}
int *particleProblemSegmentTag(ParticleProblem *p) {return p->segmentTag;};
#if DIMENSION == 3
int *particleProblemTriangleTag(ParticleProblem *p) {return p->triangleTag;};
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 particleProblemUseJacobi(ParticleProblem *p, int jacobi, double relax) {p->jacobi = jacobi; p->relax = relax;}
void particleProblemDelete(ParticleProblem *p) {
  vectorFree(p->particles);
  vectorFree(p->disks);
  vectorFree(p->diskTag);
  vectorFree(p->segments);
  vectorFree(p->segmentTag);
  vectorFree(p->contacts);
  vectorFree(p->position);
  vectorFree(p->velocity);
  vectorFree(p->externalForces);
  #if DIMENSION == 3
  vectorFree(p->triangles);
  vectorFree(p->triangleTag);
  #endif
  for (size_t i = 0; i < vectorSize(p->tagname); ++i)
    free(p->tagname[i]);
  vectorFree(p->tagname);
  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;
}

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->jacobi = 0;
  p->relax = 1.;
  p->particles = NULL;
  p->contacts = NULL;
  p->tagname = NULL;
  p->disks = NULL;
  p->diskTag = NULL;
  p->segments = NULL;
  p->segmentTag = NULL;
  p->position = NULL;
  p->velocity = NULL;
  p->externalForces = NULL;
  #if DIMENSION == 3
  p->triangles = NULL;
  p->triangleTag = NULL;
  #endif
  return p;
}

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

double *particleProblemRadius(ParticleProblem *p) {
  return (double*)p->particles;
}