#ifndef lint
static char rcsid [] = "$RCSfile$ $Revision$ $State$";
#endif
/****************************************************************************
*   File: rtmp_init_vis.c                                                   *
*                                                                           *
*       Copyright 1994 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:                                                           *
*       Created:                                                            *
*       Author: oded                                                        *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

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

#define STD_DEV(ss,s,n) (fsqrt(((ss)/(n))-(((s)/(n))*((s)/(n)))))

int32 avg_update_time,avg_search_time,avg_update_time_sqd,avg_search_time_sqd;


/* returns 1 if p1 and p2 are the same point to within 0.01 */
int32
routemap_same_point(p1,p2)
     point *p1;
     point *p2;
{
  return ((abs(p1->x - p2->x)<0.01) && 
	  (abs(p1->y - p2->y)<0.01) &&
	  (p1->which_poly == p2->which_poly));
}


/* this routine returns the index of point p in polygon poly.  If the point is not there it 
   returns -1.
*/
int32 routemap_return_index_of_vertex(p,poly)
     point *p;
     polygonal *poly;
{
  int32 i=0;

  while (i<poly->num_vertices)
    {
      if ((abs(poly->vertex[i].x-p->x)<0.01) && 
	  (abs(poly->vertex[i].y-p->y)<0.01))
	return (i);
      i++;
    }
/*
  printf("returning -1 for vertex %f,%f poly: %d radius %d\n",
  p->x,p->y,p->which_poly,p->radius_num);
  for (i=0;i<poly->num_vertices;i++)
    printf("%f , %f     %d %d\n",
    poly->vertex[i].x,poly->vertex[i].y,poly->vertex[i].which_poly,poly->vertex[i].radius_num);
*/
  return (-1);
}


/* this routine takes poly2, which has a subset of the vertices of poly1, and sorts the 
   vertices so that they will be in the same order in poly1.  Necessary since convex_hull 
   returns the polygon unordered, and order is important.
*/

void routemap_sort_polygon(poly1,poly2)
     polygonal *poly1;
     polygonal *poly2;
{
  polygonal temp;
  int32 i,j,v;

  temp.vertex=(point *) (malloc (sizeof(point)*poly2->num_vertices));
  j=0;
  for (i=0;i<poly1->num_vertices;i++)
    {
      v=routemap_return_index_of_vertex(&(poly1->vertex[i]),poly2);
      if (v!=-1)
	{
	  temp.vertex[j].x=poly1->vertex[i].x;
	  temp.vertex[j].y=poly1->vertex[i].y;
	  temp.vertex[j].radius_num=poly1->vertex[i].radius_num;
	  j++;
	}
    }
  for (i=0;i<poly2->num_vertices;i++)
    {
      poly2->vertex[i].x=temp.vertex[i].x;
      poly2->vertex[i].y=temp.vertex[i].y;
      poly2->vertex[i].radius_num=temp.vertex[i].radius_num;
    }
  free(temp.vertex);
}
 

/* for each cell, initializes the list of polys whose bounding box intersects 
   that cell.  This is so that point_in_poly will only have to check the polys
   that are in that point's cell.
*/
void
routemap_associate_polys_with_cells(routemap,m)
     ROUTEMAP_PTR routemap;
     map *m;
{
  int32 i,j;
  int32 bottom_cell_x,bottom_cell_y,top_cell_x,top_cell_y;
  float32 square_x,square_y;
  ROUTEMAP_POLY_LIST *new_local_poly;
  int32 x,y;

  for (j=0;j<routemap->num_widths;j++)
    for (i=0;i<m->num_no_gos;i++)
      {
	square_x= (m->no_gos[j][i].b.bottom.x - routemap->x_min) / routemap->size_square_x;
	square_y= (m->no_gos[j][i].b.bottom.y - routemap->y_min) / routemap->size_square_y;
	if (square_x<0.0) square_x = 0.0;
	if (square_y<0.0) square_y = 0.0;
	bottom_cell_x=(int)(ffloor(square_x));
	bottom_cell_y=(int)(ffloor(square_y));
	square_x = (m->no_gos[j][i].b.top.x - routemap->x_min) / routemap->size_square_x;  
	square_y = (m->no_gos[j][i].b.top.y - routemap->y_min) / routemap->size_square_y;
	if (square_x<0.0) square_x = 0.0;
	if (square_y<0.0) square_y = 0.0;
	top_cell_x=(int)(ffloor(square_x));
	top_cell_y=(int)(ffloor(square_y));
	
	for (x=bottom_cell_x; x<=top_cell_x; x++)
	  for (y=bottom_cell_y; y<=top_cell_y; y++)
	    if ((x<routemap->num_x_squares) && (y<routemap->num_y_squares))
	      {
		new_local_poly = (ROUTEMAP_POLY_LIST *)(malloc(sizeof(ROUTEMAP_POLY_LIST)));
		new_local_poly->poly_num=i;
		new_local_poly->poly_width=j;
		new_local_poly->next=routemap->squares[x][y].local_polys;
		routemap->squares[x][y].local_polys=new_local_poly;
	      }
      }
}



