/ant_colony.c

#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <float.h>
#include <string.h>
#include "ant_colony.h"

float eucli2d(graph_n *node1, graph_n *node2){
	return ( sqrtf( powf( (node1->x - node2->x), 2 ) + powf( (node1->y - node2->y), 2 )) );
}

long random_at_most(long max) {
  unsigned long
    /* max <= RAND_MAX < ULONG_MAX, so this is okay. */
    num_bins = (unsigned long) max + 1,
    num_rand = (unsigned long) RAND_MAX + 1,
    bin_size = num_rand / num_bins,
    defect   = num_rand % num_bins;

  long x;
  do {
   x = random();
  }
  /* This is carefully written not to overflow */
  while (num_rand - defect <= (unsigned long)x);

  /* Truncated division is intentional*/
  return x/bin_size;
}

graph_n *get_graph_node_with_number(graph *graph, int node_number){
	int i;
	if(graph->graph_nodes[node_number-1].node_number == node_number)
		return &graph->graph_nodes[node_number-1];
	for(i = 0; i < graph->size; i++){
		if(graph->graph_nodes[i].node_number == node_number)
			return &graph->graph_nodes[i];
	}
	return NULL;

}

tsp_ants *init_tsp(char *path_to_data, int n_ants, 	float alpha, float beta, float q0){
	tsp_ants *ret;
	int n_edges;
	int i,j,k;
	int pascal_height;

	ret = (tsp_ants *)malloc(sizeof(tsp_ants));
	ret->n_ants = n_ants;
	ret->alpha = alpha;
	ret->beta = beta;
	ret->q0 = q0;
	ret->graph = read_tsp_graph(path_to_data);
	if(ret->graph == NULL)
		return NULL;
	n_edges = ((ret->graph->size - 1)*ret->graph->size)/2; /* Number of edges in a full conected graph */
	pascal_height = ret->graph->size - 1;
	ret->pheromone_trails = (pheromone *)malloc(sizeof(pheromone)*(n_edges));
	ret->ants = (ant *)malloc(sizeof(ant)*n_ants);
	/* for(i = 0; i < n_ants; i++) 
		ret->ants[i].path = (graph_n *)malloc(sizeof(graph_n)*n_edges); */
	
	k = 0;
	for(j = 1; j<= pascal_height; j++){
		for (i = j+1; i <= pascal_height+1; i++){
			ret->pheromone_trails[k].node_1 = get_graph_node_with_number(ret->graph,j);
			ret->pheromone_trails[k].node_2 = get_graph_node_with_number(ret->graph,i);
			ret->pheromone_trails[k].value = 1;
			k++;
		}
	}
	return ret;
	
}

graph *read_tsp_graph(char *filename){
	FILE * fp;	
	char *line;
	int size;
	size_t len;
	ssize_t read;
	graph *ret;
	int i;

	i = 0;
	line = NULL;
	len = 0;
	ret = (graph *)malloc(sizeof(graph));
	fp = fopen(filename, "r");
	if (!fp)
		return NULL;
  	while ((read = getline(&line, &len, fp)) != -1) {    	
    	if(strstr(line,"DIMENSION")){
    		sscanf(line,"DIMENSION: %d",&size);
    		ret->size = size;
    		ret->graph_nodes = (graph_n *)malloc(sizeof(graph_n)*size);
    	}
    	if(strstr(line,"NODE_COORD_SECTION")){
    		break;
    	}
  	}
  	while ((read = getline(&line, &len, fp)) != -1) {    	
    	if(!strstr(line,"EOF")){    		
    		sscanf(line,"%d %f %f",&ret->graph_nodes[i].node_number,&ret->graph_nodes[i].x,&ret->graph_nodes[i].y);
    		i++;
    	}    	
   }   
   fclose(fp);
   if (line)
       free(line);
   return ret;
}

void init_randomizer(){
	srand(time(NULL));
}

int try_to_hit(float probability){
	float r;		
	r = (float) rand()/RAND_MAX;
	if(probability > r)
		return 1;
	else
		return 0;		
}

int is_on_interval(float down, float up, float value){
	if(value >= down && value <= up)
		return 1;
	else
		return 0;
}

int roullete(float *probability_dist,int size){
	float **range;
	float hit;
	int i;

	range = (float **)malloc(sizeof(float *)*size);
	for(i = 0; i < size; i++)
		range[i] = (float *)malloc(sizeof(float)*2);
	
	range[0][0] = 0;
	range[0][1] = probability_dist[0];
	for(i = 1; i < size; i++){
		range[i][0] = range[i-1][1] + FLT_MIN;
		range[i][1] = range[i][0] + probability_dist[i];
	}
	hit = (float)rand()/RAND_MAX;
	for(i = 0; i < size; i++){
		if (is_on_interval(range[i][0],range[i][1],hit))
			return i+1;
	}

	return -1;
}

