#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <limits.h>
#include <time.h>
#include <iostream.h>
 
 
 

class Graph
{
private:
 enum {INFINITY = INT_MAX};
 enum {MAXNODES = 100};
 enum {GOALNODE = 1000};

 // Queue of nodes in the shortest path
 struct shortPath
 {
  int node;
  float distance;
  struct shortPath *next;
 };
 typedef struct shortPath SHORTPATH;
 SHORTPATH shortest;

 // All nodes in the graph
 struct vertex
 {
  int xcoord;
  int ycoord;
  bool visited;

 };
 typedef struct vertex VERTEX;
 VERTEX vertices[MAXNODES + 2];
 

 // Number of nodes in graph
 int nNodes;

 void generatePairs();
 
 void addToShortQueue(int nNode, float fDistance);
 float findDistance (int nSrcNode, int nDestNode);
 bool isNodeEligible(int nNode);
 int getTailNode(SHORTPATH *queue);
 bool clearToGoal(int nChkNode);
 int deviationFromDiagonal(int nChkNode);
 
public:
 Graph(int nMyNodes);
 Graph(int nMyNodes, char *szFile);

 ~Graph();
 void setupMatrix();
 void dumpShortNodes(FILE *fp);
 void dumpAllNodes(FILE *fp);
 void dumpAllVector(FILE *fpi, bool bX);
 void generateShortest();
 float getShortPathLen();

};

// **************************************************************************
// Function: Graph Object Constructor
// Purpose: Build a Graph object using the number of nodes supplied
//    Establish the shortest queue and generate an array of vertices
//    randomly.
// Arguments: nMyNodes - input - Number of nodes
// **************************************************************************

Graph::Graph(int nMyNodes)
{
 if ( (nMyNodes > MAXNODES) || (nMyNodes < 1) )
 {
  cerr << "This program expects between 1 and " << MAXNODES << " nodes" << endl;
  exit(1);
 }

 nNodes = nMyNodes;

 // Build head node
 shortest.node = -1;
 shortest.distance = 0.00;
 shortest.next = NULL;

 // Build array of vertices with random values
 generatePairs();
}

// **************************************************************************
// Function: Graph Object Constructor
// Purpose: Build a Graph object using the supplied file of nodes
//    Establish the shortest queue and place the file of
//    nodes into the vertices array
// Arguments: nMyNodes - input - Number of nodes in file
//    szFile - input - Pathname of node file
// **************************************************************************

Graph::Graph(int nMyNodes, char *szFile)
{
 FILE *fp;
 char szLine[100];

 int j;
 int k;
 int l;

 if ((nMyNodes > MAXNODES) || (nMyNodes < 1) )
 {
  cerr << "This program expects between 1 and " << MAXNODES << " nodes" << endl;
  exit(1);
 }

 nNodes = nMyNodes;

 // Build head node
 shortest.node = -1;
 shortest.distance = 0.00;
 shortest.next = NULL;

 if ( (fp = fopen(szFile, "r")) == NULL)
 {
  cerr << "Unable to open " << szFile << ".  Make sure it exists in the working directory" << endl;
  exit(1);
 }

 vertices[0].xcoord = 0;
 vertices[0].ycoord = 0;
 vertices[0].visited = false;

 for (int i = 1; i <= nMyNodes; i++)
 {
  if ( (fgets(szLine, sizeof(szLine), fp)) == NULL)
  {
   cerr << "Error reading node " << i << " in file " << szFile << endl;
   exit(1);
  }

  if ( (sscanf(szLine, "%i %i %i", &j, &k, &l)) == NULL)
  {
   cerr << "Error parsing line " << i << " in file " << szFile << endl;
   exit(1);
  }

  vertices[i].xcoord = k;
  vertices[i].ycoord = l;
  vertices[i].visited = false;
 
 }
 vertices[nMyNodes + 1].xcoord = GOALNODE;
 vertices[nMyNodes + 1].ycoord = GOALNODE;
 vertices[nMyNodes + 1].visited = false;

 fclose(fp);

}
 
 
 
 
 
 

