/*  
Ant Colony System for solving a 14-city TSP problem 
    
By Andres Perez-Uribe
LSA2 - STI - EPFL
January, 2002

Email: Andres.Perez-Uribe AT a3.epfl.ch

Based on:
Dorigo and Gambardella, Ant Colony System: A Cooperative Learning 
Approach to the Traveling Salesman Problem, 
IEEE, Trans Evol. Comp. 1, 1997, 53-66

WARNING: This is not a 100% implementation of Dorigo et al. algorithm,
I have only used the basic idea. In particular, I have used a simpler
exploration-exploitation strategy, and the value of the initial
pheromone level is based on what I've called the 'naive-distance'. 
Please, refer to Dorigo et al. for the original algorithm. I think mine is
simpler, but may not work as well as theirs (especially because of
random exploration instead of their biased exploration.
 
There is no guarantee that the code will do what you
expect or that it is error free. It is simply meant
to provide a useful way to experiment with a ACS-like
algorithm.


Modification:June 6, 2002.
global update only for the best path so far
thanks to Luc-Olivier de Charrière

Last Update: June 6, 2003.
change of srand48(), lrand48(), and drand48() by srand() and rand() 
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define BIG 99999999999
#define NumCities 14

#define NumAnts 4

/* initial pheromone level (NumCities*NearestNeighTourLength)^-1 */
/* #define Tao0 : defined as (NumCities*SumNaiveDist)^-1  

/* learning rate */
#define RHO 0.8

/* exploitation rate */
#define q0 0.5

/* number of iterations */
#define MAXITER 10

float Tao[NumCities][NumCities];
int Tour[NumAnts][NumCities+1];
int BestTour[NumCities+1];
int StartCity[NumAnts];
int VisitedCities[NumAnts][NumCities];
int AntInitInCity[NumCities];
float dist[NumCities][NumCities];
float SumLen[NumAnts];
float MinLenTour;
float Tao0; 

int finished;
int randcity;

int i,j,k,t;
int iter;

/* predeclarations */
float Burma14TSPproblem(float dist[NumCities][NumCities]);
float FindNewBestSolution(float MinLenTour, int Tour[NumAnts][NumCities+1],
float SumLen[NumAnts], int BestTour[NumCities+1]);
void PrintBestPath(int BestTour[NumCities+1]);

int main()
{

/* Initialization phase */

/* Init random number generator */
srand(12345);

/* Init. Min Tour length value */
MinLenTour = BIG;

/* Init. TSP and initial pheromone value */

Tao0 = 1.0/(Burma14TSPproblem(dist)*NumCities); 

printf("Tao0 = %f Naivedist=%f\n",Tao0,Burma14TSPproblem(dist));

/* Init. pheromone level of each link */
for(i=0;i<NumCities;i++)
 for(j=0;j<NumCities;j++)
   Tao[i][j] = Tao0; 

/**************** main loop ******************/

for(iter=0;iter<MAXITER;iter++) {

/* place each ant in a randomly chosen city */

/* no cities visited yet */
  for(k=0;k<NumAnts;k++) {
   for(j=0;j<NumCities;j++) {
    VisitedCities[k][j] = 0; 
    Tour[k][j] = 0;
   }
   Tour[k][j+1] = 0;
  }

/* place only one ant per city */
for(j=0;j<NumCities;j++)
 AntInitInCity[j] = 0; 
 
for(k=0;k<NumAnts;k++) {
do {
 randcity = rand()%NumCities;
} while (AntInitInCity[randcity]);

 AntInitInCity[randcity] = 1;
 StartCity[k] = randcity;
 VisitedCities[k][randcity] = 1;
 Tour[k][0] = randcity;
}

/* The ants build their tours */ 

t=0;
finished = 0;
do {
 t = t+1;

 /* Execute one step for each ant k */
 for(k=0;k<NumAnts;k++) {

   /* choose next city */
   if(NonVisitedCities(k,VisitedCities)) {
      Tour[k][t] = ChooseNextCity(k,Tour[k][t-1],VisitedCities,Tao,dist);
      VisitedCities[k][Tour[k][t]] = 1;
   }
   else { 
      Tour[k][t] = StartCity[k];  /* ants go back to the initial city */
      finished = 1;
   }   
 }
 
   /* update pheromone level using a local rule */
   for(i=0;i<NumCities;i++)
     for(j=0;j<NumCities;j++)
       Tao[i][j] = (1-RHO)*Tao[i][j] + RHO*Tao0;

} while(!finished);

 /* Compute the length of the Tour found by each ant */
 for(k=0;k<NumAnts;k++) {
  SumLen[k] = 0;
   for(t=0;t<NumCities+1; t++)
     SumLen[k] += dist[Tour[k][t]][Tour[k][t+1]];
 }  

 MinLenTour = FindNewBestSolution(MinLenTour,Tour,SumLen,BestTour);

 printf("\n %f",MinLenTour);

   /* update pheromone level of BestTour using a global rule */
   for(i=0;i<NumCities;i++)
	Tao[BestTour[i]][BestTour[i+1]] = (1-RHO)*Tao[BestTour[i]][BestTour[i+1]] + RHO*(1.0/MinLenTour);

}
 PrintBestPath(BestTour);

}

