#ifndef lint
static char rcsid [] = "$RCSfile$ $Revision$ $State$";
#endif
/****************************************************************************
*   File: rtmp_search.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: Corridor network search algorithm                         *
*       Created: Wed Aug  3 1994                                            *
*       Author: jesmith                                                     *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

#include "librtmp_local.h"
#include "/usr/modsaf/common/libsrc/libtime/libtime.h"
#include <stdext.h>
#include <stdalloc.h>
#include <math.h>

/* Maximum number of different solution steps we'll examine */
#define SEARCH_STEP_LIMIT 500

/* Really big number */
#define HUGE_COST 1.0E+99

/* Distance large enough that subdivision might help */
#define SUBDIVIDE_DIST 2500.0

/* Helper function to perform sort */
static cmp_steps(s0, s1)
    ROUTEMAP_PLAN_STEP **s0;
    ROUTEMAP_PLAN_STEP **s1;
{
    if ((*s0)->y < (*s1)->y)
      return -1;
    else if ((*s0)->y > (*s1)->y)
      return 1;
    return 0;
}

/* Find a representative distance between two segments */
float64 routemap_seg_seg_dist_sq(a, b)
    ROUTEMAP_PSEG *a, *b;
{
    float64 dx, dy;

    dx = a->x - b->x + (a->dx - b->dx) * 0.5;
    dy = a->y - b->y + (a->dy - b->dy) * 0.5;
    return dx*dx + dy*dy;
}

/* Find a representative distance between two segments */
static float64 seg_seg_dist(a, b)
    ROUTEMAP_PSEG *a, *b;
{
    return fsqrt(routemap_seg_seg_dist_sq(a, b));
}

/* Manage a dynamically allocated array of terminal nodes (assumes that
 * terminals will be used in order).
 */
static ROUTEMAP_PLAN_STEP **terminal(which)
    int32 which;
{
    static ROUTEMAP_PLAN_STEP **terminals = NULL;
    static int32 terminal_size = 0;
    uint32 size;

    if (which >= terminal_size)
    {
	terminal_size += 128;
	size = terminal_size * sizeof(ROUTEMAP_PLAN_STEP *);
	if (terminals)
	  terminals = (ROUTEMAP_PLAN_STEP **)STDREALLOC(terminals, size);
	else
	  terminals = (ROUTEMAP_PLAN_STEP **)STDALLOC(size);
    }
    return &(terminals[which]);
}

/* Free the unmarked nodes of the tree */
static void free_search_tree(leaf)
    ROUTEMAP_PLAN_STEP *leaf;
{
    /* The nodes we want to save are marked by a zero `Y' variable */
    if (leaf && !leaf->is_free && (leaf->y != 0.0))
    {
	free_search_tree(leaf->next);
	routemap_free_step(leaf);
    }
}

/* Search the solution space with an A* algorithm.
 * We reverse the sense of 'next' here, thus allowing us to expand the
 * graph into a tree.
 */
