/*
 * $RCSfile$ $Revision$ $State$
 */
/****************************************************************************
*   File: rtmp_convolve.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:                                                           *
*           simple-convolve.c - cspace for 2d polygons using                *
*           Guibas, Stolfi traces.  This code was originally                *
*           written by Sundar Narasimhan, modified by Jose                  *
*           Robles and further modified by Tomas                            *
*            Lozano-Perez.  January 1995.                                   *
*       Created: Tue Nov  9 1993                                            *
*       Author: oded                                                        *
*       Remarks:                                                            *
*                                                                           *
****************************************************************************/

#include <math.h>
#include <stdio.h>
#include "planner.h"
#include "rtmp_convolve.h"

/* Some angle tests */
concave_vertex_p(min, max)
real min, max;
{
    if (min <= max) 
      return ((min + TWOPI - max) < PI);
    return ((min - max) < PI);
}

positive_angle_between(angle, min, max)
real angle, min, max;
{
    if (max < min) {
	max += TWOPI;
	if (angle < min)
	  angle += TWOPI;
    }
    return ((angle <= max) && (angle >= min));
}

angle_between(angle, min, max)
real angle, min, max;
{
    if (!(concave_vertex_p(min, max)))
      return (positive_angle_between(angle, min, max));
    else {
	if (max < min) 
	  return ((angle >= max) && (angle <= min));
	else {
	    if (angle < min)
	      angle += TWOPI;
	    min += TWOPI;
	    return ((angle >= max) && (angle <= min));
	}
    }
}

/* POLYGONAL utilities */

point
*new_point(x, y)
     real x, y;
{
  point *p = (point *) (malloc (sizeof(point)));
  p->x = x;
  p->y = y;
  return p;
}

polygonal
*new_polygonal(num_verts)
     int num_verts;
{
  polygonal *p = (polygonal *) (malloc (sizeof(polygonal)));
  p->num_vertices = num_verts;
  p->vertex = (point *) (malloc(sizeof(point) * num_verts));
  return p;
}

print_polygonal(p)
     polygonal *p;
{
  int i;
  if (p) {
    for (i=0; i<p->num_vertices; i++)
      printf("%d: [%f %f]\n",
	     i, p->vertex[i].x, p->vertex[i].y);
  }
}

/* PSTATE utilities  */

free_pstate_single(p, flag)
     pstate p;
     int flag;
{
  if (p != NULL) {
    if (flag) 
      free(p->p);
    free ((char *) p);
  }
  return 0;
}

free_pstate(state, flag)
     pstate state;
     int flag;
{
  pstate t = NULL, p = state;
  if (state) {
    /* Break the loop... */
    (state->prev)->next = NULL;
    while (p != NULL) {
      t = p;
      p = p->next;
      if (flag)
	free(t->p);
      free ((char *) t);
    }
  }
  return 0;
}

print_pstate_single(p)
     pstate p;
{
  if (p != NULL) {
    if (p->type == TYPE_MOVE) {
      real angle = atan2pi(p->max, p->min);
      printf("%s: [%f %f] (%f %f) %f\n",
	     "move",
	     x_coordinate(p->p), y_coordinate(p->p),
	     p->min, p->max, angle);
    }
    else
      printf("%s: [%f %f] (%f %f)\n",
	     "turn",
	     x_coordinate(p->p), y_coordinate(p->p),
	     p->min, p->max);
  }
}

print_pstate(state)
     pstate state;
{
  pstate p = state;
  int start = 1;
  if (state)
    for (; (start || p != state); p = p->next) {
      start = 0;
      print_pstate_single(p);
    }
}

poly_add_pstate(s, type, v, min, max)
     pstate *s;
     int type;
     point *v;
     real min, max;
{
  pstate new;
  
  if ((new = (pstate) calloc(sizeof(*new), 1)) == NULL) {
    fprintf(stderr, "poly_add_pstate: calloc failed\n");
    return -1;
  }
  new->p = v; new->min = min; new->max = max; new->type = type;
  new->next = NULL; new->prev = *s;
  if (*s != NULL) (*s)->next = new;
  *s = new;
  return 0;
}

#define compute_edge_angle(a, b) atan2pi(y_coordinate(b) - y_coordinate(a), \
					 x_coordinate(b) - x_coordinate(a))

pstate
poly_to_state(p)
     polygonal *p;
{
  pstate tail = NULL;
  pstate head = NULL;
  int i;
  point *a, *b, *prev;
  
  for (i=0; i<p->num_vertices; i++) {
    a = &(p->vertex[i]);
    b = &(p->vertex[(i+1)%p->num_vertices]);
    if (i == 0)
      prev = &(p->vertex[p->num_vertices-1]);
    else 
      prev = &(p->vertex[i-1]);    
    poly_add_pstate(&tail, TYPE_TURN, 
		    a,
		    /* normally we would store this with polygon */
		    compute_edge_angle(prev, a), /* min angle */
		    compute_edge_angle(a, b)); /* max angle */
    if (head == NULL) head = tail;
    poly_add_pstate(&tail, TYPE_MOVE,
		    a, 
		    x_coordinate(b) - x_coordinate(a),
		    y_coordinate(b) - y_coordinate(a));
  }
  /* Make it a circular list */
  tail->next = head;
  head->prev = tail;
  return (head);
}

