/* Program for generating data and tsp tours for Euclidean and toroidal       */
/* instances in  K dimensions.                                                */
/* Recursive tsp tour construction optimized with a fast POPMUSIC method.     */

/* The method is described in:                                                */
/* É. D. Taillard                                                             */
/* A Linearithmic Heuristic for the Travelling Salesman Problem,              */
/* EJOR, 2021.                                                                */
/* https://doi.org/10.1016/j.ejor.2021.05.034                                 */
/* https://mistic.iict-heig-vd.ch/taillard/codes.dir/tsp_fast_popmusic.c      */ 

/* The following compiling options works                                      */
/* gcc -O2 tsp_fast_popmusic.c -o tsp_fast_popmusic.exe -lm                   */
/* tsp_fast_popmusic.exe 10000 20 15 225 400 3 toto.txt                       */
/* Creates a problem of size 10000, generates 20 solutions with a sample      */
/* size of 15; sub-paths of size lower than 225 are solved with               */
/* a basic LK procedure. The initial tour is optimized with a fast POPMUSIC   */
/* That reoptimizes sub-paths of size 400 (with 3 scans)                      */
/* Instance coordinates and tours obtained are added to the file "toto.txt"   */
/* Author: E. Taillard, Date: June 24. 2021                                   */

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

#define K 2                               /* Change here for other dimensions */
typedef double COORD[K];                  /* Data associated with a city      */
double epsilon;/* Minimal solution improvement, depends on distance magnitude */

/************************** toroidal distance ********************************
 Uncomment if desired. 
double distance(COORD a, COORD b)
{
  double sum = 0, d;
  for (int i = 0; i < K; i++)
    {
      d = fabs(a[i] - b[i]);
      if (d > 0.5)
        d = 1.0 - d;
      sum += d * d;
    }
  return sqrt(sum);
}
*/
/***************** Euclidean distance between cities a and b ******************/
/* Comment if another distance is desired */
double distance(COORD a, COORD b)
{
  double sum = 0;
  for (int i = 0; i < K; i++)
    sum += (a[i] - b[i]) * (a[i] - b[i]);
  return sqrt(sum);
}
/**/

/*  System-independent generator (L'Ecuyer). Replace (double)rand()/RAND_MAX  */
double rando(void)
{
  static int x10 = 12345, x11 = 67890, x12 = 13579,     /* initial seed value */
             x20 = 24680, x21 = 98765, x22 = 43210;                         
  const int m = 2147483647;
  const int m2 = 2145483479;
  const int a12 = 63308;
  const int q12 = 33921;
  const int r12 = 12979;
  const int a13 = -183326;
  const int q13 = 11714;
  const int r13 = 2883;
  const int a21 = 86098;
  const int q21 = 24919;
  const int r21 = 7417;
  const int a23 = -539608;
  const int q23 = 3976;
  const int r23 = 2071;
  const double invm = 4.656612873077393e-10;
  int h, p12, p13, p21, p23;
  h = x10 / q13;
  p13 = -a13 * (x10 - h * q13) - h * r13;
  h = x11 / q12;
  p12 = a12 * (x11 - h * q12) - h * r12;
  if (p13 < 0)
    p13 = p13 + m;
  if (p12 < 0)
    p12 = p12 + m;
  x10 = x11;
  x11 = x12;
  x12 = p12 - p13;
  if (x12 < 0)
    x12 = x12 + m;
  h = x20 / q23;
  p23 = -a23 * (x20 - h * q23) - h * r23;
  h = x22 / q21;
  p21 = a21 * (x22 - h * q21) - h * r21;
  if (p23 < 0)
    p23 = p23 + m2;
  if (p21 < 0)
    p21 = p21 + m2;
  x20 = x21;
  x21 = x22;
  x22 = p21 - p23;
  if (x22 < 0)
    x22 = x22 + m2;
  if (x12 < x22)
    h = x12 - x22 + m;
  else
    h = x12 - x22;
  if (h == 0)
    return 0.5;
  else
    return (h * invm);
}

/********** Returns a random integer between low and high (included) **********/
int unif(int low, int high)
{
  return low + (int) ((high - low + 1) * rando());
}

/*************************** Exchange two int  ********************************/
void swap(int *a, int *b)
{
  int temp = *a;
  *a = *b;
  *b = temp;
}

/************************ Compute the length of a path ************************/
double length_path(int n, COORD* coord, int path[])
{
  double length = 0.0;
  for (int i = 0; i < n; i++)
    length += distance(coord[path[i]], coord[path[i + 1]]);
  return length;
}

