#ifndef lint
static char rcsid [] = "$RCSfile$ $Revision$ $State$";
#endif
/****************************************************************************
*   File: rtmp_plan_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 "librtmp_local.h"
#include <stdext.h>
#include <time.h>


void
routemap_add_edges_locally(p,g,m,routemap)
     point *p;
     graph *g;
     map *m;
     ROUTEMAP_PTR routemap;
{
  int32 i,j;
  ROUTEMAP_VERT_LIST *ptr;
  float32 square_x,square_y;

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

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

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

  for (ptr=routemap->squares[i][j].local_points;ptr!=NULL;ptr=ptr->next)
    if (((ptr->p->radius_num - 1) == p->radius_num) ||
	((ptr->p->radius_num + 1) == p->radius_num) ||
	(ptr->p->radius_num == p->radius_num))
      if (!routemap_line_hits_local_poly(m,p,ptr->p,
					 routemap->squares[i][j].local_polys,
					 my_min((p->radius_num),(ptr->p->radius_num))))
	routemap_add_adj_list(g,p,ptr->p);
}


int32
routemap_width_of_point(p,m,routemap)
     point *p;
     map *m;
     ROUTEMAP_PTR routemap;
{
  int32 w;
  int32 i,j;

  i=(int32)(ffloor((p->x - routemap->x_min) / routemap->size_square_x));
  j=(int32)(ffloor((p->y - routemap->y_min) / routemap->size_square_y));

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

  for (w=routemap->num_widths-1; w>0; w--)
    if (!routemap_point_in_any_poly(m,p,routemap->squares[i][j].local_polys,w))
      return (w);

  return (0);
}


void routemap_update_graph_locally(routemap,g,m,init,goal)
     ROUTEMAP_PTR routemap;
     graph* g;
     map* m;
     point *init, *goal;
{
  int32 temp;
  list* ptr;
  int32 i0,j0,i1,j1;

  g->v[0].x=init->x;
  g->v[0].y=init->y;
  g->v[0].which_poly=-1;
  g->v[0].vert_num=0;
  g->v[0].orig_vert=0;
  g->v[0].radius_num = routemap_width_of_point(&(g->v[0]),m,routemap);
  g->v[1].x=goal->x; 
  g->v[1].y=goal->y; 
  g->v[1].which_poly=-1;
  g->v[1].vert_num=1;
  g->v[1].orig_vert=0;
  g->v[1].radius_num = routemap_width_of_point(&(g->v[1]),m,routemap); 

  ptr=g->adj[0];
  while (ptr!=NULL)
    {
      temp=ptr->element;
      routemap_remove_edge(g,temp,0);
      ptr=ptr->next;
      routemap_remove_edge(g,0,temp);
    }
  ptr=g->adj[1];
  while (ptr!=NULL)
    {
      temp=ptr->element;
      routemap_remove_edge(g,temp,1);
      ptr=ptr->next;
      routemap_remove_edge(g,1,temp);
    }

  /* edges from goal and init to other points within their local squares */
  routemap_add_edges_locally(&(g->v[0]),g,m,routemap);
  routemap_add_edges_locally(&(g->v[1]),g,m,routemap);

  i0=(int)(ffloor((g->v[0].x - routemap->x_min) / routemap->size_square_x));
  j0=(int)(ffloor((g->v[0].y - routemap->y_min) / routemap->size_square_y));
  i1=(int)(ffloor((g->v[1].x - routemap->x_min) / routemap->size_square_x));
  j1=(int)(ffloor((g->v[1].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;
    }

  /* ADDING AN EDGE BETWEEN GOAL AND START VERTEX IF THEY ARE IN THE SAME
     CELL, AND IF NEITHER ONE IS INSIDE THE BIGGEST-WIDTH POLYGON */

  if ((i0==i1) && (j0==j1))
    if (!routemap_line_hits_poly(m,&(g->v[0]),&(g->v[1]),
				 routemap->squares[i0][j0].local_polys,
				 routemap->squares[i1][j1].local_polys,
				 routemap->num_widths-1)) 
      routemap_add_adj_list(g,&(g->v[0]),&(g->v[1]));

/**** THIS PUTS AN EDGE BETWEEN THE START AND GOAL POINTS IF THERE IS NO 
  ZERO-WIDTH POLYGON THAT INTERSECTS IT.  I AM TAKING IT OUT SINCE
  1. IT IS A GLOBAL COMPUTATION, AND MORE IMPORTANTLY
  2. IT LOOKS BAD WHEN YOU ADD WEIGHTS TO THE EDGES


  if (!routemap_line_hits_poly(m,&(g->v[0]),&(g->v[1]),
			       routemap->squares[i0][j0].local_polys,
			       routemap->squares[i1][j1].local_polys,
			       0)) 
    routemap_add_adj_list(g,&(g->v[0]),&(g->v[1]));
*/

}


void my_preplan(routemap,m,g,goal,plan)
     ROUTEMAP_PTR routemap;
     map* m;
     graph *g;
     ROUTE_POINTS goal;
     ROUTE_POINTS *plan;
{
  int i,j;
  int32 count=1;
  point origin,destination;
  int32 *pred;
  clock_t temp,temp2;
  int32 cell_x,cell_y;
  list *ptr;

  origin.x = goal.points[0].point[X];
  origin.y = goal.points[0].point[Y];
  origin.radius_num = 0;                      /* CHANGE THIS */
  destination.x = goal.points[1].point[X];
  destination.y = goal.points[1].point[Y];
  destination.radius_num = 0;                 /* CHANGE THIS */

  cell_x=(int)(ffloor((g->v[1].x - routemap->x_min) / routemap->size_square_x));
  cell_y=(int)(ffloor((g->v[1].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;
    }

  temp=clock()/100;

  routemap_update_graph_locally(routemap,g,m,&origin,&destination);

  temp2=clock()/100;

  if (!routemap_point_in_any_poly(m,&(g->v[1]),
				  routemap->squares[cell_x][cell_y].local_polys,0))
    {
      avg_update_time+=(temp2-temp);
      avg_update_time_sqd+=(((temp2-temp))*((temp2-temp)));
      temp2=clock()/100;
      
      pred=routemap_dijkstra(g,routemap);
      
      temp=clock()/100;
      
      avg_search_time+=(temp-temp2);
      avg_search_time_sqd+=(((temp-temp2))*((temp-temp2)));
    }
  else
    {
      pred = (int32 *) (malloc(sizeof(int32)*2));
      pred[1]=-1;  /* GOAL VERTEX IS INSIDE AN OBSTACLE, SO THERE IS NO PATH */
    }

  j=1;
  while ((pred[j]!=0) && (pred[j]!=-1))
    {
      j=pred[j];
      count++;
      if (count>g->num)
	{
	  j=1;
	  pred[1]=-1;
	  break;
	}
    }
  count++;
  
  if ((pred[j]!=-1) && 
      (!routemap_point_in_any_poly(m,&(g->v[1]),
				   routemap->squares[cell_x][cell_y].local_polys,0)))
    {
/*
      printf("the points connected to 0 are\n");
      for(ptr=routemap_my_visib_graph->adj[0];ptr!=NULL;ptr=ptr->next)
	printf("%d: %f,%f\n",ptr->element,routemap_my_visib_graph->v[ptr->element].x,routemap_my_visib_graph->v[ptr->element].y);
      printf("the points connected to 1 are\n");
      for(ptr=routemap_my_visib_graph->adj[1];ptr!=NULL;ptr=ptr->next)
	printf("%d: %f,%f\n",ptr->element,routemap_my_visib_graph->v[ptr->element].x,routemap_my_visib_graph->v[ptr->element].y);
      printf("that is all folks\n\n");
*/


/*
      routemap_smooth_plan(pred,&count,g,m,routemap);   
*/

      /* Allocate space */
      NS_ROUTE_ALLOCATE_POINTS(plan, count);
      
      /* Copy in the points */
      
      j=1; 
      i=count-1;
      while ((pred[j]!=-1) && (j!=0))
	{
	  plan->points[i].point[X] = g->v[j].x;
	  plan->points[i].point[Y] = g->v[j].y;
	  plan->points[i].point[Z] = (float32)(g->v[j].radius_num);
	  plan->points[i].point_id = i;
	  j=pred[j];
	  i--;
	}
      if (j==0)
	{
	  plan->points[i].point[X] = g->v[0].x;
	  plan->points[i].point[Y] = g->v[0].y;
	  plan->points[i].point[Z] = (float32)(g->v[0].radius_num);
	  plan->points[i].point_id = i;
	}
    }
  else /* there is no path */
    {
       NS_ROUTE_ALLOCATE_POINTS(plan, 1);
      
       plan->points[0].point[X] = g->v[0].x;
       plan->points[0].point[Y] = g->v[0].y;
       plan->points[0].point[Z] = (float32)(g->v[0].radius_num);
       plan->points[0].point_id = 0;
    }
      
  free(pred);
}

void
routemap_smooth_plan(pred,count,g,m,routemap)
     int *pred;
     int *count;
     graph *g;
     map *m;
     ROUTEMAP_PTR routemap;
{
  int32 first,second,third;
  int32 any_change=1;
  int32 i0,j0,i1,j1;

  while(any_change)
    {
      first=1;
      second=pred[first];
      third=pred[second];
      
      any_change=0;
      while(second!=0)
	{
	  /* if we can apply the triangle inequality then try to connect 
	     the first and third points. But dont do it on points that are on the
	     same polygon, since then you can go through the polygon */
	  if ((fsqrt(routemap_distance_squared(&(g->v[first]),&(g->v[second]))) +
	       fsqrt(routemap_distance_squared(&(g->v[second]),&(g->v[third]))))
	      > fsqrt(routemap_distance_squared(&(g->v[first]),&(g->v[third]))))
	    if ((g->v[first].which_poly==-1) || (g->v[third].which_poly==-1) ||
		(g->v[first].which_poly != g->v[third].which_poly))
	      {
		i0=(int)(ffloor((g->v[first].x - routemap->x_min) / routemap->size_square_x));
		j0=(int)(ffloor((g->v[first].y - routemap->y_min) / routemap->size_square_y));
		i1=(int)(ffloor((g->v[third].x - routemap->x_min) / routemap->size_square_x));
		j1=(int)(ffloor((g->v[third].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,&(g->v[first]),&(g->v[third]),
					     routemap->squares[i0][j0].local_polys,
					     routemap->squares[i1][j1].local_polys,
					     0))
		  {
		    any_change=1; 
		    (*count)=(*count)-1;
		    pred[first]=third;
		    if (third!=0)
		      {
			first=third;
			second=pred[first];
			third=pred[second];
		      }
		    else
		      second=0;
		  }
		else
		  {
		    first=second;
		    second=third;
		    third=pred[third];
		  }
	      }
	    else
	      {
		first=second;
		second=third;
		third=pred[third];
	      } 
	  else
	    {
	      first=second;
	      second=third;
	      third=pred[third];
	    } 
	}
    }
}