#include "nr3.h"
#include "ran.h"
#include <vector>
#include <string>

const double PI=atan(1.0)*4.0;
const double DEGRAD = PI/180; // go from degrees to radians
const double R_earth = 6370; // radius earth
const int Ncities = 211; // number of cities

Ranq1 ran(1234567); // use the same random generator in all calls with this seed
//
// class city is used to calculate the distance between cities 
// to store the name and the coordinates and to provide an output operator
// so that a City may be written to a stream
//

class City {
    public:
	City() {};
	City(const char* name, double theta, double phi) : _name(name), _phi(phi), _theta(theta) {_phi*=DEGRAD; _theta*=DEGRAD;}; // constructor
	double distance(const City& othercity);
        const char* name() const { return _name.c_str();};
        double theta() const { return _theta ;};
        double phi() const { return _phi;};

    private:
	double _theta;
	double _phi;
	string _name;
};

ostream& operator<<(ostream& s, const City& c); // to be able to print this city

// helper functions to calculate distances based on a vector containing the
// order of the cities

double distances[Ncities][Ncities]; // the table of distances. needs to be filled in the main code

double totaldist(const vector<int>& config);
// calculates distance of the total path in config

double swap2cities(int i, int j, const vector<int>& config);
// calculate change in distance after swapping cities i and j

double swaporder(int i, int j, const vector<int>& config);
// reverses the order of cities between i and j : links city i-1 to city j
// and city j+1 to city i
       
double swappath(int i, int j, int &pathlenght, const vector<int>& config );
// calculate distance after inserting the cities from i to i+pathlenght 
// between city j and j+1 , and check for reversed order as well.
// calculate i j and pathlenght from random start
// if reversing the order yields smaller result: return negative pathlenght
// pathlenght = 0 -> 1 city moved.

void performswap(int i, int j, int pathlenght, int method, vector<int>& config);
// int method = 0 - swap 2 cities
// method = 1 : reverse order between cities
// method = 2 : insert i+...i.pathlenght behind j

void pathchecker(vector<int>& config, int method);
// will do a cleanup by trying all possible swaps up to a certain pathlenght

void getstartconfig(vector<int> &config);
// make a random configuration

#include <list>