int isOnList(int *list, int value, int list_size){
	int i;
	for(i = 0; i< list_size; i++){
		if(list[i] == value)
			return i;
	}
	return -1;
}

void initialize_ants_randomly(tsp_ants *tsp){
	int i;
	int random_node;
	int *ants_nodes;

	ants_nodes = (int *)malloc(sizeof(int)*tsp->n_ants);
	for(i = 0; i < tsp->n_ants; i++){
		do{
			random_node = random_at_most(tsp->graph->size-1) + 1;
		}while(isOnList(ants_nodes, random_node, tsp->n_ants)!=-1);
		ants_nodes[i] = random_node;
		tsp->ants[i].present_node = get_graph_node_with_number(tsp->graph, random_node);
		tsp->ants[i].initial_node = get_graph_node_with_number(tsp->graph, random_node);
		tsp->ants[i].path = initialize_list(((tsp->graph->size - 1)*tsp->graph->size)/2);
		push(tsp->ants[i].path, tsp->ants[i].initial_node);
		tsp->ants[i].tour_value = -1;
	}
	free(ants_nodes);
}

void local_trail_update(){}

int shortest_ant_tour_index(tsp_ants *tsp){
	int i,ret;
	float shortest;

	shortest = tsp->ants[0].tour_value;
	ret = 0;
	for (i = 1; i< tsp->n_ants; i++){
		if(tsp->ants[i].tour_value < shortest){
			shortest = tsp->ants[i].tour_value;
			ret = i;
		}
	}
	return ret;	
}

int all_ants_finished(tsp_ants *tsp){
	int i;
	for (i = 0; i< tsp->n_ants; i++){
		if(tsp->ants[i].initial_node != tsp->ants[i].present_node && tsp->ants[i].tour_value != -1)
			return 0;
	}
	return 1;
}

int move_with_arg_max(tsp_ants *tsp, ant *ant){
	int i;
	int u;
	float max;
	float temp;
	int ret;

	max = -1.0;
	ret = -1.0;	
	if(ant->path->last_node_pos == tsp->graph->size - 1){ /*Must return to begin*/		
		printf("ENTRO AQUI 1\n");
		return ant->initial_node->node_number;
	}
	for(i = 1; i <= tsp->graph->size; i++){		
		if (i == ant->present_node->node_number || find(ant->path,i)){			
			continue;		
		}
		u = find_pheromone_with_nodes(tsp,i,ant->present_node->node_number);
		temp = tsp->pheromone_trails[u].value*powf((1/eucli2d(tsp->pheromone_trails[u].node_1,tsp->pheromone_trails[u].node_2)),tsp->beta); 
		// printf("Distance between %d and %d : %f\n",tsp->pheromone_trails[u].node_1->node_number,
		// 	tsp->pheromone_trails[u].node_2->node_number,
		// 	temp);		
		if(max < temp){
			max = temp;
			ret = i;
		}
	}
	return ret;
}

int move_with_probability(tsp_ants *tsp, ant *ant){
	float *probabilities, sum;	
	int i;
	int s;	
	int ret;	

	if(ant->path->last_node_pos == tsp->graph->size - 1){ /*Must return to begin*/		
		printf("ENTRO AQUI 2\n");
		return ant->initial_node->node_number;
	}
	probabilities = (float *)malloc(sizeof(float)*tsp->graph->size);
	for(i = 1; i <= tsp->graph->size; i++){
		if (i == ant->present_node->node_number || find(ant->path,i)){
			probabilities[i-1] = 0.0;
			continue;		
		}
		s = find_pheromone_with_nodes(tsp,i,ant->present_node->node_number);		
		// printf("Distance between %d and %d : %f\n",tsp->pheromone_trails[s].node_1->node_number,
		// 	tsp->pheromone_trails[s].node_2->node_number,
		// 	eucli2d(tsp->pheromone_trails[s].node_1,tsp->pheromone_trails[s].node_2));
		probabilities[i-1] = tsp->pheromone_trails[s].value*powf((1/eucli2d(tsp->pheromone_trails[s].node_1,tsp->pheromone_trails[s].node_2)),tsp->beta);
	}
	sum = 0.0;
	for(i = 0; i < tsp->graph->size; i++){
		// printf("Probability %d: %f\n",i,probabilities[i]);
		sum += probabilities[i];
	}
	for(i = 0; i < tsp->graph->size; i++){
		probabilities[i] = probabilities[i]/sum;	
		// printf("Probability of moving to city %d: %f\n",i+1,probabilities[i]);
	}	

	ret = roullete(probabilities,tsp->graph->size);
	free(probabilities);
	return ret;
}

