/*  >->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->
     AntColonySystemTSP.cc, Heiko Stamer <stamer@informatik.uni-leipzig.de>

       Ant Colony System (ACS) for the Traveling Salesman Problem (TSP)
	   
         [The Ant System: Optimization by a colony of cooperating agents]
                   by M. Dorigo, V. Maniezzo, A. Colorni
    IEEE Transactions on Systems, Man and Cybernetics - Part B, Vol.26-1 1996
	
	     [Ant Colony System: A Cooperative Learning Approach to the TSP]
                    by M. Dorigo and L. M. Gambardella
       IEEE Transactions on Evolutionary Computation, Vol. 1, No. 1, 1997
	
            http://stinfwww.informatik.uni-leipzig.de/~mai97ixb
    >->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->

  Copyright (C) 2001 - until_the_end_of_the_ants  <Heiko Stamer>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.                 */
	
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <unistd.h>
#include <time.h>
#include <assert.h>

#define N 70

double C[N][2] = { {64 , 96} , {80 , 39} , {69 , 23} , {72 , 42} , {48 , 67} , {58 ,
43} , {81 , 34} , {79 , 17} , {30 , 23} , {42 , 67} , {7 , 76} , {29 , 51} , {78
, 92} , {64 , 8} , {95 , 57} , {57 , 91} , {40 , 35} , {68 , 40} , {92 , 34} ,
{62 , 1} , {28 , 43} , {76 , 73} , {67 , 88} , {93 , 54} , {6 , 8} , {87 , 18} ,
{30 , 9} , {77 , 13} , {78 , 94} , {55 , 3} , {82 , 88} , {73 , 28} , {20 , 55}
, {27 , 43} , {95 , 86} , {67 , 99} , {48 , 83} , {75 , 81} , {8 , 19} , {20 ,
18} , {54 , 38} , {63 , 36} , {44 , 33} , {52 , 18} , {12 , 13} , {25 , 5} , {58
, 85} , {5 , 67} , {90 , 9} , {41 , 76} , {25 , 76} , {37 , 64} , {56 , 63} ,
{10 , 55} , {98 , 7} , {16 , 74} , {89 , 60} , {48 , 82} , {81 , 76} , {29 , 60}
, {17 , 22} , {5 , 45} , {79 , 70} , {9 , 100} , {17 , 82} , {74 , 67} , {10 ,
68} , {48 , 19} , {83 , 86} , {84 , 94} };

typedef int Tour[N][2];
typedef double doubleMatrix[N][N];

doubleMatrix D;

double dist(int i, int j)
{
	return sqrt(pow((C[i][0]-C[j][0]), 2.0) + pow((C[i][1]-C[j][1]), 2.0));
}

void calc_dist()
{
	for (int i = 0; i < N; i++)
		for (int j = 0; j < N; j++)
			D[i][j] = dist(i, j);
}

double max_dist()
{
	double max_dist = 0.0;
	for (int i = 0; i < N; i++)
		for (int j = 0; j < N; j++)
			if (dist(i, j) > max_dist)
				max_dist = dist(i, j);
	return max_dist;
}

double calc_length(Tour tour)
{
	double l = 0.0;
	for (int n = 0; n < N; n++)
	{
		int i = tour[n][0];
		int j = tour[n][1];
		l += D[i][j];
	}
	return (l);
}

void print_tour(Tour tour)
{
	for (int n = 0; n < N; n++)
		printf("( %d , %d ) ", tour[n][0], tour[n][1]);
	printf("\n");
}

int sum_sequence(int array[], int count)
{
	int sum = 0;
	for (int i = 0; i < count; i++)
		sum += array[i];
	return (sum);
}

/******************************************************************************/

class Ant
{
	protected:
		int START_CITY, CURRENT_CITY;
		int ALLOWED[N];
		Tour CURRENT_TOUR;
		int CURRENT_TOUR_INDEX;
	public:	
        inline Ant(int start_city) 
		{
			START_CITY = start_city;
		}
		inline void moveTo(int to_city)
		{
			ALLOWED[to_city] = 0;
			CURRENT_TOUR[CURRENT_TOUR_INDEX][0] = CURRENT_CITY;
			CURRENT_TOUR[CURRENT_TOUR_INDEX][1] = to_city;
			CURRENT_TOUR_INDEX++;
			CURRENT_CITY = to_city;
		}
};