int main() {
    char s[28],sdummy[100]; // to use getline in C++
    ifstream infil("./cities.txt"); // make an input file stream infil
    ofstream startconf("./startconf.txt");
    ofstream logfile("./log.txt");
    ofstream results("./output.txt");
    vector<City> Cityvector;

    // read in the infil
    double theta,phi;
    for (int i=0; i<Ncities; i++) { // read full infil, 211 cities
       infil.getline(s,28,','); // put all characters before , in string s
       infil >> theta; // read out theta
       infil.getline(sdummy,10,','); // put all characters before , in string s
       infil >> phi; // read out theta
       infil.getline(sdummy,10,'\n'); // put all characters before eoln in s
       City City1(s,theta,phi); // made a City object
       Cityvector.push_back(City1); // make your vector of City objects 1 longer
    }
    for (int i=0; i<Ncities; i++) {
        distances[i][i]=0;
        for (int j=i+1; j<Ncities; j++) distances[i][j]=distances[j][i]=Cityvector[i].distance(Cityvector[j]);
    }

// calculate some 100000 random path lenghts, to see the spread do some statistics
    vector<int> config;
    config.resize(Ncities);
    for (int Nstart=0; Nstart<100000; Nstart++) {
       getstartconfig(config);
       startconf << totaldist(config) << endl; // for plotting/analysis purposes
      
    }
    double overallshortdist=1e6;
    vector<int> overallshortest;
    vector<int> shortest;
    vector<int> startconfig;
    overallshortest.resize(Ncities);
    shortest.resize(Ncities);
    startconfig.resize(Ncities);
    logfile << "Trial   stategy    startdist   Temp   curdist shortest  ntries nsuccess nswap " << endl;
    for (int Ntrial=0; Ntrial<10; Ntrial++) { // try annealing from ten start trials
        getstartconfig(startconfig);
        for (int strategy=0; strategy<4; strategy++) {
        int ntries=0;
        int nsuccess=0;
        int nswaps=0;
        double ntot=0;
        double Temp=2000;
        for (int i=0; i<Ncities; i++) config[i]=startconfig[i];
        double curdist=totaldist(startconfig);
        double shortdist=curdist;
        int trymax=5000;
        if (strategy>1) trymax*=3; // strategy 2 and 3 also change path lenght
        while (Temp>1) {
          while (ntries<trymax) {
            ntries++;
            ntot++;
            int city1 = (int) (210.9999999999999*ran.doub());
            int city2 = city1;
            while (city2==city1) city2 = (int) (210.9999999999999*ran.doub());
	    double deltaE;
            int method=0;
            int pathlenght=0;
            if (strategy==0 || strategy==3) deltaE=swap2cities(city1,city2,config);
            if (strategy==1 || (strategy==3 && deltaE>0)) {
                    city2=(city1+1+(int) (50*ran.doub()))%Ncities; // randomly swap order of 2....51 cities
                    deltaE=swaporder(city1,city2,config);
                    method=1;
            }
            if (strategy==2 || (strategy==3 && deltaE>0)) {
               pathlenght = (int) (50*ran.doub()*ran.doub())+1; // stretch of cities from 1 to 51, but higher chance for shorter stretches
               city2=city1+pathlenght+(int)((Ncities-3-pathlenght)*ran.doub());
               city2=city2%Ncities; // avoid overflow past Ncities cities
               deltaE=swappath(city1,city2,pathlenght,config);
               if (pathlenght>0) method=2;
               else { pathlenght=-pathlenght; method=3;}
            }
            if (deltaE<0) {
                performswap(city1,city2,pathlenght,method,config); // swap 2 cities
                nsuccess++;
                nswaps++;
                curdist+=deltaE;
                if (shortdist>curdist) {
                    shortdist=curdist;
                    for (int i=0; i<211; i++) shortest[i]=config[i];
                    ntries=0;
                    if (overallshortdist>curdist) {
                        overallshortdist=curdist;
                        for (int i=0; i<211; i++) overallshortest[i]=config[i];
                    }
                }
            }
            else if (deltaE/Temp<20) { // guard against underflow, exp(-20) is already a negligible chance
               if (ran.doub()<exp(-deltaE/Temp)) {
                  performswap(city1,city2,pathlenght,method,config);
                  curdist+=deltaE;
                  nswaps++;
               }
            }
            if (fabs(curdist-totaldist(config))>1e-8) {
               cerr << "distance error " << city1 << " " << city2 << " " << pathlenght << " " << method << " " << curdist << " " << totaldist(config) << endl;
               throw;
            }
          } // ntries < xx
          logfile << Ntrial << " " << strategy << " " << totaldist(startconfig) << " " << Temp << " " << curdist << " " << shortdist << "  " << ntot << " " << nsuccess << " " << nswaps << endl; 
          Temp/=4;
          ntries=0;
          trymax*=3;
        } // while Temp>1
// strategy 2 works the best. There is still lots of room for improvement.
        // clean-up roll: try all swap2cities and all reversepaths for the shortest distance found here.
        logfile << "Cleanup rounds : " << Ntrial << " " << strategy << " " << shortdist << " ";
        pathchecker(shortest,0); // try all swap2cities combos
        logfile << (shortdist=totaldist(shortest)) << " ";
        pathchecker(shortest,1); // try all reversepath combos
        logfile << (shortdist=totaldist(shortest)) << " ";
        pathchecker(shortest,2); // try all reinsert path combos, pathlenghts of 2,3,4,5,6,7,8,9,10
        logfile << " " << (shortdist=totaldist(shortest)) << endl;
        results << "Trial " << Ntrial << " distance " << totaldist(shortest) << endl;
        if (overallshortdist>shortdist) {
	    overallshortdist=shortdist;
            for (int i=0; i<Ncities; i++) overallshortest[i]=shortest[i];
        }
        } // strategy
    }
    logfile << " shortest after 10 trials : " << overallshortdist << endl;
    results << " shortest after 10 trials : " << overallshortdist << endl;
    for (int i=0; i<Ncities; i++)  results << Cityvector[overallshortest[i]];
    return 0;
}

double City::distance(const City& othercity) {
	double x=cos(_theta)*cos(othercity._theta);
	x+=sin(_theta)*sin(othercity._theta)*(sin(_phi)*sin(othercity._phi)+
	   cos(_phi)*cos(othercity._phi));
        if (x>0.99999999999999) return 0; // (cos x = 1 within machine accuracy.
	x=acos(x);
	x*=R_earth;
	return x;
}