static ROUTEMAP_PLAN_STEP *a_star_search(parent, last_corridor)
    ROUTEMAP_PLAN_STEP *parent;
    ROUTEMAP_CORRIDOR  *last_corridor;
{
    ROUTEMAP_PLAN_STEP *top, *step, **term, **term0;
    ROUTEMAP_PLAN_STEP *solution;
    ROUTEMAP_CORRIDOR *connect;
    int32 n_terminals;
    int32 i, j, s;
    int32 xtended;
    ROUTEMAP_PSEG *last_seg = &last_corridor->seg;

    /* Note that we use the `x' variable in each step to hold the
     * cost to get to this node, and the `y' variable for the lower
     * bound on the total cost of the plan (`x' + dist-to-end).
     */

    term = terminal(0);
    *term = parent;
    top = parent;
    top->x = 0.0;
    top->y = seg_seg_dist(&top->corridor->seg, last_seg);
    top->corridor->cost = top->x;
    n_terminals = 1;

    for (s=0;s<SEARCH_STEP_LIMIT;)
    {
	term = terminal(0);
	top = *term;

	/* Success? */
	if (top->corridor == last_corridor)
	  break;

	/* Failure? */
	if (top->x == HUGE_COST)
	  break;

	/* Expand the top choice by one step in every possible direction,
	 * except skip corridors which have already been reached other
	 * ways with lower cost, or which have been globally excluded
	 * from the search.
	 */
	xtended = FALSE;
	for (i=0;i<top->corridor->n_connect;i++)
	{
	    connect = top->corridor->connect[i];
	    if (connect->disallowed || (connect->cost <= top->x))
	      continue;

	    step = routemap_new_step();
	    step->corridor = connect;
	    step->next = top;
	    step->x = top->x + seg_seg_dist(&top->corridor->seg,
					    &step->corridor->seg);
	    step->y = step->x + seg_seg_dist(&step->corridor->seg, last_seg);
	    step->corridor->cost = step->x;

	    /* Replace the first terminal node, then create new terminal
	     * nodes.
	     */
	    if (!xtended)
	      xtended = TRUE;
	    else
	    {
		term = terminal(n_terminals);
		n_terminals++;
	    }
	    *term = step;
	    s++;
	}

	/* See if we hit a dead-end */
	if (!xtended)
	{
	    /* It appears that any place we can reach from this top node
	     * can be reached more effectively other ways.  Give the node
	     * a monstrous cost, and slide it to the end of the list.
	     */
	    top->x = top->y = HUGE_COST;
	    term0 = terminal(0);
	    for (i=1;i<n_terminals;i++)
	    {
		term = terminal(i);
		*term0 = *term;
		term0 = term;
	    }
	    term = terminal(n_terminals-1);
	    *term = top;
	}

#if 0
	printf("\n");
	for (i=0;i<n_terminals;i++)
	{
	    ROUTEMAP_PLAN_STEP *s;
	    for (s=(*(terminal(i)));s;s=s->next)
	      printf("%x ", (int)s->corridor);
	    printf("\n");
	}
#endif

	/* Sort the terminal nodes */
	qsort(terminal(0), n_terminals, sizeof(ROUTEMAP_PLAN_STEP *),
	      cmp_steps);

	/* Prune out redundant terminal nodes */
	for (i=0;i<n_terminals;i++)
	{
	    connect = (*(terminal(i)))->corridor;

	    for (j=i+1;j<n_terminals;j++)
	    {
		term0 = terminal(j);

		if ((*term0)->corridor == connect)
		{
		    /* Crank up the cost so we know to ignore this */
		    (*term0)->x = (*term0)->y = HUGE_COST;
		}
	    }
	}
    }

    if (top->corridor == last_corridor)
    {
	solution = top;

	/* Mark the trace of the tree which we want to keep by setting the
	 * `y' variable to zero (since Y holds total cost, all other nodes
	 * in the search tree wil already be marked with a non-zero Y).
	 */
	for (step=solution;step;step=step->next)
	  step->y = 0.0;
    }
    else
      solution = NULL;

    /* Free all the unused nodes */
    for (i=0;i<n_terminals;i++)
    {
	term = terminal(i);
	free_search_tree(*term);
    }
    return solution;
}

/* Search for an occurence of the passed corridor in the steps between
 * first and last.  If found, free all the steps leading up to it or
 * coming after it, depending on the free_before flag.
 */
static ROUTEMAP_PLAN_STEP *free_to_corridor(corridor, first, last,
					    free_before)
    ROUTEMAP_CORRIDOR  *corridor;
    ROUTEMAP_PLAN_STEP *first;
    ROUTEMAP_PLAN_STEP *last;
    int32               free_before;
{
    ROUTEMAP_PLAN_STEP *i, *hold, *next;
    ROUTEMAP_PLAN_STEP *f0, *f1;

    for (i=first;i!=last;i=i->next)
      if (i->corridor == corridor)
      {
	  hold = i;
	  if (free_before)
	    f0 = first, f1 = hold;
	  else
	    f0 = hold->next, f1 = last->next;
	  for (i=f0;i!=f1;i=next)
	  {
	      next = i->next;
	      routemap_free_step(i);
	  }
	  return hold;
      }
    return NULL;
}

/* Divide the space by finding a corridor in the clump between the
 * two corridors (used when A* fails).
 */
static ROUTEMAP_CORRIDOR *divide_space(c0, c1, limit_sq)
    ROUTEMAP_CORRIDOR *c0;
    ROUTEMAP_CORRIDOR *c1;
    float64            limit_sq;
{
    ROUTEMAP_CLUMP *clump = c0->clump;
    int32 i, best;
    float64 mid_x, mid_y, dx, dy, dist, closest;

    best = -1;
    mid_x = (c0->seg.x + c1->seg.x) * 0.5;
    mid_y = (c0->seg.y + c1->seg.y) * 0.5;
    closest = 1.0E+99;
    for (i=0;i<clump->n_corridors;i++)
    {
	/*
	 * If it is disallowed OR if the corridor is the same as one of
	 * the two corridors that were passed in then skip it.  This check
	 * is to prevent infinite recursion.
	 */
	if (clump->corridors[i]->disallowed || (clump->corridors[i] == c0) ||
	    (clump->corridors[i] == c1))
	  continue;

	dx = clump->corridors[i]->seg.x - mid_x;
	dy = clump->corridors[i]->seg.y - mid_y;
	dist = dx*dx + dy*dy;
	if (dist > limit_sq)
	  continue;

	if (dist < closest)
	{
	    closest = dist;
	    best = i;
	}
    }
    if (best == -1)
      return NULL;
    return clump->corridors[best];
}

