#include <alloca.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include <stdlib.h>
#include <error.h>
#include <string.h>

#define SIMD_SIZE 8

typedef struct {
  double x;
  double y;
} point;

double sqr(double x)
{
  return x*x;
}

double dist(point cities[], int i, int j) {
  return sqrt(sqr(cities[i].x-cities[j].x)+
	      sqr(cities[i].y-cities[j].y));
}

double DistSqrd(point cities[], int i, int j) {
  return (sqr(cities[i].x-cities[j].x)+
	  sqr(cities[i].y-cities[j].y));
}

void swap(point *p, point *q)
{
  point tmp=*p;
  *p = *q;
  *q = tmp;
}

void swapd(double *p, double *q)
{
  double tmp=*p;
  *p = *q;
  *q = tmp;
}

void tsp(point cities[], point tour[], int ncities)
{
  size_t i,j;
  size_t ClosePt=0;
  double CloseDist;
  double *x, *y;
  void *x1, *y1;
  int err;
  size_t size=sizeof(double)*(ncities+2*SIMD_SIZE)&~(sizeof(double)*SIMD_SIZE-1);
  if ((err=posix_memalign(&x1,sizeof(double)*SIMD_SIZE,size)))
    error(1,err,"tsp");
  x = x1+size-sizeof(double)*ncities;
  if ((err=posix_memalign(&y1,sizeof(double)*SIMD_SIZE,size)))
    error(1,err,"tsp");
  y = y1+size-sizeof(double)*ncities;
  
  for (i=1; i<ncities; i++) {
    x[i]=cities[i-1].x;
    y[i]=cities[i-1].y;
  }
  x[0] = cities[ncities-1].x;
  y[0] = cities[ncities-1].y;
  tour[0] = cities[ncities-1];
  
  for (i=1; i<ncities; i++) {
    double ThisX = x[i-1];
    double ThisY = y[i-1];
    double ThisDist[SIMD_SIZE];
    CloseDist = DBL_MAX;
    for (j=ncities; ;) {
      int closer=0;
      long k;
      do {
        j-=SIMD_SIZE;
        for (k=0; k<SIMD_SIZE; k++)
          ThisDist[k] = sqr(x[k+j]-ThisX)+sqr(y[k+j]-ThisY);
        for (k=0; k<SIMD_SIZE; k++)
          if (ThisDist[k]<CloseDist)
            closer=1;
      } while (!closer);
      for (k=SIMD_SIZE-1; k>=0; k--)
        if (ThisDist[k]<CloseDist)
          break;
      j+=k;
      if (j < i)
        break;
      CloseDist = ThisDist[k];
      ClosePt = j;
    }
    // printf("swap %ld <-> %ld\n",i,ClosePt);
    swapd(&x[i],&x[ClosePt]);
    swapd(&y[i],&y[ClosePt]);
    tour[i].x = x[i];
    tour[i].y = y[i];
  }
  free(x1);
  free(y1);
}

int main(int argc, char *argv[])
{
  int i, ncities;
  point *cities;
  point *tour;
  FILE *psfile;
  double sumdist = 0.0;

  if (argc!=2) {
    fprintf(stderr, "usage: %s <ncities>", argv[0]);
    exit(1);
  }
  ncities = atoi(argv[1]);
  cities = malloc(ncities*sizeof(point));
  tour = malloc(ncities*sizeof(point));
  for (i=0; i<ncities; i++) {
    cities[i].x = ((double)(random()))/(double)(1U<<31);
    cities[i].y = ((double)(random()))/(double)(1U<<31);
  }
  tsp(cities,tour,ncities);
  psfile = fopen("tsp.eps","w");
  fprintf(psfile, "%%!PS-Adobe-2.0 EPSF-1.2\n%%%%BoundingBox: 0 0 300 300\n");
  fprintf(psfile, "1 setlinejoin\n0 setlinewidth\n");
  fprintf(psfile, "%f %f moveto\n",
	  300.0*tour[0].x, 300.0*tour[0].y);
  for (i=1; i<ncities; i++) {
    fprintf(psfile, "%f %f lineto\n",
	    300.0*tour[i].x, 300.0*tour[i].y);
    sumdist += dist(tour, i-1, i);
  }
  fprintf(psfile,"stroke\n");
  printf("sumdist = %f\n", sumdist);
  exit(0);
}