#ifndef lint
static char rcsid [] = "$RCSfile$ $Revision$ $State$";
#endif
/****************************************************************************
*   File: rtmp_graph.c                                                      *
*                                                                           *
*       Copyright 1993 by Loral Advanced Distributed Simulation, Inc.       *
*                                                                           *
*               Loral Advanced Distributed Simulation, Inc.                 *
*               10 Moulton Street                                           *
*               Cambridge, MA 02238                                         *
*               617-873-1850                                                *
*                                                                           *
*       This software was developed by Loral under U. S. Government contracts*
*       and may be reproduced by or for the U. S. Government pursuant to    *
*       the copyright license under the clause at DFARS 252.227-7013        *
*       (OCT 1988).                                                         *
*                                                                           *
*       Contents: Code to create and initialize a routemap                  *
*       Created: Tue Nov  9 1993                                            *
*       Author: oded                                                        *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

#include "librtmp_local.h"
#include <stdext.h>

/*
  utility routine used to print out the graph structure
*/
void routemap_dump_graph(g)
     graph g;
{
  int32 i;
  list* ptr;

  for (i=0;i<g.num;i++)
    {
      printf("%3d <%3.1f,%3.1f> --> ",i,g.v[i].x,g.v[i].y);
      ptr=g.adj[i];
      while (ptr!=NULL)
	{
	  printf("%d, ",ptr->element);
	  ptr=ptr->next;
	}
      printf("\n");
    }
}


/*
  given a point, it returns the index into the list of vertices which
  holds that point.  Not too graceful if it doesn't find the point 
*/

int32 routemap_which_vertex(g,ver)
     graph* g;
     point *ver;
{
  int32 i;

/*
OLD VERSION
  for (i=0;i<g->num;i++)
    if (routemap_same_point(&(g->v[i]),&ver))	
      return (i);
*/

  if (ver->vert_num<g->num)
    return (ver->vert_num);
  printf ("oh oh, couldn't find it in the vertex list\n");
  printf("point is %f %f %d %d\n",ver->x,ver->y,ver->which_poly,ver->vert_num);
  exit(666);
}


/*
  removes the edge from vertex row to vertex entry
*/
void routemap_remove_edge(g,row,entry)
     graph* g;
     int32 row,entry;
{
  list* ptr;
  list* back;
  list* gone;

  if (g->adj[row]!=NULL)
    if (g->adj[row]->element==entry)
      {
	gone=g->adj[row];
	g->adj[row]=g->adj[row]->next;
	free(gone);
      }
    else
      {
	back=g->adj[row];
	ptr=back->next;
	while ((ptr!=NULL) && (ptr->element!=entry))
	  {
	    back=ptr;
	    ptr=ptr->next;
	  }
	if (ptr!=NULL)
	  {
	    gone=ptr;
	    back->next=ptr->next;
	    free(gone);
	  }
      }
}


/*
  adds the edge from v1 to v2 by adding elements to the adjacency lists
  of both v1 and v2 (bidirectional graph)
*/
void routemap_add_adj_list(g,v1,v2)
     graph* g;
     point *v1,*v2;
{
  list* new1;
  list* new2;
  int32 loc1, loc2;

  loc1=routemap_which_vertex(g,v1);
  loc2=routemap_which_vertex(g,v2);
  new1=(list *) (malloc (sizeof(list)));
  new2=(list *) (malloc (sizeof(list)));
  new1->element=loc2;
  new1->next=g->adj[loc1];
  new2->element=loc1;
  new2->next=g->adj[loc2];
  g->adj[loc1]=new1;
  g->adj[loc2]=new2;
}


float32 routemap_weighted_distance(p1,p2,routemap)
     point *p1;
     point *p2;
     ROUTEMAP_PTR routemap;
{
  float32 mult;

  mult=routemap->weight_for_width[my_min((p1->radius_num),(p2->radius_num))];

  return( mult * fsqrt(sqr(p1->x - p2->x) + sqr(p1->y - p2->y)));

}


/*
  searches the graph for a path from vertex 0 to vertex 1 using Dijkstra's
  algorithm.  It returns a predecessor chain.  All the details and explanation
  can be found in Cormen, Leiseron, Rivest _Introduction to Algorithms_.
  Also:
   - I stop as soon I reach vertex 1, so that it doesn't go through and try 
   to find the shortest path between vertex 0 and _every_ other vertex.  
   - I use a binary heap structure (as suggested in CLR) to make it faster.
   - CLR's usage of heaps along with Dijsktra is broken.  I fixed it using a
   "location" array which keeps track of the location of vertex i in the 
   heap.
   - the heap is indexed from 1 to n, not from 0 to n-1
*/
int32* routemap_dijkstra(g,routemap)
     graph* g;
     ROUTEMAP_PTR routemap;
{
  int32* pred;
  int32 i;
  heap_element* heap;
  int32 heap_size;
  heap_element u;
  list *ptr;
  float32 dist;
  int32 *location;

  pred = (int *) (malloc (sizeof(int) * g->num));
  heap = (heap_element *) (malloc (sizeof(heap_element) * (g->num+1)));
  location = (int *) (malloc (sizeof(int) * g->num));
  for (i=0;i<g->num;i++)
    {
      pred[i]=-1;               /* init predecessor chain */
      heap[i+1].key=INFINITY;   /* init heap */
      heap[i+1].ver_num=i;
      location[i]=i+1;          /* init location array.  Vertex i is in 
				   location i+1 */
    }
  routemap_build_heap(heap,g->num,location);
  routemap_heap_decrease_key(heap,g->num,location[0],0.0,location);  /*source is verex 0 */
  heap_size=g->num;
  while (heap_size!=0) 
    {
      u=routemap_heap_extract_min(heap,heap_size,location);
      heap_size--;
      if (u.ver_num==1)          /* exit early if we reach vertex 1 */
	{
	  free(heap);
	  free(location);
	  return (pred);
	}
      ptr=g->adj[u.ver_num];     /* go through adjacency list */
      while (ptr!=NULL)
	{
	  /* ANYTHING PAST THE FIRST TWO TERMS IS THERE TO MAKE IT A* */
	  dist=(routemap_weighted_distance(&(g->v[u.ver_num]),&(g->v[ptr->element]),routemap) 
		+ u.key
/* MULTIPLY BY THE LOWEST WEIGHT (HIGHEST WIDTH) SO THAT THE DISTANCE-TO-GO 
   ESTIMATE IS OPTIMISTIC */
		+ (routemap_weighted_distance(&(g->v[ptr->element]),&(g->v[1]),routemap) *
		   routemap->weight_for_width[routemap->num_widths-1]));

/* THIS USED TO BE A*, BUT I DONT THINK ITS RIGHT ANYMORE */
/*
		- routemap_weighted_distance(&(g->v[u.ver_num]),&(g->v[1]),routemap) 
		+ routemap_weighted_distance(&(g->v[ptr->element]),&(g->v[1]),routemap));
*/
	  if (dist>INFINITY)
	    dist=INFINITY;
	  if (heap[location[ptr->element]].key>dist)
	    {
	      routemap_heap_decrease_key(heap,heap_size,location[ptr->element],dist,location);
	      pred[ptr->element]=u.ver_num;
	    }
	  ptr=ptr->next;
	}
    }
  free(heap);
  free(location);
  return (pred);
}