//Ryan Honig
//Period: 5
//4th Quarter Version of my Traveling Salesman Program (Includes my heuristic)

//Summary of what the program does and how its working
/*  This program first reads in the amount of cities and the coordinates of the cities from a text file.
It then generates a pool of 50 initial legal paths for the traveling salesman problem.
This can either be done randomly or with a heuristic
If the heuristic is used, then it will first pick a random point out of all of the cities.
It then finds which two other points are the closest to that point
and begins two paths starting at the first point, and going to each of the other two points.
Then, for each of those two points, it finds the next two closest points,
and creates two more new paths, thus doubling the number of paths being made.
It continues doing this until there are enough points to fill the pool,
at which point it will just continue by picking the next closest point, until a full traverse of the points is acheived.   
Then it will run the genetic crossover method for some specific number of times.
This crossover method will randomly pick two parent paths from the pool.
It then creates a set of links that contain all the links between cities found in both of the parents.
It then goes through these links, alternating choosing one link from each parent.
If the path gets broken and it cannot finish creating a legal path,
then it uses a greedy algorithm to finish fixing the path.
There is also a mutation method that occurs with a one in fifty chance.
This mutation method will pick two random points on a graph and then it will reverse the path in between those two points.

I am testing the code with traveling salesman data that can be found at this site:
http://www.iwr.uni-heidelberg.de/groups/comopt/software/TSPLIB95/tsp/
The site also has the best known solutions for these data sets so I can test to see how
close my program is coming to the best known solution.
*/


#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <math.h>

#define MAX 1000
#define POOL 50

int order[MAX];
int torder[MAX];
int linkOrd[POOL][MAX][2];
int child[MAX][2];
int originalOrder[MAX];
int mute = 0;

double power(double base, double exp)// a power function I made up when I couldn't get math.h to work at first, now that it's working I don't really need it.
{
	int k = 1;
	double ret = 1;
	while(k<=exp)
	{
		ret*=base;
		k++;
	}
	return ret;
}

int randomness(int perc)// a random function that I made
{
	struct timeval tv;
	gettimeofday(&tv, 0);
	long int seed = tv.tv_usec;
	srand(seed);
	return random()%perc;
}

double distForm(int x1, int x2, int y1, int y2)//This method finds the distance between two points
{
	double difx = (double)x1 - (double)x2;
	double dify = (double)y1 - (double)y2;
	double powx = power(difx, 2.0);
	double powy = power(dify, 2.0);
        return sqrt(powx+powy);
}


double dist(int ord[], int cityX[], int cityY[], int length)//This method finds the total distance for a given path between points
{
	int k = 0;
	double totalDist = 0;
	double curDist = 0;
	while(k < length)
	{
		int x1;
		int x2;
		int y1;
		int y2;
		if(k != length-1)
		{
			x1 = cityX[ord[k]];
			x2 = cityX[ord[k+1]];
			y1 = cityY[ord[k]];
			y2 = cityY[ord[k+1]];
		}
		else
		{
			x1 = cityX[ord[k]];
			x2 = cityX[ord[0]];
			y1 = cityY[ord[k]];
			y2 = cityY[ord[0]];
		}
		curDist = distForm(x1, x2, y1, y2);
		//printf("%f\n", curDist);
		totalDist += curDist;
		k++;
	}
	return totalDist;
}

void printOrder(int length, int norder[])// This method prints out the order of the cities
{
	int k = 0;
	while(k < length)
	{
		printf("%d, ", norder[k]);
		k++;
	}
	printf("\n");
}

void reverse(int length)// this method reverses a path between two random points, it is used to do the mutations
{
	int ptL = randomness(length);
	int ptH = randomness(length);
	int temp = 0;
	if(ptL > ptH)
	{
		temp = ptL;
		ptL = ptH;
		ptH = temp;
	}
	temp = 0;

	int count = 0;
	while(ptL+count < ptH-count)
	{
		temp = order[ptL+count];
		order[ptL+count] = order[ptH-count];
		order[ptH-count] = temp;
		temp = 0;
		count++;
	}
}

void runGenetic(int cityX[], int cityY[], int length)// this just creates the initial pool of random paths
{
 	int k = 0;
        while(k < 150)
        {
                reverse(length);
                k++;
        }
}

void makeLinks(int node ,int norder[], int length) // This method takes a path between cities and converts it into a set of links between each of the cities in a path
{
	int k = 0;
	while(k < length)
	{
		int place = norder[k];
		if(k == 0)
			linkOrd[node][place][0] = norder[length-1];
		else
			linkOrd[node][place][0] = norder[k-1];
		if(k == length - 1)
			linkOrd[node][place][1] = norder[0];
		else
			linkOrd[node][place][1] = norder[k+1];
		k++;
	}
}