// **************************************************************************
// Function: dumpShortNodes
// Purpose: Write the node name and vertices from each element on the
//    shortest queue to a file.
// Arguments: fp - output - File pointer to write to
// Returns: Nothing
// **************************************************************************

void Graph::dumpShortNodes(FILE *fp)
{
 SHORTPATH *shortItr;
 float fDist;
 int nNode;

 shortItr = &shortest;

 // Shortest queue is empty
 if ( (shortItr->node == -1) && (shortItr->next == NULL) )
  return;

 // Bypass header node
 shortItr = shortItr->next;
 fDist = 0.00;
 while (shortItr != NULL)
 {
  nNode = shortItr->node;
  fDist += shortItr->distance;
  fprintf(fp, "Node %i - (%i, %i) - Distance %f\n", nNode, vertices[nNode].xcoord, vertices[nNode].ycoord, shortItr->distance);
  shortItr = shortItr -> next;
 
 }
 
 fprintf(fp, "Total distance: %f\n", fDist);

}

// *************************************************************************
// Function: getShortPathLen
// Purpose: Get the distance from each node on the shortest path, shortest
// Arguments: None.
// Returns: Sum of the distances from each node in shortest
// *************************************************************************

float Graph::getShortPathLen()
{
 SHORTPATH *shortItr;
 float fDistance = 0.00;

 shortItr = &shortest;

 if ( (shortItr->node == -1) && (shortItr->next == NULL) )
  return 0;

 // Bypass header node
 shortItr = shortItr->next;

 while (shortItr != NULL)
 {
  fDistance += shortItr->distance;
  shortItr = shortItr -> next;
 }

 return fDistance;
}

// **************************************************************************
// Function: dumpAllNodes
// Purpose: Dump the entire contents of the vertex array to a file
// Arguments: fp - output - File pointer to dump vertex array to
// Returns: Nothing
// **************************************************************************

void Graph::dumpAllNodes(FILE *fp)
{
 for (int i = 1; i <= nNodes; i++)
 {
  fprintf(fp, "%i %i %i\n", i, vertices[i].xcoord, vertices[i].ycoord);
 }
 

}

// ************************************************************************
// Function: dumpAllVector
// Purpose: For debugging purposes -- dump either the x or y portions
//    of vertices to a file.
// Arguments: fp - output - File pointer to write to
//    bX - input - if True, dump x vector else dump y vector
// Returns: Nothing
// ************************************************************************

void Graph::dumpAllVector(FILE *fp, bool bX)
{
 for (int i = 1; i <= nNodes; i++)
 {
  if (bX)
   fprintf(fp, "%i\n", vertices[i].xcoord);
  else
   fprintf(fp, "%i\n", vertices[i].ycoord);
 }
}
 

// **************************************************************************
// Function: generatePairs
// Purpose: Build an array of vertices using "randomly" generated values
//    for the x,y pairs varying from 0 to GOALNODE
// Arguments: None
// Returns: Nothing.  On completion, an array of VERTEX structures has been
//    built.
// **************************************************************************

void Graph::generatePairs()
{
 // Set starting point
 vertices[0].visited = false;
 vertices[0].xcoord = 0;
 vertices[0].ycoord = 0;

 srand( (unsigned)time(NULL));
 for (int i = 1; i <= nNodes; i++)
 {
  vertices[i].xcoord  = (rand() % (GOALNODE));
  vertices[i].ycoord  = (rand() % (GOALNODE));
  vertices[i].visited = false;
 }

 // Add goal node to array
 vertices[nNodes + 1].visited = false;
 vertices[nNodes + 1].xcoord = GOALNODE;
 vertices[nNodes + 1].ycoord = GOALNODE;
 

 /*
 for (int j = 0; j < nNodes; j++)
 {
  int nXTemp = xcoord[j];
  int nYTemp = ycoord[j];
  for (int k = j; k > 0 && nXTemp < xcoord[k - 1]; k--)
  {
   xcoord[k] = xcoord[k - 1];
   ycoord[k] = ycoord[k - 1];
  }
  xcoord[k] = nXTemp;
  ycoord[k] = nYTemp;
 }
 */

}
 