/*** Optimize a path with Lin-Kernighan; cities numbered 0 to n (included) ****/
void path_LK(int n,                                   /* Number of cities - 1 */
             double** D,              /* Distance matrix, must be symmetrical */
             int best_sol[],  /*InOut Solution (city order) provided/returned */
             double *best_cost)              /* InOut Length of the best tour */
{
  int *s;                     /* Reference structure: successors of each city */
  int a, b, c, d, best_c, i, j, si, ssi;                           /* Indices */
  double cut_path, cost_ref_struct;       /* Cost of reference structure kept */
  int **tabu;          /* tabu[i][j] == no_iter => edge i-j cannot be removed */
  int tour_modified,                                   /* s has been modified */
      no_iter;                             /* outermost loop iteration couter */
  int improved = 1;

  s = (int*) malloc((size_t) (n + 1) * sizeof (int));
  tabu = (int**) malloc((size_t) (n + 1) * sizeof (int*));
  for (i = 0; i <= n; i++)
    tabu[i] = (int*) calloc((size_t) (n + 1), sizeof (int));
  for (i = 0; i < n; i++)
    s[best_sol[i]] = best_sol[i + 1];
  no_iter = 0;
  while (improved)
  {
    improved = 0;
    for (a = best_sol[0]; s[s[a]] != best_sol[n]; a = s[a])
    {
      b = s[a];
      cut_path = *best_cost - D[a][b];      /* Length with edge (a-b) removed */
      no_iter++;                                    /* Remove all tabu status */
      do         /* Identify best reference structure with edge (a-b) removed */
      {
        tour_modified = best_c = 0;
        cost_ref_struct = *best_cost;
        for (c = s[b]; c != best_sol[n]; c = s[c])
        {
          d = s[c];
          /*If an improving solution is obtained; change it immediately */
          if (cut_path - D[c][d] + D[c][a] + D[b][d] + epsilon < *best_cost)
          {
            best_c = c;
            cost_ref_struct = cut_path - D[c][d] + D[c][a] + D[b][d];
            break;
          }
          /* edge c-d not added at this iteration */
          if (cut_path + D[b][d] < cost_ref_struct && tabu[c][d] != no_iter)  
          {
            cost_ref_struct = cut_path + D[b][d];
            best_c = c;
          }
        }

        if (cost_ref_struct + epsilon < *best_cost)/* Adm. ref. struct. found */
        {
          tour_modified = 1;
          c = best_c;
          d = s[c];  /* Update reference structure */
          tabu[c][d] = tabu[d][c] = no_iter; /* Prevent remove again edge c-d */
          cut_path += (D[b][d] - D[c][d]);

          /* Reverse path b -> c */
          i = b;
          si = s[i];
          s[b] = d;
          while (i != c)
          {
            ssi = s[si];
            s[si] = i;
            i = si;
            si = ssi;
          }

          b = c;
          if (cut_path + D[a][b] + epsilon < *best_cost)  /* Impr. sol. found */
          {
            *best_cost = cut_path + D[a][b];
            s[a] = b;
            for (i = 0, j = 0; j <= n; i = s[i], j++)  /* Store improved sol. */
              best_sol[j] = i;
            improved = 1;
          }
        }
      }
      while (tour_modified);
      for (i = 0; i < n; i++)
        s[best_sol[i]] = best_sol[i + 1];
    }
  }

  for (i = 0; i <= n; i++)
    free(tabu[i]);
  free(tabu);
  free(s);
  /* path LK */
}

/********* Optimize a sub-path of n+1 cities with arbitrary numbering *********/
double optimize_path(int n, int path[], COORD coord[])
{
  int *order = (int*) malloc((size_t) (n + 1) * sizeof (int));
  for (int i = 0; i <= n; i++)
    order[i] = i;
  double **d = (double**) malloc((size_t) (n + 1) * sizeof (double*));
  for (int i = 0; i <= n; i++)
    d[i] = (double*) malloc ((size_t) (n + 1) * sizeof (double));
  for (int i = 0; i < n; i++)
    for (int j = i + 1; j <= n; j++)
      d[i][j] = d[j][i] = distance(coord[path[i]], coord[path[j]]);
  double length = 0.0;
  for (int i = 0; i < n; i++)
    length += d[i][i + 1];
  path_LK(n, d, order, &length);
  for (int i = 0; i <= n; i++)
    free(d[i]);
  free(d);
  for (int i = 0; i <= n; i++)
    order[i] = path[order[i]];
  for (int i = 0; i <= n; i++)
    path[i] = order[i];
  free(order);
  return length;
}