ostream& operator<< (ostream& s, const City& c) {
	s << c.name() << " ";
	double angle=c.theta()/DEGRAD;
	if (angle>90) {
	   angle=angle-90; // angle southern angletitude, wrt equator
	   s << angle << " deg. S lat. \t";
	} else {
	   angle=90-angle;
	   s << angle << " deg. N lat. \t";
	}
	angle=c.phi()*180/PI;
	if (angle<0) s << -angle << " deg. E long. "; 
	else s << angle << " deg. W long. ";
	return s << endl;
}

double totaldist(const vector<int>& config) {
    double dist=distances[config[0]][config[Ncities-1]];
    for (int i=0; i<Ncities-1; i++) dist+=distances[config[i]][config[i+1]];
    return dist;
}
   
double swap2cities(int i, int j, const vector<int>& config) {
// calculate distance after swapping cities i and j
// i and j are given by main routine
   double deltadist;
   if (abs(i-j)==1 || abs(i-j)==Ncities-1) {
// neighbouring cities, different distance calculation
// determine which city is the lowest in the order and check for position 0/211
      int jleft=j-1;
      if (jleft<0) jleft+=Ncities; // if i or j = 0, the left number should be the last in the array
      if (i==jleft) { // i is left, j is right
         int left=i-1;
         int right=i+2;
	 if (left<0) left=Ncities-1;
	 if (right>Ncities-1) right-=Ncities;
         deltadist = distances[config[i]][config[right]]+distances[config[j]][config[left]];
         deltadist -= (distances[config[i]][config[left]]+distances[config[j]][config[right]]);
      } else {
         int left=j-1;
         int right=j+2;
	 if (left<0) left=Ncities-1;
	 if (right>Ncities-1) right-=Ncities;
         deltadist = distances[config[j]][config[right]]+distances[config[i]][config[left]];
         deltadist -= (distances[config[j]][config[left]]+distances[config[i]][config[right]]);
      } 
      return deltadist;
   } 
   int firstcity=config[i];
   int secondcity=config[j];
   int left1=i-1;
   int right1=i+1;
   if (left1<0) left1+=Ncities;
   if (right1==Ncities) right1=0;
   int left2=j-1;
   int right2=j+1;
   if (left2<0) left2+=Ncities;
   if (right2==Ncities) right2=0;
   
   left1=config[left1];
   right1=config[right1];
   left2=config[left2];
   right2=config[right2];
   deltadist = distances[firstcity][left2]+distances[firstcity][right2];
   deltadist += distances[secondcity][left1]+distances[secondcity][right1];
   deltadist -= (distances[firstcity][left1]+distances[firstcity][right1]);
   deltadist -= (distances[secondcity][left2]+distances[secondcity][right2]);
   return deltadist;
}
       
double swaporder(int i, int j, const vector<int>& config) {
// calculates path distance if the order of the subpath i..j is reversed:
// run from i-1 to j,j-1,j-2,...i,j+1
// if j< i : assume wraparound - j = j+Ncities in reality
    if (abs(i-j)==1 || abs(i-j)==Ncities-1) 
         return swap2cities(i,j,config);
    // cities are not neighbouring
       int left=i-1;
       if (i==0) left=Ncities-1;
       int right=j+1;
       if (right>Ncities-1) right=0;
       left=config[left];
       right=config[right];
       double dist=distances[config[i]][right]+distances[config[j]][left];
       dist-=distances[config[i]][left];
       dist-=distances[config[j]][right];
       return dist;
}

double swappath(int i, int j, int &pathlenght, const vector<int>& config) {
// calculate distance after inserting the cities from i to i+pathlenght 
// between city j and j+1, and reversing the order
// calculate i j and pathlenght from random start
    
   double deltadist;
   int left=i-1;
   if (left<0) left=Ncities-1;
   int rpath=(i+pathlenght-1)%Ncities; // right city of the path that is being moved
   int right=(i+pathlenght)%Ncities; // will be placed next to left
   int jright=(j+1)%Ncities; 
// 
//

   left=config[left]; // get actual city numbers
   right=config[right];
   jright=config[jright];
   rpath=config[rpath];
// configuration changed from left-i-...-rpath-rightrpath----jleft-j-jright
// to left-rightpath---jleft-j-i...rpath-jright
   deltadist=distances[left][right];
   deltadist+=distances[config[j]][config[i]];
   deltadist+=distances[rpath][jright];
   deltadist-=distances[config[i]][left];
   deltadist-=distances[config[j]][jright];
   deltadist-=distances[rpath][right];
// now check for reversing order of path as well
   double orderdist=distances[left][right];
   orderdist-=distances[config[i]][left];
   orderdist-=distances[config[j]][jright];
   orderdist-=distances[rpath][right];
   orderdist+=distances[config[j]][rpath];
   orderdist+=distances[jright][config[i]];
   if (orderdist<deltadist-1e-8) { // reversing the order of the swapped path leads to a shorter distance; take that option
       pathlenght=-pathlenght; // inform main routine the stretch should be reversed
       deltadist=orderdist;
   }
   return deltadist;
}