void heur(int node, int norder[], int length) //the current heuristic
{
	int curPaths = 0;
	while(curPaths < 50)
	{
		int test1 = dist(order,cityX,cityY,length);
		int test2 = dist(torder,cityX,cityY,length);
		if(curPath == length)
			order[count] = k;
		else
			torder[count] = k;
		if(test1 > test2)
			k = child[k][1];
		else
			k = linkOrd[par][k][1];
		int place = norder[k];
		if(k == 0)
			linkOrd[node][place][0] = norder[length-1];
		else
			linkOrd[node][place][0] = norder[k-1];
		if(k == length - 1)
			linkOrd[node][place][1] = norder[0];
		else
		{
			linkOrd[node][place][1] = norder[k+1];
			order[c] = originalOrder[c];
			torder[c] = originalOrder[c];
		}
		curpath+=2;
	}

}

void makeOrder(int which, int length, int par) // This method takes a set of links between each of the cities and convertes them into a path
{
	int k = 0;
	int count = 0;
	while(count < length)
	{
		if(which == 0)
			order[count] = k;
		else
			torder[count] = k;
		if(par == -1)
			k = child[k][1];
		else
			k = linkOrd[par][k][1];
		count++;
	}
}

void makeNewPool(int which, int length, int par, int node, int norder[])//This clears the current pool and makes a new one
{	
	int k = 0;
	while( k < length)
	{
		order[k] = norder[k];
		norder[k] = 0;
	}
	
}

void clearOrders(void)// this clears the global order arrays
{
	int c = 0;
	while(c < MAX)
        {
        	order[c] = originalOrder[c];
			torder[c] = originalOrder[c];
        	c++;
        }
	c = 0;
	while(c < MAX)
	{
		child[c][0] = 0;
		child[c][1] = 0;
		c++;
	}

}
void clearIt(int x[MAX])//this clears single arrays
{
	int c = 0;
	while(c < MAX)
	{
		x[c] = 0;
		c++;
	}
}

void crossover(int length, int cityX[], int cityY[]) // This method performs the crossover between two parent paths to get the child path
{
	int par1 = randomness(POOL); // The parent paths are chosen
	int par2 = randomness(POOL);
	int newLinks[MAX][4];
	int k = 0;
	int count = 0;
	while(count < length) // this converts the parent paths into a single set of all the links in both parents
	{
		newLinks[count][0] = linkOrd[par1][count][0];
		newLinks[count][1] = linkOrd[par1][count][1];
		newLinks[count][2] = linkOrd[par2][count][0];
		newLinks[count][3] = linkOrd[par2][count][1];
		count++;
	}
	count = 0;
	int whichPar = 0;
	int prev = 0;
	int used[MAX];
	clearIt(used);
	int BROKEN;
	while(count < length)//this loop will perform the crossover to make the child path
	{
		int next;
		used[k]++;
		if(count == 0)//this is for the case of the starting city, it chooses the first link from the links from the parent path that are connected to it
		{
			next = randomness(2);
			child[k][1] = newLinks[k][2*whichPar+next];
			printf("k = %d next: %d\n", k, child[k][1]);//flag
		}
		else if(count == length - 1)//this is for the case of the last city, it connects it to the starting city
		{
			child[k][0] = prev;
			used[prev]++;
			child[k][1] = 0;
			child[0][0] = k;
			printf("k = %d previous: %d\n", k, child[k][1]);//flag
			printf("k = %d next: %d\n", k, child[k][0]);//flag

		}
		else // this is for all the cities in between the starting and ending city
		{
			child[k][0] = prev;
			printf("k = %d previous: %d\n", k, child[k][0]);//flag
			used[prev]++;
			next = randomness(2);
			int nSlot = 2*whichPar+next;
			if(used[newLinks[k][nSlot]] > 0)//it first checks if the first link from the parent paths that it chose already has a city with links on it, if it doesn't it creates the link
			{
				printf("used[%d]: %d\n", newLinks[k][nSlot], used[newLinks[k][nSlot]]);
				next = fabs(whichPar-1);
				nSlot = 2*whichPar+next;
				if(used[newLinks[k][nSlot]] > 0)//if the first link didn't work, it checks the next one
				{
					printf("used[%d]: %d\n", newLinks[k][nSlot], used[newLinks[k][nSlot]]);
					whichPar = fabs(whichPar-1);
					nSlot = 2*whichPar+next;
					if(used[newLinks[k][nSlot]] > 0)//if the second link didn't work it checks the third one
					{
						printf("used[%d]: %d\n", newLinks[k][nSlot], used[newLinks[k][nSlot]]);
						next = fabs(next-1);
						nSlot = 2*whichPar+next;
						if(used[newLinks[k][nSlot]] > 0)// if the third link didn't work it checks the fourth one
						{
							printf("used[%d]: %d\n", newLinks[k][nSlot], used[newLinks[k][nSlot]]);
							BROKEN = 1;//if the fourth link didn't work it breaks out of the loop and goes to the section of code that fixes a broken path
							break;
						}
						else
						{
							child[k][1] = newLinks[k][nSlot];
							printf("k = %d next: %d\n", k, child[k][1]);//flag
							

						}
					}
					else
					{
						child[k][1] = newLinks[k][nSlot];
						printf("k = %d next: %d\n", k, child[k][1]);//flag

					}
				}
				else
				{
					child[k][1] = newLinks[k][nSlot];
					printf("k = %d next: %d\n", k, child[k][1]);//flag
				}
			}
			else
			{
				child[k][1] = newLinks[k][nSlot];
				printf("k = %d next: %d\n", k, child[k][1]);//flag
			}
			used[k]++;
		}
		prev = k;
		k = child[k][1];
		count++;
		whichPar = fabs(whichPar-1);
	}
	if(BROKEN != 1)// if its not broken it updates the pool by replacing one of the parents with the child, but only if the child has a smaller path than the pare
	{
		makeOrder(0, length, -1);
		int q = randomness(50);//A Mutation can occur here
		if(q == 0)
		{
			printf("MUTATION!!@!");
			reverse(length);
			mute = 1;
		}
		makeOrder(1, length, par1);
		double chd = dist(order,cityX,cityY,length);
		double part1 = dist(torder,cityX,cityY,length);
		double part2 = 0;
		printf("\n\n distance child: %f\n", chd);//flag
		printf("distance parent1 ( %d ): %f\n", par1, part1);
		printOrder(length, torder);
		if(chd < part1)
		{
			makeLinks(par1, order, length);
			printf("replaced parent1\n");
		}
		else
		{
			makeOrder(1, length, par2);
			part2 = dist(torder,cityX,cityY,length);
			printf("distance parent2 ( %d ): %f\n", par2, part2);//flag
			printOrder(length, torder);
			if(chd < part2)
			{
				makeLinks(par2, order, length);
				printf("replaced parent2\n");
			}
		}
	}
	else// this uses a simple greedy algorithm to fix a path that is broken, I think I might actually have a bug in this section, I'm trying to figure out how to fix it
	{
		printf("BROKEN, Count: %d\n", count);//flag
		int c = 0;
		while(c < length)
		{
			if(count<length)
			{
				if(used[c] > 0)
				{
					printf("next\n");
					c++;
					continue;
				}
				else
				{
					printf("k = %d next: %d\n", k, c);
					child[k][1] = c;
					child[c][0] = k;
					k = c;
					used[k]++;
					count++;
				}
			}
			else
			{
				child[k][1] = 0;
				child[0][0] = k;
			}
			c++;
		}
		printf("FIXED\n\n\n");//flag
		makeOrder(0, length, -1);
		int q = randomness(50);//A Mutation can occur here
		if(q == 0)
		{
			printOrder(length, order);
			reverse(length);
			printOrder(length, order);
			printf("MUTATION!!!!");
			mute = 1;
		}
		makeOrder(1, length, par1);
      double chd = dist(order,cityX,cityY,length);
      double part1 = dist(torder,cityX,cityY,length);
      double part2 = 0;
		printf("distance child: %f\n", chd);//flag
		printf("distance parent1 ( %d ): %f\n", par1, part1);
		printOrder(length, torder);
      if(chd < part1)
      {
                makeLinks(par1, order, length);
					 printf("replaced parent1\n");
      }
      else
      {
                makeOrder(1, length, par2);
                part2 = dist(torder,cityX,cityY,length);
					 printf("distance parent2 ( %d ): %f\n", par2, part2);//flag
					 printOrder(length, torder);
                if(chd < part2)
                {
                        makeLinks(par2, order, length);
								printf("replaced parent2\n");
                }
      }
	}
}