/* Find, if possible, a set of corridors connecting the first to
 * the last.  Generate a plan step for each.
 */
ROUTEMAP_PLAN_STEP *routemap_link_corridors(routemap, start,
					    first_corridor, last_corridor,
					    first_alternate, last_alternate,
					    failed, stop_time)
    ROUTEMAP_PTR        routemap;
    ROUTEMAP_PLAN_STEP *start;
    ROUTEMAP_CORRIDOR  *first_corridor;
    ROUTEMAP_CORRIDOR  *last_corridor;
    ROUTEMAP_CORRIDOR  *first_alternate;
    ROUTEMAP_CORRIDOR  *last_alternate;
    int32              *failed;
    uint32              stop_time;
{
    ROUTEMAP_PLAN_STEP *after, *root, *skip_to, *solution;
    ROUTEMAP_CLUMP *clump;
    int32 i, j;
    float64 min_cost;

    /* Search the network for a connected path using A*.
     */
    
    after = start->next;

    /* We limit cost by clamping the number of expansion steps we'll take
     * in the search.  Thus, we can use an arbitrarily large seed for
     * our best-known cost.
     */
    min_cost = HUGE_COST;

    /* Each corridor has a cost-to-get-here variable.  We start with these
     * all very large, then as we find ways to get to each corridor,
     * we note the cost, saving the smallest.  We can thus prune the
     * search tree on the fly by not exploring paths which are more expensive
     * than those already found.
     *
     * Now is also an excellent opportunity to decide which corridors we
     * may or may not use during search.
     */
    clump = first_corridor->clump;
    for (i=0;i<clump->n_corridors;i++)
    {
	clump->corridors[i]->cost = min_cost;
	if (routemap->left_bound || routemap->right_bound)
	  clump->corridors[i]->disallowed =
	    routemap_outside_boundaries(routemap, clump->corridors[i]);
    }

    /* Allow the corridors we're linking, regardless of where they are */
    first_corridor->disallowed = FALSE;
    last_corridor->disallowed = FALSE;

    /* Disallow any specifically excluded corridors */
    for (i=0;i<routemap->num_clists;i++)
      for (j=0;j<routemap->clists[i]->num_corridors;j++)
	routemap->clists[i]->corridors[j]->disallowed = TRUE;

    root = routemap_new_step();
    root->corridor = last_corridor;
    solution = a_star_search(root, first_corridor);

    /* See if we didn't find any acceptable plans */
    if (!solution)
    {
	ROUTEMAP_CORRIDOR *subdiv;
	ROUTEMAP_PLAN_STEP *part1, *part2;
	float64 d;

	/* Try to subdivide the space into two sections and plan over
	 * the shorter distance.
	 */
	if ((time_realtime_clock() < stop_time) && 
	    ((d = seg_seg_dist(&first_corridor->seg, &last_corridor->seg)) >
	     SUBDIVIDE_DIST) &&
	    (subdiv = divide_space(first_corridor, last_corridor, 0.25*d*d)))
	{
	    part1 = routemap_link_corridors(routemap, start,
					    first_corridor, subdiv,
					    first_alternate, NULL, failed,
					    stop_time);

	    part2 = routemap_link_corridors(routemap, part1,
					    subdiv, last_corridor,
					    NULL, last_alternate, failed,
					    stop_time);

	    return part2;
	}

	/* The a_star takes care of free-ing the root node */
	*failed = TRUE;
	return start;
    }

    /* (All this first/last stuff starts to get confusing here, so
     * be careful.)  The root node in the tree is the last corridor.
     * The solution node is the first corridor.
     *
     * Link in the best plan.  Search the plan for occurence of the
     * alternate corridors, in case we have an opportunity to skip
     * some steps.
     */

    if (first_alternate &&
	(skip_to = free_to_corridor(first_alternate,
				    solution, root, TRUE)))
      start->next = skip_to;
    else
      start->next = solution;

    if (last_alternate &&
	(skip_to = free_to_corridor(last_alternate,
				    start->next, root, FALSE)))
      root = skip_to;
    root->next = after;

    return root;
}