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

// compile with
// g++ -lpthread --std=c++11 -o c_prob/aco aco.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>

using namespace std;
typedef unsigned int uint;

ostream& operator<< (ostream& out, vector<uint> a) {
  out << "[";
  for (int i = 0; i < a.size() - 1; i++) {
    out << a[i] << ",";
  }
  out << a[a.size() - 1] << "]";
  return out;
}

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;

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

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

  void init() {
    this->other = 0;

    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 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);
        //cerr << "ant " << idx << " attr[loc]=" << attractiveness[loc] << endl;
        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 toss = (double) rand() / (double) RAND_MAX;

      // cerr << "ant " << idx << " sum_total is " << sum_total << " toss is " << toss << endl;

      double cumulative = 0.0;
      for (uint loc : idxs) {
        double weight = (attractiveness[loc] / sum_total);
        //cerr << "ant " << idx << " w: " << weight + cumulative << endl;
        if (toss <= (weight + cumulative)) {
          return loc;
        }
        cumulative += weight;
      }

      cerr << "cum " << cumulative << " n " << idxs.size() << " toss " << toss << endl;
      sleep(1);

      other++;
      // cerr << "ant " << idx << " change statement" << endl;
      return idxs[idxs.size() - 1];
    }
  };

  void traverse(uint next) {
    this->update_route(next);
    this->distance_travelled += dist_matrix[this->location][next];
    this->location = next;
    //cerr << "traversing " << next << " dist: " << this->distance_travelled << endl;
  }

  void run() {
    // cerr << "started ant - # can pick: " << this->possible_locations.size() << endl;
    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]];

    // cerr << "stopped ant" << endl;
    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() {
  for (size_t i = 0; i < n_ants; i++) {
    ants[i].init();
    ants[i].idx = i;
  }
}

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();
  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];
    }
  }
}


void mainloop() {
  for (size_t i = 0; i < n_iterations; i++) {
    srand(i);
    //cerr << "starting ants" << endl;
    for (uint j = 0; j < n_ants; j++) {
      pthread_create(&(ants[j].t_handle), NULL, ant_thread, &(ants[j]));
    }
    // cerr << "joining ants" << endl;
    for (uint j = 0; j < n_ants; j++) {
      pthread_join(ants[j].t_handle, NULL);
    }
    //cerr << "summing ants" << endl;
    uint os = 0;
    for (uint j = 0; j < n_ants; j++) {
      os += ants[j].other;
      populate_ant_updated_pheromone_map(ants[j]);

      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();

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

    /*double real = 0;
      for (uint k = 0; k < n_nodes; k++) {
      real += dist_matrix[sh_route[k]][sh_route[(k + 1) % n_nodes]];
      }*/

    cerr << "iteration " << i << ": dist " << sh_dist << endl;// << " os " << os << " rdist " << real << endl;
    /*
       uint nnz = 0;
       double sum = 0, min = 1.0/0.0, max = -1.0/0.0;
       for (int i = 0; i < n_nodes; i++) {
       for (int j = 0; j < n_nodes - 1; j++) {
       if (pheromone_map[i][j] != 0.0) nnz++;
       sum += pheromone_map[i][j];
       if (min > pheromone_map[i][j]) min = pheromone_map[i][j];
       if (max < pheromone_map[i][j]) max = pheromone_map[i][j];
       }
       }
       cerr << "phmap: avg " << sum / (double) (n_nodes * n_nodes) << " min " << min << " max " << max << " nnz " << nnz << endl;*/
  }
}

uint two_opt() {
  uint swaps = 0;
  for (int i = 0; i < n_nodes - 1; i++) {
    for (int j = i+2; j < n_nodes; j++) {
      size_t ip = i == 0 ? (n_nodes - 1) : (i - 1);
      size_t ei = sh_route[i], ej = sh_route[j], eii = sh_route[ip],
        ejj = sh_route[j];
      double new_dist = sh_dist - dist_matrix[eii][ei] - dist_matrix[ejj][ej]
        + dist_matrix[ei][ej] + dist_matrix[ejj][eii];
      if (new_dist < sh_dist) {
        //cerr << "2opt " << i << " " << j << ": " << new_dist << endl;
        sh_dist = new_dist;
        reverse(sh_route.begin() + i, sh_route.begin() + j);
        swaps++;
      }
    }
  }
  return swaps;
}

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) {
        //cerr << "25opt(a) " << i << " " << j << ": " << var_a << endl;
        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) {
        //cerr << "25opt(b) " << i << " " << j << ": " << var_b << endl;
        //cerr << vector<uint>(sh_route.begin() + i - 1, sh_route.begin() + j + 1) << endl;
        for (int k = i; k < j - 1; k++) {
          sh_route[k] = sh_route[k + 1];
        }
        sh_route[j - 1] = x2;
        //cerr << vector<uint>(sh_route.begin() + i - 1, sh_route.begin() + j + 1) << endl;

        sh_dist = var_b;
        swaps++;
      } else if (var_c < sh_dist && var_c < var_a && var_c < var_b) {
        //cerr << "25opt(c) " << i << " " << j << ": " << var_c << endl;
        //cerr << vector<uint>(sh_route.begin() + i - 1, sh_route.begin() + j + 1) << endl;
        for (int k = j - 2; k >= i; k--) {
          sh_route[k + 1] = sh_route[k];
        }
        sh_route[i] = y1;
        //cerr << vector<uint>(sh_route.begin() + i - 1, sh_route.begin() + j + 1) << endl;

        sh_dist = var_c;
        swaps++;
      }
    }
  }
  return swaps;
}

#define buffersize 100000

int main(int argc, char** argv) {
  if (argc < 8) {
    cerr << argv[0] << " [n_nodes] [n_iter] [n_ants] [alpha] [beta] [evap] [weight]" << 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);
  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;



  /*uint nnz = 0;
    for (int i = 0; i < n_nodes; i++) {
    for (int j = 0; j < n_nodes - 1; j++) {
    if (dist_matrix[i][j] != 0.0) nnz++;
    }
    }
    cerr << "dist: nnz " << nnz << endl;*/

  init_aco();
  // scan and parse
  mainloop();

  //for (uint k = 0; k < n_nodes; k++) {
  //  cerr << "p: " << sh_route[k] << ": " << dist_matrix[sh_route[k]][sh_route[(k + 1) % n_nodes]] << endl;
  //}

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

  uint i = 0;
  uint j = 0;
  do {
    //for (i = 0; i < 50; i++) {
      //cerr << "2opt round " << i << endl;
      //if(two_opt() == 0) break;
    //}
    for (j = 0; j < 50; j++) {
      //cerr << "25opt round " << j << endl;
      if(two_five_opt() == 0) break;
    }
    cerr << "optimizing length: " << sh_dist << endl;
  } while (i > 0 || j > 0);

  cerr << "optimized length: " << sh_dist << endl;
  cout << sh_route << endl;
}