class NNAnt : Ant
{
	public:	
        inline NNAnt(int start_city): Ant(start_city) { };
		inline int choose()
		{
			double best_length = (double)N * max_dist();
			int best_choose = -1;
	
			for (int j = 0; j < N; j++)
			{
				if ((ALLOWED[j] == 1) && (D[CURRENT_CITY][j] < best_length))
				{
					best_choose = j;
					best_length = D[CURRENT_CITY][j];
				}
			}
			return best_choose;
		}
		inline Tour *search()
		{
			CURRENT_CITY = START_CITY;
			CURRENT_TOUR_INDEX = 0;
			for (int i = 0; i < N; i++)
				ALLOWED[i] = 1;
			ALLOWED[CURRENT_CITY] = 0;
			while (sum_sequence(ALLOWED, N) > 0)
				moveTo(choose());
			ALLOWED[START_CITY] = 1;
			moveTo(START_CITY);
			return &CURRENT_TOUR;
		}
};

class AntColonySystem;
class ACSAnt : Ant
{
	private:
		AntColonySystem *ACS;		
	public:	
        ACSAnt(AntColonySystem *acs, int start_city): Ant(start_city)
		{
			ACS = acs;
		}
		inline int choose();
		inline Tour *search();
};

class AntColonySystem
{
    private:
		double ALPHA, BETA, RHO, TAU0;
		doubleMatrix TAU, dTAU;
		static const int M = 420;
		ACSAnt *ANTS[M];
		
	public:
		double Q0;
        AntColonySystem(double alpha, double beta, double rho, double q0);
		inline double calc_tau0();
		inline void init_tau_by_value(double value);
		inline void init_tau_by_matrix(doubleMatrix matrix);
		inline void	init_uniform();
		inline void	init_random();
		inline void	init_randomMOAPC();
		inline double ETA(int i, int j);
		inline double transition(int i, int j);
		inline double sum_transition(int i, int allowed[]);
		inline void local_update_rule(int i, int j);
		inline void	clear_global_update();
		inline void add_global_update(Tour tour, double length);
		inline void global_update_rule();
		inline doubleMatrix *get_tau();
		inline Tour *search(int T);
};
		
inline int ACSAnt::choose()
{
	double q = rand() / (double)RAND_MAX;
	
	if (q <= ACS->Q0)
	{
		double best_value = -1.0;
		int best_choose = -1;
		for (int j = 0; j < N; j++)
		{
			if ((ALLOWED[j] == 1) && 
			(ACS->transition(CURRENT_CITY, j) > best_value))
			{
				best_choose = j;
				best_value = ACS->transition(CURRENT_CITY, j);
			}
		}
		return best_choose;
	}

	double sum = ACS->sum_transition(CURRENT_CITY, ALLOWED);
	double p = rand() / (double)RAND_MAX;
	double p_j = 0.0;
			
	for (int j = 0; j < N; j++)
	{
		if (ALLOWED[j] == 1) p_j += ACS->transition(CURRENT_CITY, j) / sum;
		if ((p < p_j) && (ALLOWED[j] == 1))
			return j;
	}
	return -1;
}

inline Tour *ACSAnt::search()
{
	CURRENT_CITY = START_CITY;
	CURRENT_TOUR_INDEX = 0;
	for (int i = 0; i < N; i++)
		ALLOWED[i] = 1;
	ALLOWED[CURRENT_CITY] = 0;
	while (sum_sequence(ALLOWED, N) > 0)
	{
		int LAST_CITY = CURRENT_CITY;
		moveTo(choose());
		ACS->local_update_rule(LAST_CITY, CURRENT_CITY);
	}
	ALLOWED[START_CITY] = 1;
	ACS->local_update_rule(CURRENT_CITY, START_CITY);
	moveTo(START_CITY);
	return &CURRENT_TOUR;
}

/******************************************************************************/
		
AntColonySystem::AntColonySystem(double alpha, double beta, double rho, double q0)
{
	ALPHA = alpha;
	BETA = beta;
	RHO = rho;
	Q0 = q0;
}

inline double AntColonySystem::calc_tau0()
{
	double best_length = (double)N * max_dist();
	
	for (int n = 0; n < N; n++)
	{
		NNAnt *nnANT = new NNAnt(n);
		Tour tour;
		tour = *(nnANT->search());
		double tour_length = calc_length(tour);
		if (tour_length < best_length)
			best_length = tour_length;
		delete nnANT;
	}
	return 1.0 / ((double)N * best_length);
}

inline void AntColonySystem::init_tau_by_value(double value)	
{
	TAU0 = value;
	for (int i = 0; i < N; i++)
		for (int j = 0; j < N; j++)
			TAU[i][j] = TAU0;
}

