#include <alloca.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include <stdlib.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 tsp(point cities[], point tour[], int ncities)
{
  int i,j,k,l;
  int ClosePt=0;
  double CloseDist;
  double closedist[SIMD_SIZE];
  double dist[SIMD_SIZE];
  double *tourx=alloca((ncities+SIMD_SIZE)*sizeof(double));
  double *toury=alloca((ncities+SIMD_SIZE)*sizeof(double));
  
  for (i=1; i<ncities; i++) {
    point c=cities[i-1];
    tour[i]=c;
    tourx[i]=c.x;
    toury[i]=c.y;
  }
  for (k=0; k<SIMD_SIZE; k++) {
    tourx[i+k]=DBL_MAX;
    toury[i+k]=DBL_MAX;
  }
  tour[0] = cities[ncities-1];
  tourx[0] = cities[ncities-1].x;
  toury[0] = cities[ncities-1].y;
  
  for (i=1; i<ncities; i++) {
    double ThisX = tourx[i-1];
    double ThisY = toury[i-1];
    int closer=0;
    for (k=0; k<SIMD_SIZE; k++)
        closedist[k] = DBL_MAX;
    j=i;
    do {
      for (j=SIMD_SIZE; j<ncities; j+=SIMD_SIZE) {
        for (k=0; k<SIMD_SIZE; k++)
          dist[k] = sqr(tourx[k+j]-ThisX)+sqr(toury[k+j]-ThisY);
        for (k=0; k<SIMD_SIZE; k++)
          if (dist[k]<closedist[k])
            closer=1;
        if (closer)
          break;
      }
      for (k=0; k<SIMD_SIZE; k++)
        if (dist[k]<closedist[k]) {
          ClosePt = j;
          CloseDist=dist[k];
          for (l=0; l<SIMD_SIZE; l++)
            closedist[l] = CloseDist;
          break;
        }
      j += k;
    } while (j<ncities);
    swap(&tour[i],&tour[ClosePt]);
    tourx[i]=tour[i].x;
    tourx[ClosePt]=tour[ClosePt].x;
    toury[i]=tour[i].y;
    toury[ClosePt]=tour[ClosePt].y;
  }
}

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 = alloca(ncities*sizeof(point));
  tour = alloca(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);
}