// *************************************************************************
// Function: findDistance
// Purpose: Find the cartesian distance between two given nodes using
//    the distance formula
// Arguments: nSrcNode - input - Source node
//    nDestNode - input - Destination node
// Returns: Distance between two points
// *************************************************************************
 
float Graph::findDistance(int nSrcNode, int nDestNode)
{
 int xValue;
 int yValue;
 float fReturn;

 xValue = vertices[nDestNode].xcoord - vertices[nSrcNode].xcoord;
 xValue *= xValue;

 yValue = vertices[nDestNode].ycoord - vertices[nSrcNode].ycoord;
 yValue *= yValue;

 fReturn = (float)(sqrt(xValue + yValue));
 return fReturn;

}

// *************************************************************************
// Function: generateShortest
// Purpose: Find the shortest path through the list of candidate nodes
// Arguments: None
// Returns: Nothing.  Builds the private queue of nodes comprising the
//    shortest path, shortest.
// *************************************************************************

void Graph::generateShortest()
{
 float fMinimum;
 int nMinDev;
 int nDev;
 float fCost;
 int shortNode;
 int lastShortNode;
 
 lastShortNode = 0;

 for (int start = 0; start <= nNodes; start++)
 {
  // Preset the minimum to an arbitrarily large value
  fMinimum = (float)INT_MAX;
  nMinDev = INT_MAX;
 
  // Examine each edge incident to the current node
  for (int current = 1; (current <= nNodes) ; current++)
  {
   // Consider the node only if is <> to start and if eligible
   if ( (current != start) && isNodeEligible(current) )
   {
    // Get distance from last node stored in shortest path queue
    // and this node.  Compare it to the smallest cost found
    // and least deviation.  Try to minimize both distance
    // and deviation from straight line.
    if ( ((fCost = findDistance(lastShortNode, current)) < fMinimum) && ( (nDev = deviationFromDiagonal(current)) < nMinDev) )
    {
     nMinDev = nDev;
     fMinimum = fCost;
     shortNode = current;
    }
   }
  }
 
  lastShortNode = shortNode;

  // This node will not be considered again
  vertices[shortNode].visited = true;
 
  // Put node into queue of nodes comprising shortest path
  addToShortQueue(shortNode, fMinimum);

 
  if (clearToGoal(shortNode))
  {
   // If there are no nodes between this node and the goal node,
   // calculate the distance and add the goal node to the queue

   float fDistFromGoal = findDistance(shortNode, nNodes + 1);
   addToShortQueue(nNodes + 1, fDistFromGoal);
   return;
  }
 }
}
 

// **************************************************************************
// Function: deviationFromDiagonal
// Purpose: Determine how far this node is from the diagonal that defines
//    the shortest point from start node to goal node
// Arguments: nChkNode - input - Node to check
// Returns: Difference between this nodes y and x coordinates
// **************************************************************************

int Graph::deviationFromDiagonal(int nChkNode)
{
 return (abs(vertices[nChkNode].ycoord - vertices[nChkNode].xcoord));
}

// **************************************************************************
// Function: clearToGoal
// Purpose: Determine if the node under consideration has any eligible
//    nodes from here to the goal node
// Arguments: nChkNode - input - Node under consideration
// Returns: True if there are no eligible nodes between this node and the
//    goal node.
// **************************************************************************

bool Graph::clearToGoal(int nChkNode)
{
 int nChkX;
 int nChkY;
 int nEvalX;
 int nEvalY;
 bool bVisited;

 
 nChkX = vertices[nChkNode].xcoord;
 nChkY = vertices[nChkNode].ycoord;

 for (int i = 1; i <= nNodes; i++)
 {
  if (i != nChkNode)
  {
   nEvalX = vertices[i].xcoord;
   nEvalY = vertices[i].ycoord;
   bVisited = vertices[i].visited;

   if ( (bVisited == false) && ( (nEvalX >= nChkX) && (nEvalY >= nChkY)) )
    return false;
  }
 }
 
 

 return true;

}
 
 

