// vim: set ts=2 sw=2 et tw=80:

// compile with
// c++ -O2 -lpthread --std=c++11 -o c_prob/aco aco.cc opt.cc

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <pthread.h>
#include <vector>
#include <set>
#include <iostream>
#include <algorithm>
#include <unistd.h>
#include "opt.h"

#define SINGLE_CORE 1

using namespace std;
typedef unsigned int uint;

const uint MAX_NODES = 1577;

// alpha >= 0
double alpha;
// beta >= 1
double beta;
double pheromone_evaporation_coeff;
double pheromone_constant;
uint n_iterations;

uint n_ants;

uint n_nodes;

double dist_matrix[MAX_NODES][MAX_NODES];
double pheromone_map[MAX_NODES][MAX_NODES];
double ant_updated_pheromone_map[MAX_NODES][MAX_NODES];

double sh_dist;
vector<uint> sh_route;
bool first_pass;
uint start;


struct ant {
  uint seed;
  uint idx;
  uint iter;
  pthread_t t_handle;
  uint location;
  double distance_travelled;
  set<uint> possible_locations;
  vector<uint> route;
  bool tour_complete = false;

  uint select_random(const set<uint> &s) {
    auto n = rand_r(&seed) % s.size(); // not _really_ random
    auto it = begin(s);
    advance(it, n); // 'advance' the iterator n times
    return *it;
  }

  void init() {
    this->seed = this->idx * 1000 + this->iter;

    this->distance_travelled = 0.0;

    this->route.clear();

    this->possible_locations.clear();
    for (size_t i = 0; i < n_nodes; i++) {
      this->possible_locations.insert(i);
    }

    this->location = start;

    this->update_route(start);

    this->tour_complete = false;
  };

  void update_route(uint new_loc) {
    this->route.push_back(new_loc);
    this->possible_locations.erase(new_loc);
  };

  uint pick_path() {
    if (first_pass) {
      return select_random(this->possible_locations);
    } else {
      double toss = (double) rand_r(&seed) / (double) RAND_MAX;

      double attractiveness[n_nodes];
      double sum_total = 0.0;
      vector<uint> idxs;

      for (uint loc : this->possible_locations) {
        idxs.push_back(loc);
        double pheromone_amount = pheromone_map[this->location][loc];
        double distance = dist_matrix[this->location][loc];
        if (distance == 0) { distance = 0.000000001; };

        attractiveness[loc] = pow(pheromone_amount, alpha) *
          pow(1 / distance, beta);

        sum_total += attractiveness[loc];
        if (isnan(sum_total)) { cerr << "nanalert " << attractiveness[loc] <<
          " " << pheromone_amount << " " << distance << endl; }
      }

      // it is possible to have small values for pheromone amount / distance,
      // such that with rounding errors this is equal to zero
      if (sum_total == 0.0) {
        // increment all zero's, such that they are the smallest non-zero
        // values supported by the system
        // source: http://stackoverflow.com/a/10426033/5343977
        for (uint loc : idxs) {
          attractiveness[loc] = nextafter(attractiveness[loc],
              std::numeric_limits<double>::infinity());
        }
        sum_total = nextafter(sum_total, std::numeric_limits<double>::infinity());
      }

      double cumulative = 0.0;
      for (uint loc : idxs) {
        double weight = (attractiveness[loc] / sum_total);

        if (toss <= (weight + cumulative)) {
          return loc;
        }
        cumulative += weight;
      }

      return idxs[idxs.size() - 1];
    }
  };

  void traverse(uint next) {
    this->update_route(next);
    this->distance_travelled += dist_matrix[this->location][next];
    this->location = next;
  }

  void run() {
    while (!this->possible_locations.empty()) {
      uint next = this->pick_path();
      this->traverse(next);
    }
    this->distance_travelled += dist_matrix[this->route[this->route.size() - 1]]
      [this->route[0]];

    this->tour_complete = true;
  }
};

void *ant_thread (void *ant_ptr) {
  ant& ant = *((struct ant *) ant_ptr);
  ant.run();
  return NULL;
}

ant* ants;

void init_ants(size_t it) {
  for (size_t i = 0; i < n_ants; i++) {
    ants[i].idx = i;
    ants[i].iter = it;
    ants[i].init();
  }
}

pthread_mutex_t mut;

void init_aco() {
  pthread_mutex_init(&mut, NULL);
  start = 0;
  first_pass = true;
  memset(pheromone_map, 0, sizeof(pheromone_map));
  memset(ant_updated_pheromone_map, 0, sizeof(ant_updated_pheromone_map));
  init_ants(0);
  sh_dist = -1.0;
}

void populate_ant_updated_pheromone_map(ant& ant) {
  for (size_t i = 0; i < ant.route.size(); i++) {
    size_t j = (i + 1) % n_nodes;
    double current_ph = ant_updated_pheromone_map[ant.route[i]][ant.route[j]];
    double new_ph = pheromone_constant / ant.distance_travelled;

    ant_updated_pheromone_map[ant.route[i]][ant.route[j]] =
      ant_updated_pheromone_map[ant.route[j]][ant.route[i]] =
      current_ph + new_ph;
  }
}

void update_pheromone_map() {
  for (size_t i = 0; i < n_nodes; i++) {
    for (size_t j = 0; j < n_nodes; j++) {
      pheromone_map[i][j] = (1 - pheromone_evaporation_coeff) *
        pheromone_map[i][j] + ant_updated_pheromone_map[i][j];
    }
  }
}