/* adds the point to the global visibility graph.  Checks to see if an identical
   point (from the same polygon) is already there.  As a side effect, modifies
   the vert_num field of p to be the vertex number in the global graph
*/
void
routemap_add_to_global_graph(p,g,hash_table)
     point *p;
     graph *g;
     ROUTEMAP_HASH_TABLE *hash_table;
{
  int32 i;
  point *found2; 
  ENTRY item;
  ENTRY *found;
  char *key;

  found2=routemap_hash_find(hash_table,p);
  if (found2==NULL)
    {
      g->v[g->num].x=p->x;
      g->v[g->num].y=p->y;
      g->v[g->num].which_poly=p->which_poly;
      p->vert_num=g->num;
      g->v[g->num].vert_num=g->num;
      g->v[g->num].orig_vert=p->orig_vert;
      g->v[g->num].radius_num=p->radius_num;

      routemap_hash_insert(hash_table,&(g->v[g->num])); 
      g->num++;
    }
  else
    p->vert_num=found2->vert_num; 
}


void
routemap_insert_point_to_local_list(p,g,routemap,i,j)
     point *p;
     graph *g;
     ROUTEMAP_PTR routemap;
     int i,j;
{
  ROUTEMAP_VERT_LIST *new_element;

  new_element = (ROUTEMAP_VERT_LIST *) (malloc(sizeof(ROUTEMAP_VERT_LIST)));

/* OLD METHOD OF KEEPING COPIES OF THE POINT IN BOTH THE LOCAL AND
   GLOBAL LIST.  NOW JUST KEEP IT IN THE GLOBAL LIST, AND KEEP
   A POINTER IN THE LOCAL LIST 

  new_element->p.x = p->x;
  new_element->p.y = p->y;
  new_element->p.which_poly = p->which_poly;
  new_element->p.vert_num = p->vert_num;
  new_element->p.orig_vert = p->orig_vert;
  new_element->p.radius_num = p->radius_num;
*/

  new_element->p = &(g->v[p->vert_num]);
  new_element->next = routemap->squares[i][j].local_points;
  routemap->squares[i][j].local_points = new_element;
}


void
routemap_add_to_local_squares(p,routemap,g,hash_table)
     point *p;
     ROUTEMAP_PTR routemap;
     graph *g;
     ROUTEMAP_HASH_TABLE *hash_table;
{
  int32 i,j;
  point top,bottom;
  float square_x,square_y;
  int already_there;
  point *found; 

  square_x = (p->x - routemap->x_min) / routemap->size_square_x;  
  square_y = (p->y - routemap->y_min) / routemap->size_square_y;
  if (square_x<0.0) square_x = 0.0;
  if (square_y<0.0) square_y = 0.0;

  if (SAME(square_x,fceil(square_x))) 
    square_x = fceil(square_x);
  if (SAME(square_y,fceil(square_y))) 
    square_y = fceil(square_y);

  i=(int)(ffloor(square_x));
  j=(int)(ffloor(square_y));

  if ((i>=routemap->num_x_squares) || (j>=routemap->num_y_squares))
    return;

  found=routemap_hash_find(hash_table,p);
  if (found==NULL)
    {
      p->vert_num = g->num;

      routemap_insert_point_to_local_list(p,g,routemap,i,j);
      
      if (SAME(square_x,ffloor(square_x)))  /* its on the left border, so put it into i-1 as well */
	if (i>0)
	  routemap_insert_point_to_local_list(p,g,routemap,i-1,j);
      if (SAME(square_y,ffloor(square_y)))  /* its on the bottom border, so put it into j-1 as well */
	if (j>0)
	  routemap_insert_point_to_local_list(p,g,routemap,i,j-1);
      if ((SAME(square_y,ffloor(square_y))) &&
	  (SAME(square_x,ffloor(square_x)))) /* its on the bottom-left corner, 
				                so put it into i-1,j-1 as well */
	if ((j>0) && (i>0))
	  routemap_insert_point_to_local_list(p,g,routemap,i-1,j-1);
    }
  else
    p->vert_num=found->vert_num; 
}


void
routemap_insert_poly_into_graph(poly,poly_num,w,m,g,hash_table,routemap)
     polygonal *poly;
     int32 poly_num,w;
     map *m;
     graph *g;
     ROUTEMAP_HASH_TABLE *hash_table;
     ROUTEMAP_PTR routemap;
{
  int32 j;
  ROUTEMAP_VERT_LIST *inter_list;
  ROUTEMAP_VERT_LIST *ptr;
  int32 i0,j0,i1,j1;

  inter_list = (ROUTEMAP_VERT_LIST *) (malloc(sizeof(ROUTEMAP_VERT_LIST))); 

  for (j=0;j<poly->num_vertices;j++)
    {
      /*
	 get sorted list of intersection points, including start and end
	 for each point, find what square(s) it belongs to
	 add it to all those squares (and to the global graph)
	 add each edge-part to the global graph
	 */
      inter_list->next=NULL;
      inter_list->p = (point *) (malloc(sizeof(point)));
      inter_list->p->x=poly->vertex[j].x;
      inter_list->p->y=poly->vertex[j].y;
      
      routemap_get_intersections_with_grid(routemap,
					   &(poly->vertex[j]),
					   &(poly->vertex[(j+1)%poly->num_vertices]),
					   inter_list);
      /* add points to global graph and local squares */
      for (ptr=inter_list; ptr!=NULL; ptr=ptr->next)
	{
	  if ((ptr==inter_list) || (ptr->next==NULL))
	    ptr->p->orig_vert=1; /* the first and last intersections are
				    the original vertices */
	  else
	    ptr->p->orig_vert=0;
	  ptr->p->which_poly = poly_num;
	  ptr->p->radius_num = w;
	  routemap_add_to_local_squares(ptr->p,routemap,g,hash_table);
	  routemap_add_to_global_graph(ptr->p,g,hash_table);
	}
      /* add edges (segments of the original edge) to the global graph */
      for (ptr=inter_list; ptr!=NULL; ptr=ptr->next)
	if (ptr->next!=NULL)
	  {
	    i0=(int)(ffloor((ptr->p->x - routemap->x_min) / routemap->size_square_x));
	    j0=(int)(ffloor((ptr->p->y - routemap->y_min) / routemap->size_square_y));
	    i1=(int)(ffloor((ptr->next->p->x - routemap->x_min) / routemap->size_square_x));
	    j1=(int)(ffloor((ptr->next->p->y - routemap->y_min) / routemap->size_square_y));
	    
	    if ((i0>=routemap->num_x_squares) || 
		(j0>=routemap->num_y_squares) || 
		(i0<0) || (j0<0))
	      { i0=0; j0=0; }
	    if ((i1>=routemap->num_x_squares) || 
		(j1>=routemap->num_y_squares) || 
		(i1<0) || (j1<0))
	      {	i1=0; j1=0; }
	    if (!routemap_line_hits_poly(m,ptr->p,ptr->next->p,
					 routemap->squares[i0][j0].local_polys,
					 routemap->squares[i1][j1].local_polys,
					 w))
	      routemap_add_adj_list(g,ptr->p,ptr->next->p);
	  }
    }
  /* SHOULD FREE THE INTER_LIST HERE */
}



