/*--------------------------------------------------------------------------
 Approximate TSP solvers.
 See p.363, "Discrete Optimization Algorithms," Maciej Syslo, Prentice-Hall
--------------------------------------------------------------------------*/
static char SCCSID[] = "@(#)tsp.c	1.1 5/20/92";

#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
#include <values.h>
#include <math.h>

#ifdef ARRAYTEST
/* see pg. 367, Syslo */
#define DIST(a,b) DISTa[a][b]
#define BIG 0	/* book is wrong */
static int DISTa[6][6] = {
	{BIG,	3,	93,	13,	33,	9},
	{4,	BIG,	77,	42,	21,	16},
	{45,	17,	BIG,	36,	16,	28},
	{39,	90,	80,	BIG,	56,	7},
	{28,	46,	88,	33,	BIG,	25},
	{3,	88,	18,	46,	92,	BIG}};
#else
/* warning: this macro cannot be called twice in the same arithmetic
 * expression
 */
static int dx, dy;
#define DIST(a,b) \
    (dx=(x[a]-x[b]),dy=(y[a]-y[b]),(int)(0.5+sqrt((double)(dx*dx+dy*dy))))

#endif

/*--------------------------------------------------------------------------
 Approximate solver using the Farthest Insertion heuristic. 
 Returns total distance of one-way trip generated by deleting
 longest edge of cycle.
 If nih is TRUE, uses Nearest Insertion heuristic instead.
--------------------------------------------------------------------------*/
int tsp_fi(
    /* Inputs: */
    int n,		/* number of nodes */
    int s, 		/* starting node */
    int *x,		/* X coordinate of each node */
    int *y,		/* Y coordinate of each node */
    int nih,		/* if TRUE, use nearest insertion heuristic */ 
    /* Output: */
    int *route)		/* route[i=0..n-1]= index of ith node in tour */
{
    int end1, end2;
    int i, j, argmaxcost;
    int index, nextindex;
    int inscost, newcost, total_cost, maxcost;

    int *cycle;	/* cycle[i]=next node after node i in tour; -1 if not yet in */
    int *dist;	/* dist[i] =dist from any node in tour to node i not in tour */

    assert(n > 0);

    /* Allocate variable-sized arrays */
    cycle = (int *)malloc(n * sizeof(cycle[0]));
    assert(cycle != NULL);
    dist  = (int *)malloc(n * sizeof(dist[0]));
    assert(dist != NULL);

    /* No nodes in tour yet */
    for (i=0; i<n; i++) cycle[i] = -1;
    /* Start with a one-node tour starting and ending at node s */
    cycle[s] = s;
    total_cost = 0;
    /* Find distance of all nodes from closest node in tour */
    for (i=0; i<n; i++) dist[i] = DIST(s, i);

#if 0
    printf("Dist[i,j]=\n");
    for (i=0; i<n; i++) {
	for (j=0; j<n; j++)
	    printf("%d ", DIST(i, j));
	printf("\n");
    }
#endif

    /* Until all nodes have been added to the tour */
    for (i=0; i<n-1; i++) {

	/* Find node which is farthest from (nearest to) the tour */
	maxcost = nih ? MAXINT : -MAXINT;
	argmaxcost = -1;
	for (j=0; j<n; j++) {
	    if (cycle[j] < 0) {
		if ((!nih && (dist[j] > maxcost)) || (nih&&(dist[j]<maxcost))) {
		    maxcost = dist[j];
		    argmaxcost = j;
		}
	    }
	}
	assert(argmaxcost >= 0 && argmaxcost < n);

	/* Farthest node is argmaxcost, distance is maxcost. */
	/* Now find cheapest place to insert it into the tour. */
	inscost = MAXINT;
	index = s;
	for (j=0; j<=i; j++) {
	    nextindex = cycle[index];
	    assert(nextindex >= 0 && nextindex < n);
	    newcost = DIST(index, argmaxcost);
	    newcost += DIST(argmaxcost, nextindex);
	    newcost -= DIST(index, nextindex);
	    if (newcost < inscost) {
		inscost = newcost;
		end1 = index;
		end2 = nextindex;
	    }
	    index = nextindex;
	}

	/* Cheapest place is after end1 and before end2.  Insert it there. */
	assert(cycle[end1] == end2);
	cycle[end1] = argmaxcost;
	cycle[argmaxcost] = end2;
	total_cost += inscost;
#if 0
	printf("Inserting node %d between %d and %d\n", argmaxcost, end1, end2);
	printf("Total_cost %d += (%d+%d-%d) = %d\n", 
	    total_cost-inscost, DIST(end1,argmaxcost), 
	    DIST(argmaxcost,end2),DIST(end1,end2),total_cost);
#endif

	/* Now that there's a new node in the tour, update the array of
	 * distances from other nodes to the tour.
	 */
	for (j=0; j<n; j++) {
	    /* If node j not in tour, */
	    if (cycle[j] < 0) {
		/* If closer to the new node than to any other node in the tour,
		 * record the new distance.
		 */
		newcost = DIST(argmaxcost, j);
		if (newcost < dist[j])
		    dist[j] = newcost;
	    }
	}
#if 0
	printf("Dist[]=");
	for (j=0; j<n; j++)
	    printf("%d ", dist[j]);
#endif
    }

    /* Find longest move in tour. */
    index = s;
    maxcost = -MAXINT;
    for (i=0; i<n; i++) {
	newcost = DIST(index, cycle[index]);
	if (newcost > maxcost) {
	    maxcost = newcost;
	    argmaxcost = index;
	}
	index = cycle[index];
    }
#if 0
    printf("Longest move in tour is %d,%d, cost %d\n", 
	argmaxcost, cycle[argmaxcost], maxcost);
#endif

    /* Return the tour to the caller as an array of nodes to visit
     * rather than a map {current node, next node} as it is in dist[].
     * Start tour such that longest move in tour is (route[n-1],route[0]).
     */
#ifdef ARRAYTEST
    maxcost = 0;
    index = s;
#else
    index = cycle[argmaxcost];
#endif
    for (i=0; i<n; i++) {
	route[i] = index;
	index = cycle[index];
    }

    free(cycle);
    free(dist);

    return total_cost - maxcost;	/* Return length of one-way trip */
}

