#include <stdio.h>
#include "routing.h"

int Relax_Edge(Map_t * Map_Ptr, Edge_t * Edge_Ptr)
{

  // Temporary variables for brevity
  Distance_t Distance = Edge_Ptr->Distance;
  Node_t * Node_From_Ptr = &(Map_Ptr->Node_List_Ptr[Edge_Ptr->From]);
  Node_t * Node_To_Ptr = &(Map_Ptr->Node_List_Ptr[Edge_Ptr->To]);

#ifdef DEBUG_ROUTING
  fprintf(stderr,
	  "Relaxing (%lu (%ld) , %lu (%ld) ), dist=%ld\n",
	  Edge_Ptr->From, Node_From_Ptr->Cost,
	  Edge_Ptr->To, Node_To_Ptr->Cost,
	  Edge_Ptr->Distance);
#endif
  
  // The value to return
  int Ret_Val = 0;

  /*
    WARNING/GOTCHA:
    Subtractions instead of additions are used to avoid overflow errors
  */
  if (Node_From_Ptr->Cost < Node_To_Ptr->Cost - Distance)
    {
      /* Mark the "From" node as the "previous" node of the "to" node */
      Node_To_Ptr->Previous_ID = Edge_Ptr->From;
      Node_To_Ptr->Cost = Node_From_Ptr->Cost + Distance;
      Ret_Val = 1;

#ifdef DEBUG_ROUTING
      fprintf(stderr, "From previous of To\n");
#endif
    }
  else
    {
      if (Node_To_Ptr->Cost < Node_From_Ptr->Cost - Distance )
	{
	  /* Mark the "To" node as the "previous" node of the "From" node */
	  Node_From_Ptr->Previous_ID = Edge_Ptr->To;
	  Node_From_Ptr->Cost = Node_To_Ptr->Cost + Distance;
	  Ret_Val = 1;

#ifdef DEBUG_ROUTING
	  fprintf(stderr, "To previous of From\n");
#endif
	}
    }

#ifdef DEBUG_ROUTING
  fprintf(stderr,
	  "Now is (%lu(%ld),%lu(%ld))\n",
	  Edge_Ptr->From, Node_From_Ptr->Cost,
	  Edge_Ptr->To, Node_To_Ptr->Cost
	  );
#endif

  return Ret_Val;
}

void Dumb_Dijkstra(Map_t * Map_Ptr, Edge_t Path_Edge)
{
  // First, initialize all the cost values to the highest possible
  Index_t i;
  for (i=0; i < Map_Ptr->Num_Nodes; i++)
    {
      Map_Ptr->Node_List_Ptr[i].Cost = DISTANCE_MAX;
    }

  // Next, set the FROM node to cost 0
  Map_Ptr->Node_List_Ptr[Path_Edge.From].Cost = 0;

  /* Now, do the "canonical" (i.e. REALLY SLOW) iteration presented in class.
     This is a horribly stupid way of doing it, but it is guaranteed to work.
   */
  Index_t Cur_Node;
  Index_t Cur_Edge;

  for (Cur_Node = 0; Cur_Node < Map_Ptr->Num_Nodes; Cur_Node++)
    {
      // Variable to track whether any change was made in this pass
      int Changed = 0;
      for (Cur_Edge = 0; Cur_Edge < Map_Ptr->Num_Edges; Cur_Edge++)
	{
	  // Relax the edge
	  if ( Relax_Edge(Map_Ptr,
			  &(Map_Ptr->Edge_List_Ptr[Cur_Edge])) )
	    {
	      // If it did anything, mark the "changed" flag
	      Changed = 1;
	    }
	} // End edge iteration

      // If nothing changed this round, we can stop.
      if ( !Changed )
	{
	  break;
	}
    } // End node iteration

  /********** Edge-relaxing is done... *******/
#ifdef DEBUG_ROUTING
  fprintf(stderr, "Edges relaxed!\n");
#endif

  /* At this point, all the edges should be totally relaxed, and we can
     simply trace from the "to" back to "from", then back to "to".
     Or something.*/

  // Count up the number of nodes in the path, starting from the "To" node
  Index_t Nodes_In_Path = 1;
  Index_t Current_NodeID = Path_Edge.To;
  while (Current_NodeID != Path_Edge.From)
    {
      Current_NodeID = Map_Ptr->Node_List_Ptr[Current_NodeID].Previous_ID;
      Nodes_In_Path++;
    }

#ifdef DEBUG_ROUTING
  fprintf(stderr, "%lu nodes in path\n", Nodes_In_Path);
#endif

  // Next, make a little array to store the path
  Index_t Path_Array[Nodes_In_Path];

  // Populate the array by tracing the path again.
  Current_NodeID = Path_Edge.To;
  Path_Array[Nodes_In_Path - 1] = Path_Edge.To;

  // Start at the end of the path
  i = Nodes_In_Path - 2;
  while (Current_NodeID != Path_Edge.From)
    {
      // Move to the previous node in the path
      Current_NodeID = Map_Ptr->Node_List_Ptr[Current_NodeID].Previous_ID;
      // Add it to the path
      Path_Array[i] = Current_NodeID;
      i--;
    }

  // Finally, print that stuff out!
  printf("The shortest distance from node %lu to node %lu is %lu.\n",
	 Path_Edge.From, Path_Edge.To,
	 Map_Ptr->Node_List_Ptr[Path_Edge.To].Cost);

  printf("The shortest path from node %lu to node %lu is %lu",
	 Path_Edge.From, Path_Edge.To, Path_Array[0]);

  for (i=1; i < Nodes_In_Path; i++)
    {
      printf(" -> %lu", Path_Array[i]);
    }
  putchar('\n');

}