void
routemap_build_local_visib_graphs(m,g,routemap)
     map* m;
     graph* g;
     ROUTEMAP_PTR routemap;
{
  int32 i,j,k,tot_vertices,w;
  ROUTEMAP_VERT_LIST *ptr;
  ROUTEMAP_VERT_LIST *ptr2;
  float32 **temp_poly;
  polygonal convex;
  point *potentials;
  point top,bottom;
  int32 num_allocated_verts;
  int32 passed_tangency_check,v_temp;
  ROUTE_POINTS plan;
  ROUTE_POINTS goal;
  ROUTE_POINT  final_points[2];
  int32 count;
  float32 direct_dist,path_dist;
  float32 temp;
  float32 direct_dist_sqd,path_dist_sqd;
  ROUTEMAP_HASH_TABLE hash_table;
  int32 cell_x,cell_y;

  /* initialize squares */
  routemap->squares= (ROUTEMAP_SQUARE **) 
    (malloc (sizeof(ROUTEMAP_SQUARE *) * routemap->num_x_squares));
  for (i=0;i<routemap->num_x_squares;i++)
    routemap->squares[i]= (ROUTEMAP_SQUARE *) 
      (malloc (sizeof(ROUTEMAP_SQUARE) * routemap->num_y_squares));
  for (i=0;i<routemap->num_x_squares;i++)
    for (j=0;j<routemap->num_y_squares;j++)
      {
	routemap->squares[i][j].local_points=NULL;
	routemap->squares[i][j].local_polys=NULL;
      }

  routemap->size_square_x = 
    (routemap->x_max - routemap->x_min) / (float32)(routemap->num_x_squares);
  routemap->size_square_y = 
    (routemap->y_max - routemap->y_min) / (float32)(routemap->num_y_squares);

  tot_vertices=0;
  /* initialize global visibility graph */
  for (w=0;w<routemap->num_widths;w++)
    for (i=0;i<m->num_no_gos;i++)
      tot_vertices+=(m->no_gos[w][i].num_vertices);
  num_allocated_verts=(((routemap->num_x_squares+routemap->num_y_squares)*tot_vertices) + 
		       ((routemap->num_x_squares+routemap->num_y_squares)*routemap->num_glue_points) +
		       2);
  g->v=(point *) (malloc (sizeof(point) * num_allocated_verts));
  g->adj=(list **) 
    (malloc (sizeof(list *) * num_allocated_verts));
  for (i=0;i<num_allocated_verts;i++)
    g->adj[i]=NULL;
  g->num=2; /* the first two are origin and destination */

  routemap_hash_init(&hash_table,10,
		     (int32)(routemap->x_min),(int32)(routemap->y_min),
		     (int32)(routemap->x_max-routemap->x_min)/100,
		     (int32)(routemap->y_max-routemap->y_min)/100);

  routemap_associate_polys_with_cells(routemap,m);

  for (w=0;w<routemap->num_widths;w++)
    {
      /* for each polygon, for each edge, get a list of places where it intersects
	 the squares, and break it up into the squares accordingly 
	 */
      for (i=0;i<m->num_no_gos;i++)
	{
	  printf("i is %d. ",i); fflush(stdout);
	  routemap_insert_poly_into_graph(&(m->no_gos[w][i]),i,w,m,g,&hash_table,routemap);
	}

  
      /* do the same for edges around the convex hull of the polygon.  We need to not only 
	 add the edges to the global graph, but also add the points to the local squares
	 and global graph.  Well, we actually just need to add the points that are intersections
	 with the grid, not the points on the polygon 
	 */
      
      for (i=0;i<m->num_no_gos;i++)
	{
	  printf("convex %d. ",i); fflush(stdout);
	  temp_poly=(float32 **)(malloc(sizeof(float32 *) * m->no_gos[w][i].num_vertices));
	  for (k=0;k<m->no_gos[w][i].num_vertices;k++)
	    {
	      temp_poly[k]=(float32 *) (malloc(sizeof(float32)*2));
	      temp_poly[k][0]=m->no_gos[w][i].vertex[k].x;
	      temp_poly[k][1]=m->no_gos[w][i].vertex[k].y;
	    }
	  convex.vertex=(point *) (malloc(sizeof(point)*(m->no_gos[w][i].num_vertices)));
	  if (convex_hull(2,m->no_gos[w][i].num_vertices,temp_poly,NULL,&convex))
	    {
	      routemap_sort_polygon(&(m->no_gos[w][i]),&convex);

	      routemap_insert_poly_into_graph(&(convex),i,w,m,g,&hash_table,routemap);
	    }
	  free(temp_poly);
	  free(convex.vertex);
	}
      
      printf("hello\n");
      /* now add edges to the global graph that go between vertices in
	 the same local square THAT COME FROM POLYGONS OF THE SAME
	 WIDTH.  Dont connect points that belong to the same polygon
	 (thats already been done), and dont connect a point which is
	 inside a polygon (since polygons can overlap).
	 */

      for (i=0;i<routemap->num_x_squares;i++)
	{
	  for (j=0;j<routemap->num_y_squares;j++)
	    {
	      printf("%d-%d. ",i,j); fflush(stdout);
	      for (ptr=routemap->squares[i][j].local_points; ptr!=NULL; ptr=ptr->next)
		for (ptr2=ptr->next;ptr2!=NULL;ptr2=ptr2->next)
		  if ((ptr->p->radius_num==w) &&
		      (ptr->p->which_poly!=ptr2->p->which_poly) && 
		      (ptr->p->radius_num==ptr2->p->radius_num))
		    {
		      passed_tangency_check=1;

		      if ((ptr->p->orig_vert) || (ptr2->p->orig_vert))
			{
			  if (ptr->p->orig_vert)
			    {
			      v_temp=routemap_return_index_of_vertex(ptr->p,
								     &(m->no_gos[w][ptr->p->which_poly]));
			      if (v_temp==-1)
				{
				  printf("serious fuck up\n");
				  exit(668);
				}
			      else
				passed_tangency_check=(passed_tangency_check &&
						       routemap_is_line_tangent_to_poly(ptr->p,
											ptr2->p,
											&(m->no_gos[w][ptr->p->which_poly]),
											v_temp));
			    }
			  if (ptr2->p->orig_vert)
			    {
			      v_temp=routemap_return_index_of_vertex(ptr2->p,
								     &(m->no_gos[w][ptr2->p->which_poly]));
			      if (v_temp==-1)
				{
				  printf("serious fuck up\n");
				  exit(668);
				}
			      else
				passed_tangency_check=(passed_tangency_check &&
						       routemap_is_line_tangent_to_poly(ptr->p,
											ptr2->p,
											&(m->no_gos[w][ptr2->p->which_poly]),
											v_temp));
			    }
			}
		      else passed_tangency_check=1;
		  
		      if (passed_tangency_check)
			if (!routemap_line_hits_local_poly(m,ptr->p,ptr2->p,
							   routemap->squares[i][j].local_polys,w))
			  routemap_add_adj_list(g,ptr->p,ptr2->p);
		    }
	    }
	}
      /*
	 add some collection of points along the boundary of a square to its
	 local set of points.  Then connect them to other points.  Dont add
	 a point along the boundary if its inside a polygon
	 */
      
      printf("adding potential glue points\n");
      potentials= (point *) (malloc (sizeof(point)*routemap->num_glue_points));
      for (i=0;i<routemap->num_glue_points;i++)
	{
	  potentials[i].which_poly=-1;
	  potentials[i].orig_vert=0;
	  potentials[i].radius_num=w;
	}
      for (i=0;i<routemap->num_x_squares;i++)
	{
	  printf("%d. ",i); fflush(stdout);
	  for (j=0;j<routemap->num_y_squares;j++)
	    {
	      bottom.x=routemap->x_min + ((float32)i*routemap->size_square_x);
	      bottom.y=routemap->y_min + ((float32)j*routemap->size_square_y);
	      top.x=routemap->x_min + ((float32)(i+1)*routemap->size_square_x);
	      top.y=routemap->y_min + ((float32)(j+1)*routemap->size_square_y);

	      potentials[0].x=bottom.x; potentials[0].y = bottom.y;

	      if ((routemap->num_glue_points%2)!=1)
		{
		  printf("number of glue points must be odd!\n");
		  exit(667);
		}
	      for (k=1;k<=(routemap->num_glue_points-1)/2;k++)
		{
		  potentials[k].x=
		    bottom.x + 
		      (((float32)k)*(top.x-bottom.x)/((float32)((routemap->num_glue_points-1)/2)+1.0));
		  potentials[k].y=bottom.y;
	      
		  potentials[k+((routemap->num_glue_points-1)/2)].x=bottom.x;
		  potentials[k+((routemap->num_glue_points-1)/2)].y=
		    bottom.y + 
		      (((float32)k)*(top.y-bottom.y)/((float32)((routemap->num_glue_points-1)/2)+1.0));
		}
	  
	      for (k=0;k<routemap->num_glue_points;k++)
		if (!routemap_point_in_any_poly(m,&(potentials[k]),routemap->squares[i][j].local_polys,w))
		  {
		    routemap_add_to_local_squares(&(potentials[k]),routemap,g,&hash_table);
		    routemap_add_to_global_graph(&(potentials[k]),g,&hash_table);
		  }
	    }
	}
  
      for (i=0;i<routemap->num_x_squares;i++)
	{
	  printf("%d. ",i); fflush(stdout);
	  for (j=0;j<routemap->num_y_squares;j++)
	    {
	      for (ptr=routemap->squares[i][j].local_points;ptr!=NULL;ptr=ptr->next)
		if (ptr->p->which_poly==-1)  /* only connect potential points to other things */
		  for (ptr2=ptr->next;ptr2!=NULL;ptr2=ptr2->next)
		    if ((ptr->p->radius_num==w) &&
			(ptr2->p->radius_num==w))  /* only connect points from the same radius */
		      {
			passed_tangency_check=1;
			
			if ((ptr->p->orig_vert) || (ptr2->p->orig_vert))
			  {
			    if (ptr->p->orig_vert)
			      {
				v_temp=routemap_return_index_of_vertex(ptr->p,
								       &(m->no_gos[w][ptr->p->which_poly]));
				if (v_temp==-1)
				  {
				    printf("serious fuck up\n");
				    exit(668);
				  }
				else
				  passed_tangency_check=(passed_tangency_check &&
							 routemap_is_line_tangent_to_poly(ptr->p,
											  ptr2->p,
											  &(m->no_gos[w][ptr->p->which_poly]),
											  v_temp));
			      }
			    if (ptr2->p->orig_vert)
			      {
				v_temp=routemap_return_index_of_vertex(ptr2->p,
								       &(m->no_gos[w][ptr2->p->which_poly]));
				if (v_temp==-1)
				  {
				    printf("serious fuck up\n");
				    exit(668);
				  }
				else
				  passed_tangency_check=(passed_tangency_check &&
							 routemap_is_line_tangent_to_poly(ptr->p,
											  ptr2->p,
											  &(m->no_gos[w][ptr2->p->which_poly]),
											  v_temp));
			      }
			  }
			else passed_tangency_check=1;
			
			if (passed_tangency_check)
			  if (!routemap_line_hits_local_poly(m,ptr->p,ptr2->p,
							     routemap->squares[i][j].local_polys,w))
			    routemap_add_adj_list(g,ptr->p,ptr2->p);
			
		      }
	    }
	}
    }    

  /* now connecting points of different widths.  Do this by connecting points from width w
     only to points of width w+1.  We connect them if there is no polygon of width <= w
     and width > 0 that intersects that edge.  As always, we only connect points that 
     are in the same square.
     */

  for (w=0; w<routemap->num_widths-1; w++)
    {
      printf("connecting level %d to level %d...\n",w,w+1);
      for (i=0 ; i<routemap->num_x_squares ; i++)
	for (j=0 ; j<routemap->num_y_squares ; j++)
	  for (ptr=routemap->squares[i][j].local_points; ptr!=NULL; ptr=ptr->next)
	    if (ptr->p->radius_num == w)
	      for (ptr2=routemap->squares[i][j].local_points; ptr2!=NULL; ptr2=ptr2->next)
		if (ptr2->p->radius_num == w+1)
		  if (!routemap_line_hits_local_poly(m,ptr->p,ptr2->p,
						     routemap->squares[i][j].local_polys,w))
/* NOT CLEAR IF THAT LAST ARGUMENT SHOULD HAVE BEEN W OR W+1  */
		    routemap_add_adj_list(g,ptr->p,ptr2->p);
    }
		  
	      

#if 0

  /* and now a little hack to measure average time to find a path between two points.
     We pick two random points, make sure that they are not within a polygon, and then
     call my_preplan on them.  The two global variables - avg_search_time and avg_update_time
     get updated, and need to be divided by the number of experiments
     */

  avg_update_time=0;
  avg_search_time=0;
  direct_dist=0.0;
  path_dist=0.0;
  avg_update_time_sqd=0;
  avg_search_time_sqd=0;
  direct_dist_sqd=0.0;
  path_dist_sqd=0.0;
  
  count=0;
/*  srandom((int)(time(NULL)));  */
  srandom(1); 
  for (i=0;i<500;i++)
    {
      bottom.x=routemap->x_min + (random()%(int32)(routemap->x_max-routemap->x_min));
      bottom.y=routemap->y_min + (random()%(int32)(routemap->y_max-routemap->y_min));
      cell_x=(int)(ffloor((bottom.x - routemap->x_min) / routemap->size_square_x));
      cell_y=(int)(ffloor((bottom.y - routemap->y_min) / routemap->size_square_y));
      if ((cell_x<0) || (cell_y<0) || 
	  (cell_x>=routemap->num_x_squares) || (cell_y>=routemap->num_y_squares))
	{
	  cell_x=0;
	  cell_y=0;
	}
      while (routemap_point_in_any_poly(m,&bottom,routemap->squares[cell_x][cell_y].local_polys,0)) 
	{
	  bottom.x=routemap->x_min + (random()%(int32)(routemap->x_max-routemap->x_min));
	  bottom.y=routemap->y_min + (random()%(int32)(routemap->y_max-routemap->y_min));
	  cell_x=(int)(ffloor((bottom.x - routemap->x_min) / routemap->size_square_x));
	  cell_y=(int)(ffloor((bottom.y - routemap->y_min) / routemap->size_square_y));
	  if ((cell_x<0) || (cell_y<0) || 
	      (cell_x>=routemap->num_x_squares) || (cell_y>=routemap->num_y_squares))
	    {
	      cell_x=0;
	      cell_y=0;
	    }
	  count++;
	}
      top.x=routemap->x_min + (random()%(int32)(routemap->x_max-routemap->x_min));
      top.y=routemap->y_min + (random()%(int32)(routemap->y_max-routemap->y_min));
      cell_x=(int)(ffloor((top.x - routemap->x_min) / routemap->size_square_x));
      cell_y=(int)(ffloor((top.y - routemap->y_min) / routemap->size_square_y));
      if ((cell_x<0) || (cell_y<0) || 
	  (cell_x>=routemap->num_x_squares) || (cell_y>=routemap->num_y_squares))
	{
	  cell_x=0;
	  cell_y=0;
	}
      while (routemap_point_in_any_poly(m,&top,routemap->squares[cell_x][cell_y].local_polys,0)) 
	{
	  top.x=routemap->x_min + (random()%(int32)(routemap->x_max-routemap->x_min));
	  top.y=routemap->y_min + (random()%(int32)(routemap->y_max-routemap->y_min));
	  cell_x=(int)(ffloor((top.x - routemap->x_min) / routemap->size_square_x));
	  cell_y=(int)(ffloor((top.y - routemap->y_min) / routemap->size_square_y));
	  if ((cell_x<0) || (cell_y<0) || 
	      (cell_x>=routemap->num_x_squares) || (cell_y>=routemap->num_y_squares))
	    {
	      cell_x=0;
	      cell_y=0;
	    }
	  count++;
	}
      
      goal.num_pts = 2;
      goal.points  = final_points;
      final_points[0].point[X] = top.x;
      final_points[0].point[Y] = top.y;
      final_points[1].point[X] = bottom.x;
      final_points[1].point[Y] = bottom.y;
      my_preplan(routemap,m,g,goal,&plan);
      temp=routemap_distance(top.x,top.y,bottom.x,bottom.y);
      direct_dist+=temp;
      direct_dist_sqd+=(temp*temp);
      temp=0.0;
      for (j=0;j<plan.num_pts-1;j++)
	temp+=routemap_distance(plan.points[j].point[X],plan.points[j].point[Y],plan.points[j+1].point[X],plan.points[j+1].point[Y]);
      temp=temp/10.0;
      path_dist+=temp;
      path_dist_sqd+=(temp*temp);
      if (!(i%100))
      printf("%d: search %.1f +- %.1f, update %.1f +- %.1f, direct dist %.1f +- %.1f, path dist %.1f +- %.1f\n",
	     i+1,
	     (float32)(avg_search_time)/(float32)(i+1),
	     STD_DEV((float32)avg_search_time_sqd,(float32)avg_search_time,(float32)(i+1)),
	     (float32)avg_update_time/(float32)(i+1),
	     STD_DEV((float32)avg_update_time_sqd,(float32)avg_update_time,(float32)(i+1)),
	     direct_dist/(float)(i+1),
	     STD_DEV(direct_dist_sqd,direct_dist,(float32)(i+1)),
	     path_dist/(float)(i+1),
	     STD_DEV(path_dist_sqd,path_dist,(float32)(i+1)));
    }
  printf("{%d, %.1f , %.1f , %.1f , %.1f , %.1f , %.1f , %.1f , %.1f}\n",
	 routemap->num_x_squares,
	     (float32)(avg_search_time)/(float32)i,
	     STD_DEV((float32)avg_search_time_sqd,(float32)avg_search_time,(float32)i),
	     (float32)avg_update_time/(float32)i,
	     STD_DEV((float32)avg_update_time_sqd,(float32)avg_update_time,(float32)i),
	     direct_dist/(float)i,
	     STD_DEV(direct_dist_sqd,direct_dist,(float32)i),
	     path_dist/(float)i,
	     STD_DEV(path_dist_sqd,path_dist,(float32)i));
  printf("and it took %d hits to do %d samples\n",count,i);
  exit(2);
#endif

}