void performswap(int i, int j, int pathlenght, int method, vector<int>& config) {
// swap cities i..i+pathlenght , take them out and insert after j
   if (method==0) { // swap i and j
       int dum=config[j];
       config[j]=config[i];
       config[i]=dum;
       return;
    }
    if (method==1) { // reverse visited order from i to j
        if (abs(i-j)==1 || abs(i-j)==Ncities-1) {
           performswap(i,j,0,0,config);
           return;
        }
        if (j<i) j+=Ncities;
        vector<int> tmp;
        tmp.resize(j-i+1);
        for (int k=0; k<j-i+1; k++) tmp[k]=config[(j-k)%Ncities];
        for (int k=i; k<j+1; k++) config[k%Ncities]=tmp[(k-i)%Ncities];
        return;
    }
// move stretch of cities if method == 2, replace order as well if method==3
    vector<int> tmp;
    tmp.resize(Ncities);
    int k=0;
    if (i<j) {
       for (; k<i; k++) tmp[k]=config[k]; // cities up to i stay at same place
       for (; k<j-pathlenght+1; k++) tmp[k]=config[k+pathlenght];// cities from end of path to j move behind city i-1
       for (int l=0; l<pathlenght; l++) {
           if (method==2) tmp[k++]=config[i+l];
           if (method==3) tmp[k++]=config[i+pathlenght-1-l]; // reversed order
       }
       for (; k<Ncities; k++) tmp[k]=config[k];
    } else {
       for (; k<j+1; k++) tmp[k]=config[k]; // cities 0-j
       for (int l=0; l<pathlenght; l++) {
           if (method==2) tmp[(k++)%Ncities]=config[(i+l)%Ncities]; // cities i.. pathlenght 
           if (method==3) tmp[(k++)%Ncities]=config[(i+pathlenght-1-l)%Ncities]; // cities i.. pathlenght 
       }
       for (; k<i+pathlenght; k++) tmp[(k)%Ncities]=config[(k-pathlenght)%Ncities]; // cities i.. pathlenght 
       for (; k<j+Ncities; k++) tmp[(k)%Ncities]=config[(k)%Ncities]; // cities i+pathlenght... j+Ncities-1, part behind the removed cities
    }
    for (int l=0; l<Ncities; l++) config[l]=tmp[l];
}

void getstartconfig(vector<int> &config) {
   list<int> allcities;
   for (int j=0; j<Ncities; j++) allcities.push_back(j); // a list from 0 to 211
   for (int i=0; i<Ncities; i++) {
       double random = ran.doub()*allcities.size(); // draw a random city from the citylist
       list<int>::iterator it=allcities.begin();
       for (int k=0; k< ((int) random); k++) it++; // step the iterator to point towards this random city
       config[i]=*it; // put this city at location i in the config list
       allcities.erase(it); // remove this city from the allcities list
   }
}

void pathchecker(vector<int>& config, int method) {
   if (method==0) {
       for (int i=0; i<Ncities-1; i++) for (int j=i+1; j<Ncities; j++) {
	  double deltaE=swap2cities(i,j,config);
          if (deltaE<0) {
             performswap(i,j,0,0,config);
             j=i+1;
          }
       }
   }
   if (method==1) {
      for (int i=0; i<Ncities-2; i++) for (int j=i+2; j<Ncities; j++) {
         double deltaE=swaporder(i,j,config);
         if (deltaE<0) performswap(i,j,0,1,config);
      }
   }
   if (method==2) {
      for (int pathlenght=2; pathlenght<40; pathlenght++) {
         for (int i=0; i<Ncities; i++) for (int j=i+2*pathlenght;j<i+Ncities-pathlenght-1; j++) {
             int city2=j%Ncities;
             int p=pathlenght;
             double deltaE=swappath(i,city2,p,config);
             if (deltaE<0) {
                if (p<0) performswap(i,city2,pathlenght,3,config);
                else performswap(i,city2,pathlenght,2,config);
             }
          }
      }
   }
}