int main()
{
	FILE* fin;
	fin = fopen("a280.txt","r");
	int length;
	fscanf(fin,"%d",&length);
	int cityX[length];
	int cityY[length];
	int tempo[length];
	int k = 0;
	while(k < length)// scan the text file to get the city coordinates
	{
		fscanf(fin,"%d %d %d",&tempo[k],&cityX[k],&cityY[k]);
		k++;
	}
	fclose(fin);

	k = 0;
	//set the original order, the order in the text file, of the path
	while(k < length)
	{
		order[k] = k;
		originalOrder[k] = k;
		k++;
	}

	/*makeLinks(0, originalOrder, length);// generates 50 randomly ordered, legal paths for the pool
	k = 1;
	while(k < POOL)
	{
		runGenetic(cityX, cityY, length);
		//printf("k = %d   ", k);
		//printOrder(length,order);
		//printf("Distance: %f\n", dist(order, cityX, cityY, length));
		makeLinks(k, order, length);
		int c = 0;
		while(c < length)
		{
			order[c] = originalOrder[c];
			c++;
		}
		k++;
	}
	//printf("\n\n");
	*/
	heur(0, originalOrder, length);// uses the heuristic to generate 50 paths for the initial pool of paths

	makeOrder(0, length, 0);
	printOrder(length,order);
	reverse(length);
	printOrder(length,order);
	printf("%f\n", dist(order, cityX, cityY, length));


	int t = 0;
	while(t < 200)// run the genetic crossover 450 times
	{
		printf("RUN NUMBER: %d\n", t);
		crossover(length, cityX, cityY);
		printf("\n");
		printOrder(length, order);
		double di = dist(order, cityX, cityY, length);
		printf("Distance = %f \n", di);
		if(di <= 0)
			printOrder(length, order);
		printf("\n");
		if(mute == 1)
		{
			printOrder(length, order);
			break;
		}
		clearOrders();
		t++;
	}
}