/*********** Build recursively a path between path[0] and path[n] *************/
double build_path(int n, int path[], COORD coord[], 
                  int nb_clust, int path_length)
{
  if (n < 2)
    return 0.0;
  if (n <= path_length)
    return optimize_path(n, path, coord);

  /* Temporary path */
  int *tmp_path = (int *) malloc((size_t) (n + 1) * sizeof (int));
  for (int i = 0; i <= n; i++)
    tmp_path[i] = path[i];

  /* Set tmp_path[1] be the closest city from path[0] */
  double d, dmin = distance(coord[path[0]], coord[tmp_path[1]]);
  for (int i = 2; i < n; i++)
  {
    d = distance(coord[path[0]], coord[tmp_path[i]]);
    if (dmin > d)
    {
      dmin = d;
      swap(tmp_path + 1, tmp_path + i);
    }
  }
  /* Set tmp_path[nb_clust] be the closest city from path[n] */
  dmin = distance(coord[tmp_path[2]], coord[path[n]]);
  for (int i = 2; i < n; i++)
  {
    d = distance(coord[tmp_path[i]], coord[path[n]]);
    if (dmin > d)
    {
      dmin = d;
      swap(tmp_path + 2, tmp_path + i);
    }
  }
  /* Choose a sample of nb_clust - 2 random cities from path */
  for (int i = 3; i <= nb_clust; i++)
    swap(tmp_path + i, tmp_path + unif (i, n - 1));
  swap(tmp_path + 2, tmp_path + nb_clust);

  int *sample = (int *) malloc((size_t) (nb_clust + 2) * sizeof (int));
  sample[0] = path[0];
  sample[nb_clust + 1] = path[n];
  for (int i = 1; i <= nb_clust; i++)
    sample[i] = tmp_path[i];

  optimize_path(nb_clust + 1, sample, coord);

  /* Assign each city of path to the closest of the sample */
  int *assignment = (int *) malloc((size_t) (n + 1) * sizeof (int));
  for (int i = 1; i < n; i++)
  {
    dmin = 9.99e99;
    for (int j = 1; j <= nb_clust; j++)
    {
      d = distance(coord[path[i]], coord[sample[j]]);
      if (d < dmin)
      {
        dmin = d;
        assignment[i] = j;
      }
    }
  }

  /* build clusters: ith cluster has (start_clust[i+1]-start_clust[i]) cities */
  int *start_clust = (int*) calloc((size_t) (nb_clust + 1), sizeof (int));
  for (int i = 1; i < n; i++)
    start_clust[assignment[i]]++;
  for (int i = 1; i <= nb_clust; i++)
    start_clust[i] += start_clust[i - 1];
  /* Clusters are stored in tmp_path in the order given by sample */
  int *assigned = (int*) calloc((size_t) (nb_clust + 1), sizeof (int));
  for (int i = 1; i < n; i++)
  {
    tmp_path[start_clust[assignment[i] - 1] + assigned[assignment[i]]++]
     = path[i];
    if (path[i] == sample[assignment[i]])
      swap(tmp_path + start_clust[assignment[i] - 1],
            tmp_path + start_clust[assignment[i] - 1]
                     + assigned[assignment[i]] - 1);
  }

  /* Reorder original path */
  for (int i = 1; i < n; i++)
    path[i] = tmp_path[i - 1];

  free(tmp_path);
  free(sample);
  free(assigned);
  free(assignment);

  for (int cluster = 0; cluster < nb_clust-1; cluster++)
  {
    int moved = 1;
    while (moved)
    {
      moved = 0;
      for (int i = start_clust[cluster]+2; i < start_clust[cluster+1]; i++)
        if (distance(coord[path[i]], coord[path[start_clust[cluster+1]]]) 
            < distance(coord[path[start_clust[cluster]+1]],
                       coord[path[start_clust[cluster+1]]])) 
        { swap(path + i, path + start_clust[cluster]+1);
          moved = 1;
        }
      for (int i = start_clust[cluster+1]+1; i < start_clust[cluster+1]-1; i++)
        if (distance(coord[path[i]], coord[path[start_clust[cluster+1]]]) <
            distance(coord[start_clust[cluster+1]],
                     coord[start_clust[cluster+1]+1]) )
        { swap(path + i, path + start_clust[cluster+1]);
          moved = 1;
        }
    }
  }

  int start, end;
  for (int i = 0; i < nb_clust; i++)
  {
    start = start_clust[i];
    end = start_clust[i + 1] + 1;
    /* Recursively optimize sub-path corresponding to each cluster */
    build_path(end - start, path + start, coord, nb_clust, path_length);
  }
  free(start_clust);

  return length_path(n, coord, path);
}