// *************************************************************************
// Function: getTailNode
// Purpose: Return the node on the tail of the queue
// Arguments: queue - input - Queue to extract tail node from
// Returns: Node from the tail of the queue, or NULL if queue is empty
// *************************************************************************

int Graph::getTailNode(SHORTPATH *queue)
{
 SHORTPATH *shortItr;
 int nNode;

 shortItr = queue;

 if ( (shortItr->node == -1) && (shortItr->next == NULL) )
  return NULL;

 while (shortItr != NULL)
 {
  nNode = shortItr->node;
  shortItr = shortItr -> next;
 }

 return nNode;

}
 

// *************************************************************************
// Function: addToShortQueue
// Purpose: Add this node to the queue of nodes comprising the optimal
//    path (shortest).
// Arguments: nNode - input - Node to be placed on the queue
//    nDistance - input - Distance between this node and previous one
// Returns: Nothing
// *************************************************************************
 
void Graph::addToShortQueue(int nNode, float fDistance)
{
 SHORTPATH *myShort;
 SHORTPATH *shortItr;
 SHORTPATH *lastShort;

 shortItr = &shortest;

 // Walk to the end of the queue
 while(shortItr != NULL)
 {
  // Dont want to enqueue the same node 2x
  if ((shortItr->node) == nNode)
   return;
  lastShort = shortItr;
  shortItr = shortItr -> next;
 
 }

 // Create a new node and place it on the tail of the queue
 myShort = new SHORTPATH;
 myShort->next = NULL;
 myShort->node = nNode;
 myShort->distance = fDistance;

 lastShort -> next = myShort;
 

}

// ***************************************************************************
// Function: isNodeEligible
// Purpose: Determine if the node under consideration is eligible to be
//    considered to be on the optimal path.
// Arguments: nNode - input - Node under consideration
// Returns: True if node is eligible, False if not
// ***************************************************************************
bool Graph::isNodeEligible(int nNode)
{
 int nShortNode;
 int nShortx;
 int nShorty;
 int nNodex;
 int nNodey;

 // Get the tail node off the shortest path queue
 // If shortPath queue empty, node is always eligible
 if ( (nShortNode = getTailNode(&shortest)) == -1)
  return true;

 
 // Compare the coordinate of the node under consideration with the
 // node from the queue.  This node will only be considered eligible
 // if is greater than the last queued node

 nShortx = vertices[nShortNode].xcoord;
 nShorty = vertices[nShortNode].ycoord;
 nNodex = vertices[nNode].xcoord;
 nNodey = vertices[nNode].ycoord;

 return ( (nNodex > (nShortx)) && (nNodey > (nShorty)) );

}
 
 
 

void main(int argc, char **argv)
{

 
 FILE *fp;
 Graph *g;
 int nNodes;
 

 if ( (argc != 2) && (argc != 3) )
 {
  cout << "Usage:  dijkstra <nodes> or dijkstra <nodes> <filename>" << endl;
  exit(1);
 }

 nNodes = atoi(argv[1]);

 if (argc == 3)
  g = new Graph(nNodes, argv[2]);
 else
  g = new Graph(nNodes);

 // Calculate shortest path
 g ->generateShortest();

 // Dump short path nodes to a file
 fp = fopen("shortnodes.prn", "w");
 if (fp)
  g->dumpShortNodes(fp);
 fclose(fp);

 // Dump all nodes to a file
 fp = fopen("allnodes.prn", "w");
 if (fp)
  g->dumpAllNodes(fp);
 fclose(fp);

 /*
 fp = fopen("x.prn", "w");
 if (fp)
  g->dumpAllVector(fp, true);
 fclose(fp);

 fp = fopen("y.prn", "w");
 if (fp)
  g->dumpAllVector(fp, false);
 fclose(fp);
 */

 cout << "Shortest path len is " << g->getShortPathLen() << endl;
 
 

}