#include <cmath>
#include <iostream>
#include <iomanip>
#include <vector>
#include <cstdlib>

using namespace std;

template <class T> inline T sqr(const T& x){return x*x;}

// Euclidian Distance between two points in 2D
inline double Distance(const vector<double>& R1, const vector<double>& R2)
{  return sqrt(sqr(R1[0]-R2[0])+sqr(R1[1]-R2[1]));}


// Class for "salesman" problem.
class Salesman{
  int maxAllSteps;// Number of all steps at constant temperature should not exceed maxAllSteps
  int maxAccepted;// Number of accepted steps at constant temperature should not exceed maxAccepted
  int maxTsteps;  // Temperature is lowered not more than maxTsteps
  double fCool;   // Factor to multiply temperature at each cooling step
  double Tstart;  // Starting temperature - has to be high enough
public:
  Salesman(int maxAllSteps_, int maxAccepted_, int maxTsteps_, double fCool_, double Tstart_, int city_size):
    maxAllSteps(maxAllSteps_*city_size), maxAccepted(maxAccepted_*city_size), maxTsteps(maxTsteps_), fCool(fCool_), Tstart(Tstart_) {};
  void anneal(const vector<vector<double> >& R, vector<int>& city);
private:
  void reverse(vector<int>& city, vector<int>& n);// This routine performs a path segment reversal. 
  void trnspt(vector<int>& city, vector<int>& n); // This routine does the actual path transport.
  double revcst(const vector<vector<double> >& R, const vector<int>& city, vector<int>& n);//cost function for a proposed path reversal. 
  double trncst(const vector<vector<double> >& R, const vector<int>& city, vector<int>& n);//cost function for a proposed path segment transport. 
};

// This routine returns the value of the cost function for a proposed path segment transport. 
// city contains the present itinerancy and array R[1...ncity] store vector coordinates.
// The first three elements of array n[] give the starting and ending cities of the path to be transported,
// and the point among the remaining cities after which it is to be inserted.
// On output, de is the cost of the change. The actual transport is not performed by this routine.
double Salesman::trncst(const vector<vector<double> >& R, const vector<int>& city, vector<int>& n)
{
  n[3] = (n[2]+1) % city.size();              // city that comes after n[2]
  n[4] = (n[0]+city.size()-1) % city.size();  // city before n[0]
  n[5] = (n[1]+1) % city.size();              // city after n[1]
  // The segment between cities (n[1],n[5]), (n[0],n[4]), (n[2],n[3]) will be removed and
  // segemnts are added between (n[0],n[2]), (n[1],n[3]), (n[4],n[5])
  double de = -Distance(R[city[n[1]]],R[city[n[5]]]) - Distance(R[city[n[0]]],R[city[n[4]]]) - Distance(R[city[n[2]]],R[city[n[3]]]);
  de +=        Distance(R[city[n[0]]],R[city[n[2]]]) + Distance(R[city[n[1]]],R[city[n[3]]]) + Distance(R[city[n[4]]],R[city[n[5]]]);
  return de;
}

// This function returns the value of the cost function for a proposed path reversal. 
// city contains the present itinerancy and array R[0...ncity-1] store vector coordinates.
// The first two values n[0] and n[1] of array n give the starting and ending cities along the path
// segment which is to be reversed. On output, de is the cost of making the reversal.
// The actual reversal is not performed by this routine.
double Salesman::revcst(const vector<vector<double> >& R, const vector<int>& city, vector<int>& n)
{
  n[2] = (n[0]+city.size()-1) % city.size(); // city before n[2]
  n[3]=  (n[1]+1) % city.size();             // city after n[1]
  double de = -Distance(R[city[n[0]]],R[city[n[2]]]) - Distance(R[city[n[1]]],R[city[n[3]]]);
  de +=        Distance(R[city[n[0]]],R[city[n[3]]]) + Distance(R[city[n[1]]],R[city[n[2]]]);
  return de;
}

