#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>


#define dt .001
#define sqr(a) ((a)*(a))
#define cube(a) ((a)*(a)*(a))
#define sixth(a) sqr(cube(a))
#define twelfth(a) sqr(sixth(a))
#define TOL 1E-6
#define MAXPARTICLES 100
#define MINDIST .55
#define CUBESIZE 10
#define SIXTWELVEOFFSET .5

int numParticles = 2;

double particles[MAXPARTICLES][2][2][3];  //which particle, time, x/v, x/y/z
double masses[MAXPARTICLES];
FILE *FOUT;

void dist(double * ans, double * a, double * b) {  // a vector from a -> b
   ans[0] = a[0] - b[0];
   ans[1] = a[1] - b[1];
   ans[2] = a[2] - b[2];
}

double vabs(double *a) {
   return sqrt(sqr(a[0]) + sqr(a[1]) + sqr(a[2]));
}

double expt(double a, int n) {
   int i;
   double ans = 1;
   for(i = 1; i < n; i++)
      ans *= a;
   return ans;
}

void force(double *ans, double coords[2][3], double *masses) { // provides the force
   double dst[3];
   double F;
   int i;
   dist(dst, coords[0], coords[1]);
// Sprint
//   F = vabs(dst) - 4.0;

// Gravity
/*   if(vabs(dst) > MINDIST)
      F = - masses[0] * masses[1] / sqr(vabs(dst));
   else
      F = - masses[0] * masses[1] / sqr(MINDIST); // */
// "Six-twelve"
   F = 1.2/expt(vabs(dst) + SIXTWELVEOFFSET, 13) - 12/expt(vabs(dst) + SIXTWELVEOFFSET, 6);

   for(i = 0; i < 3; i++)
      ans[i] = F * dst[i] / vabs(dst);
}

void leapfrog() {
   double curF[3], t, Fbin[MAXPARTICLES][3], virtualCube[3], newCoords[2][3], newMasses[2];
   static int offset = 0;
   int d, j, p, p1, p2;
   int i, which = 1;

   //push x forward
//   force(curF, 0, 1);
/*   for(d = 0; d < 3; d++) {
      for(p = 0; p < numParticles; p++)
         particles[p][0][0][d] += particles[p][0][1][d] * (dt / 2) + curF[d] * (dt * dt / 4) / 2;
   }*/

   //calculate and draw each point
   for(i = 0, t = 0; i < 1000000/*100000000*/; i++, t += dt, which ^= 1) {
      for(p = 0; p < numParticles; p++)
         for(d = 0; d < 3; d++)
            Fbin[p][d] = 0;
      for(p1 = 0; p1 < numParticles; p1++) {
         for(d = 0; d < 3; d++)
            newCoords[0][d] = particles[p1][1 - which][0][d];
         newMasses[0] = masses[p1];
         for(p2 = p1 + 1; p2 < numParticles; p2++)
            for(virtualCube[0] = -1; virtualCube[0] < 2; virtualCube[0]++)
               for(virtualCube[1] = -1; virtualCube[1] < 2; virtualCube[1]++)
                  for(virtualCube[2] = -1; virtualCube[2] < 2; virtualCube[2]++) {
                     for(d = 0; d < 3; d++)
                        newCoords[1][d] = particles[p2][1 - which][0][d] + virtualCube[d] * CUBESIZE;
                     newMasses[1] = masses[p2];
                     //for(d = 0; d < 3; d++) printf("%12g ", newCoords[0][d]); printf("\n");
                     //for(d = 0; d < 3; d++) printf("%12g ", newCoords[1][d]); printf("\n");
                     force(curF, newCoords, masses);
                     //for(d = 0; d < 3; d++) printf("%12g ", curF[d]); printf("\n"); printf("\n");
                     for(d = 0; d < 3; d++) {
                        Fbin[p1][d] += curF[d];
                        Fbin[p2][d] -= curF[d];
                     }
                  }
      }
//      printf("\n");
      for(p = 0; p < numParticles; p++) {
         for(d = 0; d < 3; d++) {
            particles[p][which][1][d] = particles[p][1 - which][1][d] + (Fbin[p][d] / masses[p]) * dt;
            particles[p][which][0][d] = particles[p][1 - which][0][d] + particles[p][which][1][d] * dt;
            if(particles[p][which][0][d] > CUBESIZE / 2)
               particles[p][which][0][d] = fmod(particles[p][which][0][d] + CUBESIZE / 2, CUBESIZE) - CUBESIZE / 2;
            if(particles[p][which][0][d] < -CUBESIZE / 2)
               particles[p][which][0][d] = fmod(particles[p][which][0][d] - CUBESIZE / 2, CUBESIZE) + CUBESIZE / 2;
         }
      }

      fprintf(FOUT, "%12g ", t);
      for(p = 0; p < numParticles; p++)
         for(d = 0; d < 3; d++)
            fprintf(FOUT, "%12g %12g ", particles[p][which][0][d], particles[p][which][1][d]);
      fprintf(FOUT, "\n");
//      if(!(++offset % 100))
//         fprintf(stderr, "%10d: %12.8g\n", offset, t);
   }
}

int main(int argc, char *argv[]) {
   int i, j, k;
   double total, totalMass;

   srand(time(NULL));

   if(argc > 1)
      FOUT = fopen(argv[1], "w");
   else
      FOUT = fopen("plural.in", "w");


   for(i = 0; i < numParticles; i++) {
      for(k = 0; k < 2; k++)
         particles[i][0][0][k] = ((double)(random()) / (RAND_MAX / CUBESIZE)) - (CUBESIZE / 2);
      for(k = 0; k < 2; k++)
         particles[i][0][1][k] = ((double)(random()) / (RAND_MAX / 8)) - 4;
   }

   fprintf(FOUT, "%d ", numParticles);
   for(i = 0; i < numParticles; i++) {
      masses[i] = 5;
      fprintf(FOUT, "%g ", masses[i]);
   }
   fprintf(FOUT, "\n");

   for(j = 0; j < 2; j++)
      for(k = 0; k < 2; k++) {
         total = 0;
         for(i = 0; i < numParticles; i++)
            total += particles[i][0][j][k] * masses[i];
         for(i = 0; i < numParticles; i++)
            particles[i][0][j][k] -= total / masses[i] / numParticles;
      }

   particles[0][0][0][0] = -0.05;
   particles[0][0][0][1] = 0;
   particles[0][0][1][0] = 0;
   particles[0][0][1][1] = 0;
   particles[1][0][0][0] = 0.05;
   particles[1][0][0][1] = 0;
   particles[1][0][1][0] = 0;
   particles[1][0][1][1] = 0;


   leapfrog();
}