/* returns 1 if (x,y) is a vertex which already exists as one of the first
   num vertices of poly
*/
int32 routemap_repeated_vertex(x,y,poly,num)
     float32 x,y;
     polygonal *poly;
     int32 num;
{
  int32 i;
  
  for (i=0;i<num;i++)
    if ((SAME(poly->vertex[i].x,x)) && (SAME(poly->vertex[i].y,y)))
      return (1);
  return (0);
}

     
void routemap_create_map_and_initial_visib_graph(routemap,m,g,max_num_verts,
						 max_num_obstacles,xmin,
						 ymin,xmax,ymax,graph_file)
     ROUTEMAP_PTR routemap;
     map *m;
     graph *g;
     int32 max_num_verts,max_num_obstacles;
     float32 xmin,ymin,xmax,ymax;
     char *graph_file;
{
  ADDRESS obstacles[4000];
  float32 vertices[500][2];
  int32 i,j,k,l,width;
  uint32 obstacle_mask;
  int32 num_actual_obstacles;
  int32 num_actual_verts;
  polygonal extra_width_square;
  polygonal *temp;
  polygonal *temp2;
  FILE *fp_defaults;
  int32 num_verts,num_obstacles;

  routemap->num_widths = 3;
  routemap->extra_width = (float *) (malloc(sizeof(float)*routemap->num_widths));
  routemap->extra_width[0] = 12.0;
  routemap->extra_width[1] = 50.0; 
  routemap->extra_width[2] = 200.0;   
/*
  routemap->extra_width[3] = 400.0;
*/
  routemap->weight_for_width = (float *) (malloc(sizeof(float)*routemap->num_widths));
  routemap->weight_for_width[0] = 1.0;
  routemap->weight_for_width[1] = 0.85; 
  routemap->weight_for_width[2] = 0.7;
/*
  routemap->weight_for_width[3] = 0.55;
*/
  num_verts=max_num_verts;
  num_obstacles=max_num_obstacles;

  printf("getting up to %d obstacles with up to %d vertices each from the rectangle (%f,%f)-(%f,%f) \n"
	 ,num_obstacles,num_verts,xmin,ymin,xmax,ymax);
  printf("and expanding them by ");
  for (i=0;i<routemap->num_widths;i++)
    printf("%f , ",routemap->extra_width[i]);
  printf("\nThe graph file is %s\n",graph_file);

  extra_width_square.num_vertices=4;
  extra_width_square.vertex=(point *) (malloc (sizeof(point) * extra_width_square.num_vertices));

  obstacle_mask = ROUTEMAP_LAKES|ROUTEMAP_BOULDERS|ROUTEMAP_CANOPIES|ROUTEMAP_RIVERS; 

  routemap_get_obstacles(routemap,
			 obstacle_mask,
			 (int32)xmin,(int32)ymin,(int32)xmax,(int32)ymax,  
			 &num_obstacles,obstacles);
  
  printf("there are %d polygons here\n",num_obstacles);
  m->num_no_gos=num_obstacles;
  m->no_gos = (polygonal **) (malloc (sizeof(polygonal *) * routemap->num_widths));
  for (i=0; i<routemap->num_widths;i++)
    m->no_gos[i] = (polygonal *) (malloc (sizeof(polygonal) * num_obstacles));

  printf("they have: "); fflush(stdout);

  for (width=0; width<routemap->num_widths; width++)
    {
      i=0;
      num_actual_obstacles=0;
      while (i<num_obstacles)
	{
	  extra_width_square.vertex[0].x = -routemap->extra_width[width];
	  extra_width_square.vertex[0].y = -routemap->extra_width[width];
	  extra_width_square.vertex[1].x = routemap->extra_width[width];
	  extra_width_square.vertex[1].y = -routemap->extra_width[width];
	  extra_width_square.vertex[2].x = routemap->extra_width[width];
	  extra_width_square.vertex[2].y = routemap->extra_width[width];
	  extra_width_square.vertex[3].x = -routemap->extra_width[width];
	  extra_width_square.vertex[3].y = routemap->extra_width[width];
	  
	  num_verts=max_num_verts;
	  routemap_obstacle_to_vertices(obstacles[i],&num_verts,vertices);
	  if (num_verts!=0)
	    {
	      if (routemap_obstacle_type(obstacles[i])==ROUTEMAP_RIVERS)
		{
		  if (num_verts>2)
		    {
		      m->no_gos[width][num_actual_obstacles].num_vertices=(2*num_verts)-1;
		      printf("%d->%d ",num_verts,2*(num_verts)-1); fflush (stdout);
		      m->no_gos[width][num_actual_obstacles].vertex = 
			(point *) (malloc(sizeof(point) * (2*num_verts)-1)); 
		      for (j=0;j<num_verts;j++)
			{
			  m->no_gos[width][num_actual_obstacles].vertex[j].x = vertices[j][X];
			  m->no_gos[width][num_actual_obstacles].vertex[j].y = vertices[j][Y];
			  printf("( %f , %f )\n",vertices[j][X],vertices[j][Y]);
			  m->no_gos[width][num_actual_obstacles].vertex[j].radius_num = width; 
			}
		      k=num_verts;
		      for (j=num_verts-1;j>0;j--)
			{
			  m->no_gos[width][num_actual_obstacles].vertex[k].x = vertices[j][X];
			  m->no_gos[width][num_actual_obstacles].vertex[k].y = vertices[j][Y];
			  m->no_gos[width][num_actual_obstacles].vertex[k].radius_num = width;
			  k++;
			}
		    }
		  else /* degenerate river - just one segment */
		    {
		      m->no_gos[width][num_actual_obstacles].num_vertices=num_verts;
		      printf("%d->%d ",num_verts,num_verts); fflush(stdout);
		      m->no_gos[width][num_actual_obstacles].vertex = 
			(point *) (malloc(sizeof(point) * num_verts)); 
		      for (j=0;j<num_verts;j++)
			{
			  m->no_gos[width][num_actual_obstacles].vertex[j].x = vertices[j][X];
			  m->no_gos[width][num_actual_obstacles].vertex[j].y = vertices[j][Y];
			  m->no_gos[width][num_actual_obstacles].vertex[j].radius_num = width;
			}
		    }
		}
	      else
		{
		  m->no_gos[width][num_actual_obstacles].num_vertices=num_verts;
		  printf("%d ",num_verts); fflush(stdout);
		  m->no_gos[width][num_actual_obstacles].vertex = 
		    (point *) (malloc(sizeof(point) * num_verts));
		  j=0;
		  num_actual_verts=0;
		  while (j<num_verts)
		    {
		      if (!routemap_repeated_vertex(vertices[num_actual_verts][X],
						    vertices[num_actual_verts][Y],
						    &(m->no_gos[width][num_actual_obstacles]),
						    num_actual_verts))
			{
			  m->no_gos[width][num_actual_obstacles].vertex[num_actual_verts].x = 
			    vertices[num_actual_verts][X];
			  m->no_gos[width][num_actual_obstacles].vertex[num_actual_verts].y = 
			    vertices[num_actual_verts][Y];
			  m->no_gos[width][num_actual_obstacles].vertex[num_actual_verts].radius_num = 
			    width;
			  num_actual_verts++;
			}
		      else 
			m->no_gos[width][num_actual_obstacles].num_vertices--;
		      j++;
		    }
		}
	      /* now we make the polygon a little bit larger */
	      
	      if (m->no_gos[width][num_actual_obstacles].num_vertices<3)  /* not for single segment rivers*/
		{
		  temp=(polygonal *)(malloc(sizeof(polygonal)));
		  temp->num_vertices=m->no_gos[width][num_actual_obstacles].num_vertices;
		  temp->vertex = (point *) 
		    (malloc(sizeof(point)*(m->no_gos[width][num_actual_obstacles].num_vertices)));
		  for (k=0;k<m->no_gos[width][num_actual_obstacles].num_vertices;k++)
		    {
		      temp->vertex[k].x = m->no_gos[width][num_actual_obstacles].vertex[k].x;
		      temp->vertex[k].y = m->no_gos[width][num_actual_obstacles].vertex[k].y;
		    }
		}
	      else
		temp=routemap_ensure_simple_polygon(routemap_cspace_poly(&extra_width_square,
									 &(m->no_gos[width][num_actual_obstacles])));


#if 0
/* this is debugging stuff.  Single-segment rivers screw up */

	  for (l=0;l<m->no_gos[width][num_actual_obstacles].num_vertices;l++)
	    printf("%f %f\n",m->no_gos[width][num_actual_obstacles].vertex[l].x,
		   m->no_gos[width][num_actual_obstacles].vertex[l].y);
	  printf("\n");

	  temp2=routemap_cspace_poly(&extra_width_square,&(m->no_gos[width][num_actual_obstacles]));

	  for (l=0;l<temp2->num_vertices;l++)
	    printf("%f %f\n",temp2->vertex[l].x,temp2->vertex[l].y);
	  printf("\n");

/*	  temp=routemap_ensure_simple_polygon(temp2); */
	  temp=temp2;

	  for (l=0;l<temp->num_vertices;l++)
	    printf("%f %f\n",temp->vertex[l].x,temp->vertex[l].y);
	  printf("\n");
#endif

	      free(m->no_gos[width][num_actual_obstacles].vertex); 
	      m->no_gos[width][num_actual_obstacles].num_vertices=temp->num_vertices;
	      m->no_gos[width][num_actual_obstacles].vertex = 
		(point *) (malloc(sizeof(point)*temp->num_vertices));
	      
	      for (k=0;k<temp->num_vertices;k++)
		{
		  m->no_gos[width][num_actual_obstacles].vertex[k].x = temp->vertex[k].x;
		  m->no_gos[width][num_actual_obstacles].vertex[k].y = temp->vertex[k].y;
		  m->no_gos[width][num_actual_obstacles].vertex[k].radius_num = width;
		}
	      
	      /* print_polygonal(&(m->no_gos[width][num_actual_obstacles])); */
	      routemap_calculate_bounding_box(&(m->no_gos[width][num_actual_obstacles]));
	      num_actual_obstacles++;
	    }
	  i++;
	}
      printf(" vertices with %d actual obstacles\n",num_actual_obstacles);
      m->num_no_gos=num_actual_obstacles;
    }

  routemap->x_min=xmin;
  routemap->y_min=ymin;
  routemap->x_max=xmax;
  routemap->y_max=ymax;

  routemap_build_local_visib_graphs(m,g,routemap);
}