// This routine does the actual path transport, once metropolis has approved. city[0..ncity-1]
// is an input array giving the present itinerary. The array n has as its six elements the beginning
// n[0] and end n[1] of the path to be transported, the adjacent cities n[2] and n[3] between
// which the path is to be placed, and the cities n[4] and n[5] that precede and follow the path.
// n[3], n[4], and n[5] are calculated by function trncst. On output, city is modified to
// reflect the movement of the path segment.
void Salesman::trnspt(vector<int>& city, vector<int>& n)
{
  static vector<int> ncity(city.size());

  int m1 = 1 + ((n[1]-n[0]+city.size()) % city.size()); // Find number of cities between n[1] and n[0]
  int m2 = 1 + ((n[4]-n[3]+city.size()) % city.size()); // ... and between n[4] and n[3]
  int m3 = 1 + ((n[2]-n[5]+city.size()) % city.size()); // ... and between n[2] and n[5]
  
  int nn=0;
  for (int j=0;j<m1;j++){
    int jj = (j+n[0]) % city.size(); // Copy the choosen segment between n[0]..n[1]
    ncity[nn++] = city[jj];
  }
  for (int j=0; j<m2; j++){
    int jj = (j+n[3]) % city.size();// Then copy n[3]..n[4]
    ncity[nn++] = city[jj];
  }
  for (int j=0; j<m3; j++){
    int jj = (j+n[5]) % city.size();// Finally, between n[5]..n[2]
    ncity[nn++] = city[jj];
  }
  city = ncity; // copy into original
}

// This routine performs a path segment reversal. city[0..ncity-1] is an input array giving the
// present itinerary. The vector n has as its first four elements the first and last cities n[0],n[1]
// of the path segment to be reversed, and the two cities n[2] and n[3] that immediately
// precede and follow this segment. n[2] and n[3] are found by function revcst. On output,
// iorder[0..ncity-1] contains the segment from n[0] to n[1] in reversed order.
void Salesman::reverse(vector<int>& city, vector<int>& n)
{
  int nn = (1+((n[1]-n[0]+city.size()) % city.size()))/2; // half the lenght of the segment to be reversed
  // the segment is reversed in the following way n[0]<->n[1], n[0]+1<->n[1]-1, n[0]+2<->n[1]-2,...
  // Start at the ends of the segment and swap pairs of cities, moving towards the center.
  for (int j=0; j<nn; j++){
    int k = (n[0]+j) % city.size();
    int l = (n[1]-j+city.size()) % city.size();
    swap(city[k],city[l]);
  }
}

// This algorithm finds the shortest round-trip path between cities whose coordinates are in the
// arrays R[0..ncity-1]. The array city[0..ncity-1] specifies the order in
// which the cities are visited. On input, the elements of city may be set to any permutation
// of the numbers 0 to ncity-1. This routine will return the best alternative path it can find.
void Salesman::anneal(const vector<vector<double> >& R, vector<int>& city)
{
  
  double path=0.0; // total size of the path between all cities
  for (int i=0; i<city.size()-1; i++) path += Distance(R[city[i]],R[city[i+1]]);
  path += Distance(R[city[city.size()-1]],R[city[0]]);
    
  vector<int> n(6);  // temporary
  double T = Tstart; // temperature at start
  
  for (int j=0; j<maxTsteps; j++){ // Loop for cooling down
    int nsucc=0;                   // number of successful moves at constant temperature
    for (int k=0; k<maxAllSteps; k++){ // visiting configurations at constant temperature
      int nn; // number of cities not in the segment n[0]...n[1]
      do {
	n[0]= static_cast<int>(city.size()*drand48());     // Two cities n[0] and n[1] are choosen at random
	n[1]= static_cast<int>((city.size()-1)*drand48()); // and this segement is going to be changed
	if (n[1] >= n[0]) ++n[1];
	nn = 1+((n[0]-n[1]+city.size()-1) % city.size()); // number of cities not on the segment n[0]..n[1]
      } while (nn<3);                                     // Does not make sense to reverse all cities
      bool idec = (drand48()<0.5);
      if (idec) { // idec can be true or false with equal probability
	n[2] = n[1] + 1 + static_cast<int>(abs(nn-2)*drand48()); // greater than n1 but smaller than n0 (n1<n2<n0)
	n[2] = n[2] % city.size();                               // therefore this is a random point out of the segement
	double de=trncst(R,city,n);                              // the cost of segment transport
	bool ans = (de<0) || (drand48() < exp(-de/T));           // METROPOLIS STEP
 	if (ans){                                                // step accepted
 	  ++nsucc;
 	  path += de;
 	  trnspt(city,n);                                        // The order of cities is actually modified and the segment is transported
 	}
      } else {
 	double de=revcst(R,city,n);                             // the const of segment reverse
	bool ans = (de<0) || (drand48() < exp(-de/T));          // METROPOLIS
 	if (ans) {                                              // step accepted
 	  ++nsucc;
 	  path += de;
 	  reverse(city,n);                                      // the segment is actually reversed
 	}
      }
      if (nsucc >= maxAccepted) break;                          // only maxAccepted is necessary at constant temperature
    }
    clog<<left<<"T="<<setw(12)<<T<<" path="<<setw(15)<<left<<path<<"  Successful steps: "<<setw(6)<<nsucc<<right<<endl;
    T *= fCool;                                                 // The system is cooled down
    if (nsucc == 0) return;                                     // If the path does not want to change any more, we can stop
  }
}