double mainloop(size_t i) {
#if SINGLE_CORE
    for (uint j = 0; j < n_ants; j++) {
      ant_thread(&(ants[j]));
    }
#else
    for (uint j = 0; j < n_ants; j++) {
      pthread_create(&(ants[j].t_handle), NULL, ant_thread, &(ants[j]));
    }
    for (uint j = 0; j < n_ants; j++) {
      pthread_join(ants[j].t_handle, NULL);
    }
#endif

    unsigned long sum = 0L;
    for (uint j = 0; j < n_ants; j++) {
      populate_ant_updated_pheromone_map(ants[j]);
      sum += ants[j].distance_travelled;

      if (ants[j].iter > 200 && n_nodes == 76) { // eil76 after 200th iteration
        three_opt(ants[j].route, dist_matrix, ants[j].distance_travelled);
      }

      if (sh_dist < 0 || ants[j].distance_travelled < sh_dist) {
        sh_dist = ants[j].distance_travelled;
        sh_route = ants[j].route;

        cerr << "new short distance is " << sh_dist << " ant " << j << endl;
      }
    }

    update_pheromone_map();

    first_pass = false;

    init_ants(i+1);

    memset(ant_updated_pheromone_map, 0, sizeof(ant_updated_pheromone_map));

    return (double) sum / (double) n_ants;
}

uint two_five_opt() {
  uint swaps = 0;
  for (int i = 0; i < n_nodes - 2; i++) {
    for (int j = i+2; j < n_nodes; j++) {
      size_t ip = i == 0 ? (n_nodes - 1) : (i - 1);
      size_t x1 = sh_route[ip];
      size_t x2 = sh_route[i];
      size_t x3 = sh_route[i+1];

      size_t y3 = sh_route[j-2];
      size_t y1 = sh_route[j-1];
      size_t y2 = sh_route[j];

      double var_a = sh_dist - dist_matrix[x2][x1] - dist_matrix[y2][y1]
        + dist_matrix[x1][y1] + dist_matrix[y2][x2];

      double var_b = sh_dist - dist_matrix[x2][x1] - dist_matrix[y2][y1]
        - dist_matrix[x2][x3] + dist_matrix[x2][y2]
        + dist_matrix[y1][x2] + dist_matrix[x1][x3];

      double var_c = sh_dist - dist_matrix[x2][x1] - dist_matrix[y2][y1]
        - dist_matrix[y1][y3] + dist_matrix[x1][y1]
        + dist_matrix[y1][x2] + dist_matrix[y3][y2];

      if (var_a < sh_dist && var_a < var_b && var_a < var_c) {
        sh_dist = var_a;
        reverse(sh_route.begin() + i, sh_route.begin() + j);
        swaps++;
      } else if (var_b < sh_dist && var_b < var_a && var_b < var_c) {
        for (int k = i; k < j - 1; k++) {
          sh_route[k] = sh_route[k + 1];
        }
        sh_route[j - 1] = x2;

        sh_dist = var_b;
        swaps++;
      } else if (var_c < sh_dist && var_c < var_a && var_c < var_b) {
        for (int k = j - 2; k >= i; k--) {
          sh_route[k + 1] = sh_route[k];
        }
        sh_route[i] = y1;
        sh_dist = var_c;
        swaps++;
      }
    }
  }
  return swaps;
}

#define buffersize 100000

int main(int argc, char** argv) {
  unsigned started = time(NULL);
  int optruns;

  if (argc < 9) {
    cerr << argv[0] << " [n_nodes] [n_iter] [n_ants] [alpha] [beta] [evap] [weight] [optruns]" << endl;
    return 1;
  }

  n_nodes = atoi(argv[1]);
  n_iterations = atoi(argv[2]);
  n_ants = atoi(argv[3]);
  sscanf(argv[4], "%lf", &alpha);
  sscanf(argv[5], "%lf", &beta);
  sscanf(argv[6], "%lf", &pheromone_evaporation_coeff);
  sscanf(argv[7], "%lf", &pheromone_constant);
  sscanf(argv[8], "%d", &optruns);
  ant ants_arr[n_ants];
  ants = ants_arr;

  cerr << n_nodes << endl;
  char * pch;
  int row = 0, column = 0;

  char buffer[buffersize];
  for (size_t i = 0; i < n_nodes; i++) {
    fgets(buffer, buffersize, stdin);
    pch = strtok(buffer, " ");
    column = 0;

    while (pch != NULL) {
      sscanf(pch, "%lf", &(dist_matrix[row][column]));
      pch = strtok(NULL, " ");
      column++;
    }
    row++;
  }

  cerr << "reading done" << endl;

  init_aco();

  unsigned limit = 175;
  unsigned last_time = started;
  unsigned last_delta = 0;
  for (size_t i = 0; i < n_iterations; i++) {
    double d = mainloop(i);
    unsigned now = time(NULL);
    cerr << "iter: " << i << " - dist: " << sh_dist << " - avg: "
         << d << " - elapsed: " << now - started << endl;
    if ((now - started) + last_delta > limit) {
        cerr << "out of time" << endl;
        break;
    }
    last_delta = now - last_time;
    last_time = now;
  }

  cerr << "pre-optimization length: " << sh_dist << endl;

  cerr << sh_route << endl;

  for (uint j = 0; j < optruns; j++) {
    two_five_opt();
    three_opt(sh_route, dist_matrix, sh_dist);
    cerr << "optimized length " << j << " : " << sh_dist << endl;
  }

  cout << sh_route << endl;
}