/********** Fast POPMUSIC: Optimize independent subpaths of R cities **********/
double fast_POPMUSIC(int n, int R, int nr_scan, int path[], COORD coord[])
{
  if (R > n || n <= 0) /* Avoid misuse of parameter R */
    R = n;
  int *tmp = (int*) malloc((size_t) (R / 2 + 1) * sizeof (int));
  int sub_paths = n / R + 1 - (n % R == 0); /* Number of sub-path to optimize */

  for (int scan = 0; scan < nr_scan; scan++)
  { /* Rotate path from R/2 cities */
    for (int i = 0; i < R / 2; i++)
      tmp[i] = path[n - R / 2 + i];
    for (int i = n - R / 2 - 1; i >= 0; i--)
      path[i + R / 2] = path[i];
    for (int i = 0; i < R / 2; i++)
      path[i] = tmp[i];
    path[n] = path[0];

    for (int i = 0; i < sub_paths; i++) 
    {
      int start = (int) ((double) n / sub_paths * i + 0.5),
          end = (int) ((double) n / sub_paths * (i + 1) + 0.5);
      optimize_path((end - start), path + start, coord);
    }
  }
  free(tmp);
  return length_path(n, coord, path);
}


/******************************************************************************/
int main(int argc, char *argv[])
{
  if (argc != 8)
  {
    printf("Usage : %s number_cities,  nr_tours, sample_size path_length, "
           "pop_length, nr_scans, result_file\n", argv[0]);
    printf("Standard run example: %s 10000 20 15 225 225 2 fast_popmusic.txt\n",
           argv[0]);
    return EXIT_FAILURE;
  }

  const int N = atoi(argv[1]),
            NR_RES = atoi(argv[2]),
            SAMPLE_SIZE = atoi(argv[3]),
            PATH_LENGTH = atoi(argv[4]),
            R = atoi(argv[5]),
            SCAN = atoi(argv[6]);
  COORD *coord;
  int *tour;
  /* Memory allocation */
  if (!(coord = (COORD*) malloc((size_t) N * sizeof (COORD))) ||
      !(tour = (int*) malloc((size_t) (N + 1) * sizeof (int))))
    return EXIT_FAILURE;


  FILE *res_file = fopen(argv[7], "a");
  if (res_file)
    printf("%s file opened...\n", argv[7]);
  else
    return EXIT_FAILURE;
  fprintf(res_file, "Parameters of the run:\n");
  fprintf(res_file, "%s\n%d cities\n%d tours\n%d %d %d %d\n", argv[0], 
          N, NR_RES, SAMPLE_SIZE, PATH_LENGTH, R, SCAN);
  fprintf(res_file, "Instance data\n");
  for (int i = 0; i < N; i++) /* Problem instance generation, can be replaced */
  { for (int k = 0; k < K; k++)            /* By reading the data from a file */
    {
      coord[i][k] = rando();
      fprintf(res_file, "%.10f ", coord[i][k]);
    }
    fprintf(res_file, "\n");
  }
  for (int no_res = 0; no_res < NR_RES; no_res++)
  {
    /* Choose a random city for being the start and the end of a path */
    for (int i = 0; i < N; i++)
      tour[i] = i;
    swap(tour, tour + unif(0, N - 1));
    tour[N] = tour[0];
    double length = length_path(N, coord, tour);
    epsilon = 1.0e-15 * length;/* Length computed with double, inexact arith. */

    clock_t cpu = clock();
    double length_init = build_path(N, tour, coord, SAMPLE_SIZE, PATH_LENGTH);
    length = fast_POPMUSIC(N, R, SCAN, tour, coord);
    fprintf(res_file, "Tour: %d, init length: %f, opt length: %f, cpu: %f\n",
            no_res, length_init, length, 
            (double) (clock() - cpu) / CLOCKS_PER_SEC);
    for (int i = 0; i < N; i++)
      fprintf(res_file, "%d ", tour[i]);
    fprintf(res_file, "\n");
  }
  fclose(res_file);
  printf("%s successfully updated\n", argv[7]);
  free(coord);
  free(tour);
  return EXIT_SUCCESS;
}