#include "TSP.h"     // Gets the Node data type and the function prototypes
#include <stdlib.h>  // Needed to get the NULL constant
#include <stdio.h>
#include <math.h>
#include <float.h>

/**********************************************************
 *Name           : Brandon Cox
 *Username       : bpcox
 *Description    :This program sorts a set of numbers into a linked list to try and find the most effecient path
 Constants that define the method to use when adding points to the tour
 Replace the body of all the functions in this file.
 *********************************************************/

// Print out the point stored at the given node.
// You can assume node is not NULL.
// Round to 4 decimal places and output a line feed (\n), e.g.:
// (1.2345, 6.7890)
void printNode(Node* node)
{
   Node * print = node;
   while(print !=NULL)
   {
      printf("(%lf, %lf)",print->x,print->y);
      print = print -> next;
   }
}

// Print out all the points in the tour from first to last.
// Passed a pointer to the first node in the tour.
// If the first is NULL, doesn't print anything.
void printTour(Node* first)
{
   Node * print = first;
   while(print != NULL)
   {
      printf("(%.4lf, %.4lf)\n",print->x,print->y);
      print = print -> next;
   }
}

// Get the number of points in the tour.
// Passed a pointer to the first node in the tour.
// If first is NULL, return a size of 0.
int tourSize(Node* first)
{
   if (first == NULL)
   {
      return 0;
   }
   int count = 0;
   Node * node = first;
   while(node != NULL)
   {
      count = count + 1;
      node = node -> next;
   }

   return count;
}

// Calculate the Euclidean distance between two nodes.
// You can assume both a and b are both not NULL.
double distance(Node* a, Node* b)
{
   double xdist = 0;
   double ydist = 0;
   double dist = 0;
   Node * var2 = a;
   Node * var1 = b;
   xdist = ((var2 -> x) - (var1 -> x));
   ydist = ((var2 -> y) - (var1 -> y));
   dist = sqrt((xdist * xdist + ydist * ydist));
   return dist;
}

// Calculate the total distance between all points in the tour.
// Since the tour is circular, this includes the distance from the last point back to the start.
// Passed a pointer to the first node in the tour.
// If first is NULL, return a tour length of 0.0.
double tourDistance(Node* first)
{
   double dist = 0;
   Node * node = first;
   Node * temp = node;
   if (node == NULL)
   {
      return 0;
   }
   while(node->next != NULL)
   {
      dist = distance(node->next,node)+ dist;
      node = node -> next;
   }
   dist = distance(node, temp) + dist;

   return dist;
}

// Add a new point after the point that it is closest to.
// If there is a tie, insert it after the first such point you find.
// Passed a pointer to the first node in the tour, NULL if creating a new tour.
// Returns pointer to the first node in the linked list after the addition.
Node* addNearestNeighbor(Node* first, double x, double y)
{
	double temp;
	int count = 0;
	int pos = 0;
	Node* node = first;
	Node* node2 = first;
	double dist = DBL_MAX;
	Node* tempn = first;
	tempn = malloc(sizeof(first));
	tempn->x = x;
	tempn->y = y;
	while (node != NULL)
	{

		temp = distance(tempn, node);
		count = count + 1;
		if (temp < dist && temp > 0)
		{
			dist = temp;
			pos = count;
		}
		node = node->next;
	}
	node2 = malloc(sizeof(first));
	if ((pos == count) || (pos == 0))
	{
		Node* last = first;
		node2->x = x;
		node2->y = y;
		node2->next = NULL;

		if (first == NULL)
		{
			first = node2;
			return first;
		}

		while (last->next != NULL)
		{
			last = last->next;
		}
		last->next = node2;
		return first;
	}
	else
	{
		Node* prev = first;
		node2->x = x;
		node2->y = y;
		while (pos != 1)
		{
			prev = prev->next;
			pos = pos - 1;
		}
		node2->next = prev->next;
		prev->next = node2;

		return first;
	}
}

// Add a new point after the point where it results in the least increase in tour length.
// If there is a tie, insert it after the first such point you find.
// Passed a pointer to the first node in the tour, NULL if creating a new tour.
// Returns pointer to the first node in the linked list after the addition.
Node* addSmallestIncrease(Node* first, double x, double y)
{
	double temp;
	int count = 0;
	int pos = 0;
	Node* node = first;
	Node* node2 = first;
	double dist = DBL_MAX;
	Node* tempn = first;
	tempn = malloc(sizeof(first));
	tempn->x = x;
	tempn->y = y;
	while (node != NULL)
	{
		if (node->next != NULL)
		{
			temp = distance(tempn, node) + distance(tempn, node->next) - distance(node, node->next);
		}
		else
		{
			temp = distance(tempn, first) + distance(tempn, node) - distance(node, first);
		}
		count = count + 1;
		if (temp < dist)
		{
			dist = temp;
			pos = count;
		}
		node = node->next;
	}


	node2 = malloc(sizeof(first));
	if ((pos == count) || (pos == 0))
	{
		Node* last = first;
		node2->x = x;
		node2->y = y;
		node2->next = NULL;

		if (first == NULL)
		{
			first = node2;
			return first;
		}

		while (last->next != NULL)
		{
			last = last->next;
		}
		last->next = node2;
		freeTour(node2);
		return first;
	}
	else
	{
		Node* prev = first;
		node2->x = x;
		node2->y = y;
		while (pos != 1)
		{
			prev = prev->next;
			pos = pos - 1;
		}
		node2->next = prev->next;
		prev->next = node2;

		return first;
	}
}

// Deallocate all the memory of the Node structures in the linked list.
// Passed a pointer to the first node in the tour.
// If first is NULL, don't do anything.
void freeTour(Node* first)
{
   Node* current = first;
   while (current != NULL)
   {
      Node* after = current->next;
      free(current);
      current = after;
   }
   first = NULL;
}