// vim: set ts=2 sw=2 et tw=80:
#include "opt.h"

using namespace std;

typedef unsigned int uint;
typedef tuple<uint, uint, uint> triple;

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;
}

ostream& operator<< (ostream& out, triple& a) {
  out << "(" << get<0>(a) << "," << get<1>(a) << "," << get<2>(a) << ")";
  return out;
}

double reverse_segment_if_better(vector<uint>& tour, const double (&dist_matrix) [1577][1577],
    uint i, uint j, uint k) {

  uint A = tour[i-1];
  uint B = tour[i];
  uint C = tour[j-1];
  uint D = tour[j];
  uint E = tour[k-1];
  uint F = tour[k % (tour.size()-1)];

  double d0 = (dist_matrix[A][B] + dist_matrix[C][D] + dist_matrix[E][F]);
  double d1 = (dist_matrix[A][C] + dist_matrix[B][D] + dist_matrix[E][F]);
  double d2 = (dist_matrix[A][B] + dist_matrix[C][E] + dist_matrix[D][F]);
  double d3 = (dist_matrix[A][D] + dist_matrix[E][B] + dist_matrix[C][F]);
  double d4 = (dist_matrix[F][B] + dist_matrix[C][D] + dist_matrix[E][A]);

  if (d0 > d1) {
    reverse(tour.begin() + i, tour.begin() + j);
    return -d0 + d1;
  }
  else if (d0 > d2) {
    reverse(tour.begin() + j, tour.begin() + k);
    return -d0 + d2;
  }
  else if (d0 > d4) {
    reverse(tour.begin() + i, tour.begin() + k);
    return -d0 + d4;
  }
  else if (d0 > d3) {
    vector<uint> tmp;
    for (uint z = j; z < k; z++) {
      tmp.push_back(tour[z]);
    }
    for (uint z = i; z < j; z++) {
      tmp.push_back(tour[z]);
    }


    for (uint f = i; f < k; f++) {
      tour[f] = tmp[f - i];
    }
    return -d0 + d3;
  }
  return 0;
}

void three_opt(vector<uint>& tour, const double (&dist_matrix) [1577][1577], double& sh_dist) {
  double delta = 0;
  uint n = tour.size() - 1;
  for (uint i = 0; i < 1; i++) {
    for (uint i = 1; i < n; i++)
      for (uint j = i+2; j < n; j++)
        for (uint k = j+2; k < n; k++) {
          double d = reverse_segment_if_better(tour, dist_matrix, i, j, k);
          if (d > 0) cerr << d << " " << i << "," << j << "," << k << endl;
          delta += d;
        }
    if (delta >= 0)
      break;
  }
  sh_dist += delta;
}