polygonal
*state_to_poly(state)
    pstate state;
{
  int i, num_vertices = 0;
  polygonal *p;
  pstate s;

  if (state)
    for (s=state; 
	 (s != state || num_vertices == 0); 
	 s = s->next, num_vertices++) ;
  p = new_polygonal(num_vertices);
  for (i = 0, s=state; i < num_vertices; s = s->next, i++) {
    p->vertex[i].x = x_coordinate(s->p);
    p->vertex[i].y = y_coordinate(s->p);
  }
  return p;
}

/* Cspace computation, using n^2 implementation of Guibas trace algorithm. */
/* This could be sped up at the expense of hair. */

polygonal
*routemap_cspace_poly(moving, obstacle)
     polygonal *moving, *obstacle;
{
  pstate p, q, result, poly_conv_pstates_single();
  polygonal *result_poly;

  p = poly_to_state(obstacle);
  q = poly_to_state(moving);
  result = poly_conv_pstates_single(p, q);

#ifdef DEBUG
  printf("Obstacle:\n");
  print_pstate(p);
  printf("Moving:\n");
  print_pstate(q);
  printf("Result:\n");
  print_pstate(result);
#endif

  result_poly = state_to_poly(result);

  free_pstate(p, 0);
  free_pstate(q, 0);
  free_pstate(result, 0); /* CHANGE BACK TO 1 */
  return (result_poly);
}

/* convolver */
poly_test_pstates(p, q)
     pstate p, q;
{
  real pmax, qmax;
  if (p->type == q->type) {
    if (p->type == TYPE_MOVE)
      return 0;
    /* both are turns */
    if (p->max < p->min)
      pmax = p->max + TWOPI;
    else
      pmax = p->max; /*robles*/
    if (q->max < q->min)
      qmax = q->max + TWOPI;
    else
      qmax = q->max; /*robles*/

    /* BUG? -- if this is checking for range overlap, it should be an AND, */
    /* not an OR test.  Luckily, this is not really used. -- TLP */

    return ((pmax >= q->min) || (qmax >= p->min));
  }
  else {
    if (p->type == TYPE_MOVE) {
      real angle = atan2pi(p->max, p->min);
      return (angle_between(angle, q->min, q->max));
    }
    else {
      return (poly_test_pstates(q, p));
    }
  }
}

poly_convolve_single(p, q, type, x, y, min, max)
     pstate p, q;
     int *type;
     real *x, *y, *min, *max;
{
  if (p->type == q->type) {
    /* convolve the angles */
    *type = TYPE_TURN;
    /* angle convolutions not done for now!! */
    return 0;
  } else {
    if (p->type == TYPE_MOVE)
      return (poly_convolve_single(q, p, type, x, y, min, max));
    /* p->type is TYPE_TURN */
    *type = TYPE_MOVE;
    
    if (concave_vertex_p(p->min, p->max)) {
      *x = x_coordinate(p->p) + x_coordinate(q->p) + q->min;
      *y = y_coordinate(p->p) + y_coordinate(q->p) + q->max;
      *min = - (q->min);
      *max = - (q->max);
    } else {
      *x = x_coordinate(p->p) + x_coordinate(q->p);
      *y = y_coordinate(p->p) + y_coordinate(q->p);
      *min = q->min;
      *max = q->max;
    }
    return 0;
  }
}