int main(int argc, char *argv[], char *env[])
{
  int ncity=100;          // Number of all cities to visit
  int maxTsteps = 100;    // Temperature is lowered not more than maxTsteps
  double Tstart = 0.5;    // Starting temperature - has to be high enough
  double fCool = 0.9;     // Factor to multiply temperature at each cooling step
  int maxAllSteps  = 100; // Number of all steps at constant temperature should not exceed maxAllSteps*ncities
  int maxAccepted = 10;   // Number of accepted steps at constant temperature should not exceed maxAccepted*ncities
  int i=0;
  while (++i<argc){
    std::string str(argv[i]);
    if (str=="-ncity" && i<argc-1)     ncity = atoi(argv[++i]);
    if (str=="-maxTsteps" && i<argc-1) maxTsteps = atoi(argv[++i]);
    if (str=="-Tstart" && i<argc-1) Tstart = atof(argv[++i]);
    if (str=="-fCool" && i<argc-1)  fCool = atof(argv[++i]);
    if (str=="-maxAllSteps" && i<argc-1)  maxAllSteps = atoi(argv[++i]);
    if (str=="-maxAccepted" && i<argc-1)  maxAccepted = atoi(argv[++i]);
    if (str=="-h" || str=="--help"){
      std::clog<<"******************** Salesman problem ****************\n";
      std::clog<<"**                                                  **\n";
      std::clog<<"**      Copyright Kristjan Haule, 22.04.2006        **\n";
      std::clog<<"******************************************************\n";
      std::clog<<"\n";
      std::clog<<"salesman [Options]\n" ;
      std::clog<<"Options:   -ncity       Number of cities to vosit ("<<ncity<<")\n";
      std::clog<<"           -maxTsteps   Temperature is lowered not more than maxTsteps ("<<maxTsteps<<")\n";
      std::clog<<"           -Tstart      Starting temperature - has to be high enough ("<<Tstart<<")\n";
      std::clog<<"           -fCool       Factor to multiply temperature at each cooling step ("<<fCool<<")\n";
      std::clog<<"           -maxAllSteps Number of all steps at constant temperature should not exceed maxAllSteps*ncities ("<<maxAllSteps<<")\n";
      std::clog<<"           -maxAccepted Number of accepted steps at constant temperature should not exceed maxAccepted*ncities ("<<maxAccepted<<")\n";
      std::clog<<"*****************************************************\n";
      return 0;
    }
  }
  
  srand48(time(0));
  vector<int> city(ncity);
  vector<vector<double> > R(city.size());
  for (int i=0; i<R.size(); i++) R[i].resize(2);
  
  for (int i=0;i<R.size();i++){
    R[i][0]= drand48();
    R[i][1]= drand48();
    city[i]=i;
  }

 
  Salesman salesm(maxAllSteps, maxAccepted, maxTsteps, fCool, Tstart, city.size());

  salesm.anneal(R,city);
  
  clog<<"# *** System Frozen *** Final path: is"<<endl;
  for (int i=0;i<city.size();i++){
    int ii=city[i];
    cout<<left<<setw(4)<<ii<<" "<<setw(10)<<R[ii][0]<<" "<<setw(10)<<R[ii][1]<<right<<endl;
  }
  int ii=city[0];
  cout<<left<<setw(4)<<ii<<" "<<setw(10)<<R[ii][0]<<" "<<setw(10)<<R[ii][1]<<right<<endl;
  return 0;
}