/*  >->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->->
RecruitingAntColonySystemTSP.cc, Heiko Stamer <stamer@informatik.uni-leipzig.de>
Recruiting Ant Colony System (RACS) for the Traveling Salesman Problem (TSP)

     [Recruiting Ant Colony System:
             Extending the Ant Colony System by a Group Recruitment Strategy]
  
         [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)
		{
			if (to_city >= 0)
			{
				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 RecruitingAntColonySystem;
class TransportAnt;
class SearchAnt : Ant
{
	friend class RecruitingAntColonySystem;
	friend class TransportAnt;
	
	private:
		RecruitingAntColonySystem *RACS;
	public:	
        SearchAnt(RecruitingAntColonySystem *racs, int start_city): Ant(start_city)
		{
			RACS = racs;
		}
		inline int choose();
		inline Tour *search();
};

class TransportAnt : Ant
{
	friend class RecruitingAntColonySystem;
	friend class SearchAnt;
	
	private:
		RecruitingAntColonySystem *RACS;
		int K;
		double XI;
		
	public:	
        TransportAnt(RecruitingAntColonySystem *racs, int k, double xi);
		inline int choose();
		inline Tour *follow();
};

class RecruitingAntColonySystem
{
	friend class SearchAnt;
	friend class TransportAnt;
	
    private:
		double ALPHA, BETA, RHO, TAU0, Q0;
		doubleMatrix TAU, dTAU;
		static const int M = N;
		static const int R0 = 5;
		static const int R1 = M;
		static const int R = R0 * R1;
		double THETA[R0][N][N], dTHETA[R0][N][N];
		SearchAnt *SEARCH_ANTS[M];
		TransportAnt *TRANSPORT_ANTS[R];
		double BEST_LENGTH;
		Tour BEST_TOUR;
		
	public:
        RecruitingAntColonySystem(double alpha, double beta, double rho, double q0);
		inline double calc_tau0();
		inline void init_tau_by_value(double value);
		inline void init_theta_by_value(double value);
		inline void init_tau_by_matrix(doubleMatrix matrix);
		inline void init_theta_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 theta_transition(int k, int i, int j);
		inline double sum_transition(int i, int allowed[]);
		inline double sum_theta_transition(int k, int i, int allowed[]);
		inline void local_update_rule(int i, int j);
		inline void	clear_global_update();
		inline void	clear_theta_update();
		inline void add_global_update(Tour tour, double length);
		inline void add_theta_update(int k, Tour tour, double length);
		inline void global_update_rule();
		inline void theta_update_rule();
		inline doubleMatrix *get_tau();
		inline void sort_search_ants();
		inline Tour *search(int T);
};
		
inline int SearchAnt::choose()
{
	double q = rand() / (double)RAND_MAX;
	
	if (q <= RACS->Q0)
	{
		double best_value = -1.0;
		int best_choose = -1;
		for (int j = 0; j < N; j++)
		{
			if ((ALLOWED[j] == 1) && 
			(RACS->transition(CURRENT_CITY, j) > best_value))
			{
				best_choose = j;
				best_value = RACS->transition(CURRENT_CITY, j);
			}
		}
		return best_choose;
	}

	double sum = RACS->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 += RACS->transition(CURRENT_CITY, j) / sum;
		if ((p < p_j) && (ALLOWED[j] == 1))
			return j;
	}
	return -1;
}

inline Tour *SearchAnt::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());
		RACS->local_update_rule(LAST_CITY, CURRENT_CITY);
	}
	ALLOWED[START_CITY] = 1;
	RACS->local_update_rule(CURRENT_CITY, START_CITY);
	moveTo(START_CITY);
	return &CURRENT_TOUR;
}

TransportAnt::TransportAnt(RecruitingAntColonySystem *racs, int k, double xi)
	: Ant((int)((double)N * (rand() / (double)RAND_MAX)))
{
	RACS = racs;
	K = k;
	XI = xi;
}
				
inline int TransportAnt::choose()
{
	double q = rand() / (double)RAND_MAX;
	
	if (q < XI)
	{
		double sum = RACS->sum_theta_transition(K, CURRENT_CITY, ALLOWED);
		if (sum > 0.0)
		{
			double p = rand() / (double)RAND_MAX;
			double p_j = 0.0;
			
			for (int j = 0; j < N; j++)
			{
				if (ALLOWED[j] == 1) 
					p_j += RACS->theta_transition(K, CURRENT_CITY, j) / sum;
				if ((p < p_j) && (ALLOWED[j] == 1))
					return j;
			}
			return -1;
		}
		else
		{
			sum = RACS->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 += RACS->transition(CURRENT_CITY, j) / sum;
				if ((p < p_j) && (ALLOWED[j] == 1))
					return j;
			}
			return -1;
		}
	}
	else
	{
		double p0 = rand() / (double)RAND_MAX;
		if (p0 > XI)
		{
			K = (int)((double)RACS->R0 * (rand() / (double)RAND_MAX));
			return -1;
		}
		double sum = RACS->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 += RACS->transition(CURRENT_CITY, j) / sum;
			if ((p < p_j) && (ALLOWED[j] == 1))
				return j;
		}
		return -1;
	}
}
		
inline Tour *TransportAnt::follow()
{
	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;
}

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

inline double RecruitingAntColonySystem::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 RecruitingAntColonySystem::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 RecruitingAntColonySystem::init_theta_by_value(double value)	
{
	for (int k = 0; k < R0; k++)
		for (int i = 0; i < N; i++)
			for (int j = 0; j < N; j++)
				THETA[k][i][j] = value;
}

inline void RecruitingAntColonySystem::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 RecruitingAntColonySystem::init_theta_by_matrix(doubleMatrix matrix)	
{
	for (int k = 0; k < R0; k++)
		for (int i = 0; i < N; i++)
			for (int j = 0; j < N; j++)
				THETA[k][i][j] = matrix[i][j];
}

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

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

inline void	RecruitingAntColonySystem::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;
		SEARCH_ANTS[k] = new SearchAnt(this, c);
	}
}

inline double RecruitingAntColonySystem::ETA(int i, int j)
{
	return ( 1.0 / D[i][j] );
}
		
inline double RecruitingAntColonySystem::transition(int i, int j)	
{
	if (i != j)
		return ( TAU[i][j] * pow( ETA(i, j), BETA ) );
	else
		return(0.0);
}

inline double RecruitingAntColonySystem::theta_transition(int k, int i, int j)	
{
	if (i != j)
		return ( THETA[k][i][j] * pow( ETA(i, j), BETA ) );
	else
		return(0.0);
}

inline double RecruitingAntColonySystem::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 double RecruitingAntColonySystem::sum_theta_transition(int k, int i, int allowed[])
{
	double sum = 0.0;
	for (int j = 0; j < N; j++) 
		sum += ((double)allowed[j] * theta_transition(k, i, j));
	return (sum);
}

inline void RecruitingAntColonySystem::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	RecruitingAntColonySystem::clear_global_update()
{
	for (int i = 0; i < N; i++)
		for (int j = 0; j < N; j++)
			dTAU[i][j] = 0.0;
}

inline void	RecruitingAntColonySystem::clear_theta_update()
{
	for (int k = 0; k < R0; k++)
		for (int i = 0; i < N; i++)
			for (int j = 0; j < N; j++)
				dTHETA[k][i][j] = 0.0;
}				

inline void RecruitingAntColonySystem::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 RecruitingAntColonySystem::add_theta_update(int k, Tour tour, double length)
{
	for (int n = 0; n < N; n++)
	{
		int i = tour[n][0];
		int j = tour[n][1];
		dTHETA[k][i][j] += (1.0 / length);
		// symmetric TSP
		dTHETA[k][j][i] += (1.0 / length);
	}
}

inline void RecruitingAntColonySystem::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 void RecruitingAntColonySystem::theta_update_rule()
{
	for (int k = 0; k < R0; k++)
		for (int i = 0; i < N; i++)
			for (int j = 0; j < N; j++)
				THETA[k][i][j] = (1.0 - ALPHA) * THETA[k][i][j] + ALPHA *
				dTHETA[k][i][j];
}

inline doubleMatrix *RecruitingAntColonySystem::get_tau()
{
	return &TAU;
}

inline void RecruitingAntColonySystem::sort_search_ants()
{
	SearchAnt *tmp;
	bool xchg = true;
	int last, up;
	
	last = M - 1;
	while (xchg)
	{
		xchg = false;
		up = last;
		for (int i = 1; i < up; i++)
			if (calc_length(SEARCH_ANTS[i-1]->CURRENT_TOUR) > 
			calc_length(SEARCH_ANTS[i]->CURRENT_TOUR))
			{
				tmp = SEARCH_ANTS[i];
				SEARCH_ANTS[i] = SEARCH_ANTS[i-1];
				SEARCH_ANTS[i-1] = tmp;
				xchg = true;
				last = i;
			}
	}
}
	
inline Tour *RecruitingAntColonySystem::search(int T)
{
	Tour best_tour, tour;
	double best_length = (double)N * max_dist(), tour_length;
	clear_global_update();
	
	// do T iterations of ant-cycle algorithm
	int t;
	for (t = 0; t < T; t++)
	{	
		// Search Ants
		for (int k = 0; k < M; k++)
		{
			tour = *(SEARCH_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("search [%d / %d]: %lf \n", t, T, tour_length);
				//print_tour(best_tour);
			}
		}
				
		// R0 local best Search Ants recruit R1 Transport Ants from nest
		sort_search_ants();
		init_theta_by_value(0.0);
		clear_theta_update();
		for (int k = 0; k < R0; k++)
		{
			add_theta_update(k, SEARCH_ANTS[k]->CURRENT_TOUR, 
				calc_length(SEARCH_ANTS[k]->CURRENT_TOUR));
			for (int l = 0; l < R1; l++)
				TRANSPORT_ANTS[(k * R1) + l] = 
				new TransportAnt(this, k, 1.00);
		}
		theta_update_rule();
		clear_theta_update();
		
		// Transport Ants
		for (int k = 0; k < R; k++)
		{
			tour = *(TRANSPORT_ANTS[k]->follow());
			tour_length = calc_length(tour);
			if ((tour_length + .0001) < best_length)
			{
				best_tour = tour;
				best_length = tour_length;
				clear_global_update();
				add_global_update(tour, tour_length);
				//printf("transport [%d / %d]: %lf \n", t, T, tour_length);
				//print_tour(best_tour);	
			}
		}
		// Transport Ants go back to nest
		for (int k = 0; k < R; k++)
			delete TRANSPORT_ANTS[k];
			
		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();
	
	// Recruiting Ant Colony System		
	RecruitingAntColonySystem *racs = 
		new RecruitingAntColonySystem(0.1, 2.0,	0.1, 0.9);

	double tau0 = racs->calc_tau0();
	racs->init_tau_by_value(tau0);
	racs->init_random();
	racs->search(1000);
	
	return 0;
}