/*--------------------------------------------------------------------------
 Run tsp_fi in several different ways, as directed by try_n, nih, and fih.
 Returns total distance of one-way trip generated by deleting
 longest edge of cycle.
--------------------------------------------------------------------------*/
int tsp_fin(
    /* Inputs: */
    int n,		/* number of nodes */
    int *x,		/* X coordinate of each node */
    int *y,		/* Y coordinate of each node */
    int try_n,		/* if TRUE, call solver n times */
    int nih,		/* if TRUE, call nearest-insertion solver */
    int fih,		/* if TRUE, call farthest-insertion solver */
    /* Output: */
    int *route)		/* route[i=0..n-1]= index of ith node in tour */
{
    int len, minlen;
    int *temproute;
    int s;
    int j;

    temproute = (int *)malloc(n * sizeof(temproute[0]));
    assert(temproute != NULL);

    /* For each starting node */
    minlen = MAXINT;
    for (s=0; s<n; s++) {
	if (fih) {
	    len = tsp_fi(n, s, x, y, 0, temproute);
	    /* If this solution is better, save it. */
	    if (len < minlen) {
		minlen = len;
		for (j=0; j<n; j++)
		    route[j] = temproute[j];
	    }
	}
	if (nih) {
	    len = tsp_fi(n, s, x, y, 1, temproute);
	    /* If this solution is better, save it. */
	    if (len < minlen) {
		minlen = len;
		for (j=0; j<n; j++)
		    route[j] = temproute[j];
	    }
	}
	/* Stop after first call if not trying n starting points. */
	if (!try_n) break;
    }

    free(temproute);
    return minlen;
}

#ifdef MAIN

#ifdef ARRAYTEST
main()
{
    int tour[10];
    int dist;
    int i;

    printf("Book test:\n");
    dist = tsp_fi(6, 0, NULL, NULL, 0, tour);
    printf("dist = %d\n", dist);
    for (i=0; i<6; i++) {
	printf("tour[%d] = %d\n", i, tour[i]);
    }
    assert(dist == 104);

    exit(0);
}

#else

main()
{
    static int x[10] = {0, 1, 1, 0, -1};
    static int y[10] = {0, 0, 1, 1, -1};
    int tour[10];
    int dist;
    int i;

    printf("Unit triangle:\n");
    dist = tsp_fi(3, 0, x, y, 0, tour);
    printf("dist = %d\n", dist);
    for (i=0; i<3; i++) {
	printf("tour[%d] = %d\n", i, tour[i]);
    }
    assert(dist == 2);

    printf("Unit square:\n");
    dist = tsp_fi(4, 0, x, y, 1, tour);
    printf("dist = %d\n", dist);
    for (i=0; i<4; i++) {
	printf("tour[%d] = %d\n", i, tour[i]);
    }
    assert(dist == 3);

    printf("Unit square with diagonal tail:\n");
    dist = tsp_fin(5, x, y, 1, 1, 1, tour);
    printf("dist = %d\n", dist);
    for (i=0; i<5; i++) {
	printf("tour[%d] = %d\n", i, tour[i]);
    }
    assert(dist == 4);

    exit(0);
}
#endif

#endif