void move_to_new_city(tsp_ants *tsp, ant *ant){
	int city_to_move;
	graph_n *node;

	if(ant->initial_node == ant->present_node && ant->tour_value != -1){
		printf("ENTRO AQUI 3\n");
		return;
	}
	if(try_to_hit(tsp->q0)){
		city_to_move = move_with_arg_max(tsp, ant);
	}else{
		city_to_move = move_with_probability(tsp, ant);
	}
	node = get_graph_node_with_number(tsp->graph,city_to_move);
	ant->tour_value += eucli2d(ant->present_node,node);
	ant->present_node = node;
	push(ant->path,node);
	// printf("Moving to city %d\n",city_to_move);
}

int find_pheromone_with_nodes(tsp_ants *tsp, int node_1, int node_2){
	int i;
	int n_edges;

	n_edges = ((tsp->graph->size - 1)*tsp->graph->size)/2;

	for(i = 0; i < n_edges; i++){
		if( (tsp->pheromone_trails[i].node_1->node_number == node_1 || tsp->pheromone_trails[i].node_2->node_number == node_1) && 
			(tsp->pheromone_trails[i].node_1->node_number == node_2 || tsp->pheromone_trails[i].node_2->node_number == node_2) )
			return i;
	}
	return -1;
}

int global_trail_update(tsp_ants *tsp){
	float pheromoneDelta;
	int i;	
	int shortest_index;
	graph_n *node_1, *node_2;

	shortest_index = shortest_ant_tour_index(tsp);
	pheromoneDelta = 1/tsp->ants[shortest_index].tour_value;	
	// printf("PATH: ");
	pop(tsp->ants[shortest_index].path,&node_2);
	do{
		node_1 = node_2;		
		pop(tsp->ants[shortest_index].path,&node_2);		
		i = find_pheromone_with_nodes(tsp, node_1->node_number, node_2->node_number);
		// printf("%d-%d\t",node_1->node_number, node_2->node_number);
		if(i == -1){
			printf("Error: cannot find node\n");
			return -1;
		}
		tsp->pheromone_trails[i].value = (1 - tsp->alpha)*tsp->pheromone_trails[i].value + tsp->alpha*pheromoneDelta;	
	}while(tsp->ants[shortest_index].path->last_node_pos != -1);	
	return shortest_index;
}

float generate_solution(tsp_ants *tsp){
	int i;
	int shortest_ant_tour_index;		
	float ret;
	initialize_ants_randomly(tsp);
	do{
		for(i = 0; i< tsp->n_ants; i++){
			move_to_new_city(tsp, &tsp->ants[i]);
			local_trail_update();
		}
	}while(!all_ants_finished(tsp));	
	shortest_ant_tour_index = global_trail_update(tsp);	
	ret = tsp->ants[shortest_ant_tour_index].tour_value;
	return ret;
}

void release_graph(graph *graph){
	free(graph->graph_nodes);
	free(graph);
}

void release_tsp_ants(tsp_ants *tsp){
	int i;
	for(i = 0; i < tsp->n_ants; i++){
		free(tsp->ants[i].path->list_nodes);
		free(tsp->ants[i].path);
	}
	free(tsp->ants);
	free(tsp->pheromone_trails);
	release_graph(tsp->graph);
	free(tsp);
}

list *initialize_list(int size){	
	list *ret;
	ret = (list *)malloc(sizeof(list));
	ret->size = size;
	ret->list_nodes = (graph_n **)malloc(sizeof(graph_n *)*size);	
	ret->last_node_pos = -1;	
	return ret;
}


int find(list *list, int node_number){
	int i;
	for (i=0;i<=list->last_node_pos;i++){
		if(list->list_nodes[i]->node_number == node_number)
			return 1;
	}
	return 0;
}

int pop(list *list, graph_n **node){	
	if(list->last_node_pos > -1){
		*node = list->list_nodes[list->last_node_pos];
		list->list_nodes[list->last_node_pos] = NULL;		
		list->last_node_pos--;
		return 1;
	}else{
		return 0;
	}	
}

int push(list *list, graph_n *node){	
	if(list->last_node_pos < list->size - 1){
		list->last_node_pos++;
		list->list_nodes[list->last_node_pos] = node;			
		return 1;
	}else{
		return 0;
	}	
}