int NonVisitedCities(int k,int VisitedCities[NumAnts][NumCities])
{
 int i;

 i = -1;
 do {
  i++; 
 } while(VisitedCities[k][i] && i<NumCities); 
 
 if(i==NumCities)
  return(0);
 else return(1);
}

int ChooseNextCity(int k, int antpos,int
VisitedCities[NumAnts][NumCities],float Tao[NumCities][NumCities],float
dist[NumCities][NumCities])
{
  int nonvisited;
  int i;
  int NextCity;
  int RndCity;
  float prob_explore; /* probability of exploration */
  
  float WeightedPheromone[NumCities][NumCities];
  int ArgMaxWeightedPhero;
/*    float  SumWeightedPheromone; */

  nonvisited = 0;
/*    SumWeightedPheromone = 0.0; */
  ArgMaxWeightedPhero = 0;
  for(i=0;i<NumCities;i++)
    if(!VisitedCities[k][i]) {
      nonvisited++;
      WeightedPheromone[antpos][i] =
Tao[antpos][i]/(dist[antpos][i]*dist[antpos][i]);
/*        SumWeightedPheromone += WeightedPheromone[antpos][i]; */
      if (WeightedPheromone[antpos][i] >
WeightedPheromone[antpos][ArgMaxWeightedPhero])
	ArgMaxWeightedPhero = i;
    }
    else WeightedPheromone[antpos][i] = 0;

  /* exploration-exploitation */
  prob_explore = rand()/RAND_MAX;

  if(prob_explore < q0)
    NextCity = ArgMaxWeightedPhero;
  else { 
    if(nonvisited==1) RndCity = 1;
     else RndCity = 1+rand()%(nonvisited-1);
    i=-1;
    j=0;
    do {
     i++;
     if(!VisitedCities[k][i]) 
       j++; 
     } while((i<NumCities) && (j<RndCity));
    NextCity = i; 
  }
  
  return(NextCity);
}

float Burma14TSPproblem(float dist[NumCities][NumCities])
{
  float Xpos[14] = {16.47, 16.47, 20.09, 22.39, 25.23, 22.00, 20.47, 
                    17.20, 16.30, 14.05, 16.53, 21.52, 19.41, 20.09};

  float Ypos[14] = {96.10, 94.44, 92.54, 93.37, 97.24, 96.05, 97.02, 
                    96.29, 97.38, 98.12, 97.38, 95.59, 97.13, 94.55};
  
  int i,j;
  float SumNaiveDist;

  for(i=0;i<NumCities;i++)
    for(j=0;j<NumCities;j++) {
      dist[i][j] = (float)sqrt((double)((Xpos[j]-Xpos[i])*(Xpos[j]-Xpos[i]) +
(Ypos[j]-Ypos[i])*(Ypos[j]-Ypos[i])));
    }

  SumNaiveDist = 0.0;
  for(i=0;i<NumCities;i++)
      SumNaiveDist += dist[i][(i+1)%NumCities];

  return(SumNaiveDist);
}

float FindNewBestSolution(float MinLenTour, int Tour[NumAnts][NumCities+1],
float SumLen[NumAnts], int BestTour[NumCities+1])
{
  float NewMinLenTour;
  
  NewMinLenTour = MinLenTour;
  for(k=0;k<NumAnts;k++)
    if(SumLen[k] < NewMinLenTour) {
      for(i=0;i<NumCities+1;i++)
       BestTour[i] = Tour[k][i];
     NewMinLenTour = SumLen[k];
    }

  return(NewMinLenTour);
}

void PrintBestPath(int BestTour[NumCities+1])
{
  int i;
 
  printf("\n Best Path....\n");
  for(i=0;i<NumCities+1;i++)
    printf("%d - ",BestTour[i]); 
  printf("\n");
}