pstate 
poly_conv_pstates_single(p, q)
     pstate p, q;
{
  pstate result_head = NULL;
  pstate result_tail = NULL;
  pstate qstart = q, qs = q, pstart = p;
  int firstp = 1, firstq = 1;
  
  while (p != pstart || firstp) {
    firstp = 0;
#ifdef DEBUG
    printf("P: "); print_pstate_single(p);
#endif
    q = qstart; firstq = 1;
    while ((q != qstart && q != qs) || firstq) {
      firstq = 0;
#ifdef DEBUG
      print_pstate_single(q);
#endif
      if (poly_test_pstates(p, q)) {
	int type; 
	real x, y, min, max;

	/* look backward for first included segment */
	if (q == qstart) {
	  while (poly_test_pstates(p, q->prev)) {
#ifdef DEBUG
	    printf("Backing through "); print_pstate_single(q);
#endif
	    q = q->prev;
	  }
	  qs = q;
	}

#ifdef DEBUG
	printf("Q: "); print_pstate_single(q);
#endif
	poly_convolve_single(p, q, &type, &x, &y, &min, &max);
	if (type == TYPE_MOVE) {
	  point *p2;
	  /* check that new point is not the same as last point.  If it is, */
	  /* don't bother adding the new point. */
	  if ((result_tail == NULL) ||
	      (x_coordinate(result_tail->p) != x) ||
	      (y_coordinate(result_tail->p) != y))
	    {
	      p2 = new_point(x, y);
	      
	      /* check that the new edge is not aligned with the previous. */
	      /* If it is, flush the previous point.*/
	      if (result_tail && 
		  (result_tail->prev != NULL)) {
		real dx1, dy1, dx2, dy2, cross;
		dx1 = x - x_coordinate(result_tail->p);
		dy1 = y - y_coordinate(result_tail->p);
		dx2 = x_coordinate(result_tail->p) 
		      - x_coordinate(result_tail->prev->p); 
		dy2 = y_coordinate(result_tail->p) 
		      - y_coordinate(result_tail->prev->p);
		cross = dx1 * dy2 - dx2 * dy1;
		if ((cross > -0.001) && (cross < 0.001)) {
		  pstate temp = result_tail->prev;
#ifdef DEBUG
		  printf("Cutting: "); print_pstate_single(result_tail);
#endif
		  free_pstate_single(result_tail, 0); /* CHANBGE TO 1 */
		  result_tail = temp;
		}
	      }
	      /* The last test could have stripped off a vertex, so make */
	      /* sure (again) that we are not repeating a vertex */
	      if ((result_tail == NULL) ||
		  (x_coordinate(result_tail->p) != x) ||
		  (y_coordinate(result_tail->p) != y)) {
		poly_add_pstate(&result_tail, TYPE_MOVE, p2, min, max);
		if (result_head == NULL) result_head = result_tail;
#ifdef DEBUG
		printf("-->R: "); print_pstate_single(result_tail);
#endif
	      }
	    }
	}
      }
      q = q->next;
    }
    p = p->next;
  }
  /* Circular list */
  result_tail->next = result_head;
  result_head->prev = result_tail;
  return (result_head);
}

/* normally, atan2 returns -pi to pi.  This returns 0 to 2pi */
real
atan2pi(y, x)
real y, x;
{
  real result;
  if (y == 0.0) {
    if (x >= 0.0)
      return 0.0;
    if (x < 0.0)
      return PI;
  }

  result = atan2(y, x);
  if (result < 0.0)
    result = TWOPI + result;
  return (result);
}

/* This if for debugging */

polygonal
*read_poly(name)
char *name;
{
  FILE *fp;
  char str[100], *tem;
  int  lineno=0, cur_index=0, no_verts=0;
  real x, y;
  polygonal *current_polygon;

  if ((fp = fopen(name, "r")) == NULL) {
    fprintf(stderr, "read_poly: cannot open file '%s'\n", name);
    return NULL;
  }

  while (!feof(fp)) {
    fgets(str, 100, fp);
    lineno++;

    tem = str;
    if (*tem == '#' || *tem == '\n') continue;
	
    if (strncmp(str, ":poly", 5) == 0) {
      if (sscanf(str, "%*s%*[ \t]%d", &no_verts) != 1) {
	fprintf(stderr, "read_poly: malformed line.\n");
	fprintf(stderr, "read_poly: error in line '%d'?\n", lineno);
	break;
      }
      current_polygon = new_polygonal(no_verts);
      if (current_polygon == NULL) {
	fprintf(stderr, "read_poly: error in line '%d'?\n", lineno);
	break;
      }

    } else {
      if (current_polygon == NULL) {
	fprintf(stderr, "read_poly: no current polygon. Line %d\n",
		lineno);
	break;
      }
      if (sscanf(tem, "%f%*[ \t]%f", &x, &y) != 2) {
	fprintf(stderr, "read_poly, mangled point entry. Line %d\n",
		lineno);
	break;
      }
      current_polygon->vertex[cur_index].x = x;
      current_polygon->vertex[cur_index].y = y;
      cur_index++;
    }
    str[0]='\n';
  }
  fclose(fp);
  printf("Read polygon from %s:\n", name);
  print_polygonal(current_polygon);
  return (current_polygon);
}

/*
main(argc, argv)
     int argc;
     char *argv[];
{
  polygonal *moving, *obstacle, *co;

  if (argc < 2) {
    printf("Useage: co moving-poly-file-name obstacle-poly-file-name\n");
    exit(1);
  }

  if ((moving = read_poly(argv[1])) == NULL)
    exit(1);
  if ((obstacle = read_poly(argv[2])) == NULL)
    exit(1);

  co = routemap_cspace_poly(moving, obstacle);
  printf("Answer is:\n");
  print_polygonal(co);

}
*/