inline void AntColonySystem::init_tau_by_matrix(doubleMatrix matrix)	
{
	for (int i = 0; i < N; i++)
		for (int j = 0; j < N; j++)
			TAU[i][j] = matrix[i][j];
}

inline void	AntColonySystem::init_uniform()
{
	// uniformly distributed
	for (int k = 0; k < M; k++)
		ANTS[k] = new ACSAnt(this, (k % N));
}

inline void	AntColonySystem::init_random()
{	
	// randomly distributed
	for (int k = 0; k < M; k++)
		ANTS[k] = new ACSAnt(this, (int)((double)N * (rand() / (double)RAND_MAX)));
}

inline void	AntColonySystem::init_randomMOAPC()
{	
	// randomly distributed with MOAPC (most one ant per city)
	bool MOAPCarray[N];
	assert(M <= N);
	
	for (int n = 0; n < N; n++)
		MOAPCarray[n] = false;
		
	for (int k = 0; k < M; k++)
	{
		int c;
		do
		{
			c = (int)((double)N * (rand() / (double)RAND_MAX));
		}
		while (MOAPCarray[c]);
		
		MOAPCarray[c] = true;
		ANTS[k] = new ACSAnt(this, c);
	}
}

inline double AntColonySystem::ETA(int i, int j)
{
	return ( 1.0 / D[i][j] );
}
		
inline double AntColonySystem::transition(int i, int j)	
{
	if (i != j)
		return ( TAU[i][j] * pow( ETA(i, j), BETA ) );
	else
		return(0.0);
}
	
inline double AntColonySystem::sum_transition(int i, int allowed[])
{
	double sum = 0.0;
	for (int j = 0; j < N; j++) 
		sum += ((double)allowed[j] * transition(i, j));
	return (sum);
}

inline void AntColonySystem::local_update_rule(int i, int j)
{
	TAU[i][j] = (1.0 - RHO) * TAU[i][j] + RHO * TAU0;
	// symmetric TSP
	TAU[j][i] = TAU[i][j];
}
	
inline void	AntColonySystem::clear_global_update()
{
	for (int i = 0; i < N; i++)
		for (int j = 0; j < N; j++)
			dTAU[i][j] = 0.0;
}
				

inline void AntColonySystem::add_global_update(Tour tour, double length)
{
	for (int n = 0; n < N; n++)
	{
		int i = tour[n][0];
		int j = tour[n][1];
		dTAU[i][j] += (1.0 / length);
		// symmetric TSP
		dTAU[j][i] += (1.0 / length);
	}
}

inline void AntColonySystem::global_update_rule()
{
	for (int i = 0; i < N; i++)
		for (int j = 0; j < N; j++)
			TAU[i][j] = (1.0 - ALPHA) * TAU[i][j] + ALPHA * dTAU[i][j];
}

inline doubleMatrix *AntColonySystem::get_tau()
{
	return &TAU;
}
	
inline Tour *AntColonySystem::search(int T)
{
	Tour best_tour, tour;
	double best_length = (double)N * max_dist(), tour_length;
	clear_global_update();
	
	// do T iterations of ACS algorithm
	int t;
	for (t = 0; t < T; t++)
	{	
		for (int k = 0; k < M; k++)
		{
			tour = *(ANTS[k]->search());
			tour_length = calc_length(tour);
			if (tour_length < best_length)
			{
				best_tour = tour;
				best_length = tour_length;
				clear_global_update();
				add_global_update(tour, tour_length);
				//printf("[%d / %d]: %lf \n", t, T, tour_length);
			}
		}
		global_update_rule();
	}
		
	//printf("[%d/%d] best tour (length = %f):\n", t, T, best_length);
	//print_tour(best_tour);
	//printf("[%d/%d] iterations done\n", t, T);
	printf("%f\n", best_length);
	return (&best_tour);
}

int main(int argc, char* argv[])
{
	// PRNG initalisieren
	time_t timer;
	time(&timer);
	pid_t pid = getpid() + getppid();	
	unsigned long seed = (timer * pid);
	if (seed == 0) 
	{
	    time(&timer);
	    seed = 7 * timer * pid;
	    if (seed == 0) seed = pid; else seed = seed % 56000;
	} else seed = seed % 56000;
	srand((unsigned int)seed);
	
	// EUC2D
	calc_dist();
	
	// Ant Colony System		
	AntColonySystem *acs = new AntColonySystem(0.1, 2.0, 0.1, 0.9);
	double tau0 = acs->calc_tau0();
	acs->init_tau_by_value(tau0);
	acs->init_uniform();
	acs->search(1000);
	
	return(0);
}