void read_visib_graph_from_file(fp,g)
     FILE *fp;
     graph *g;
{

/*
  int i,j,num_edges;
  list *new1;

  fscanf(fp,"%d\n",&(g->num));
  g->v = (point *) (malloc(sizeof(point)*(g->num)));
  for (i=0; i<g->num; i++)
    fscanf(fp,"%f %f %f\n",&(g->v[i].x),&(g->v[i].y),&(g->v[i].radius));
  g->adj= (list **) (malloc(sizeof(list *) * (g->num)));
  for (i=0;i<g->num;i++)
    {
      g->adj[i]=NULL;
      fscanf(fp,"%d\n",&num_edges);
      for (j=0;j<num_edges;j++)
	{
	    new1=(list *) (malloc (sizeof(list)));
	    fscanf(fp,"%d ",&(new1->element));
	    new1->next=g->adj[i];
	    g->adj[i]=new1;
	}
    }
  fclose(fp);
*/
}


void dump_visib_graph_to_file(fp,g)
     FILE *fp;
     graph *g;
{
  int i,num_edges;
  list *ptr;

/*
  fprintf(fp,"%d\n",g->num);
  for (i=0; i< g->num; i++)
    fprintf(fp,"%f %f %f\n",g->v[i].x,g->v[i].y,g->v[i].radius);
  for (i=0; i<g->num; i++)
    {
      num_edges=0;
      for(ptr=g->adj[i]; ptr!=NULL; ptr=ptr->next)
	num_edges++;
      fprintf(fp,"%d\n",num_edges);
      for (ptr=g->adj[i]; ptr!=NULL; ptr=ptr->next)
	fprintf(fp,"%d ",ptr->element);
      fprintf(fp,"\n");
    }
  fclose(fp);
*/
}