/*
 *   GRacer
 *
 *   Copyright (C) 1999 Takashi Matsuda <matsu@users.sourceforge.net>
 *
 * This program is free software; you can redistribute it and/or 
 * modify it under the terms of the GNU General Public License as 
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 * USA
 */

#include <assert.h>
#include <string.h>
#include "gr_course.h"
#include "gr_memory.h"
#include "gr_vertex.h"

enum {
  TOKEN_NUM_GRID,
  TOKEN_GRID,
  TOKEN_NUM_CLINE,
  TOKEN_CLINE,
  TOKEN_NUM_MAT,
  TOKEN_MAT,
  TOKEN_NUM_SEG,
  TOKEN_SEG,
  TOKEN_NUM_VERT,
  TOKEN_NUM_SURF,
  TOKEN_TITLE,
  TOKEN_DESC,
  TOKEN_MODEL,
  TOKEN_TIMESTAMP,

  TOKEN_INVALID = -1,
};

static struct {
  int type;
  char *str;
  int len;
} tokens[] = {
  {TOKEN_NUM_GRID,	"NUM_GRID",	8},
  {TOKEN_GRID,		"GRID",		4},
  {TOKEN_NUM_CLINE,	"NUM_CLINE",	9},
  {TOKEN_CLINE,		"CLINE",	5},
  {TOKEN_NUM_MAT,	"NUM_MAT",	7},
  {TOKEN_MAT,		"MAT",		3},
  {TOKEN_NUM_SEG,	"NUM_SEG",	7},
  {TOKEN_SEG,		"SEG",		3},
  {TOKEN_NUM_VERT,	"NUM_VERT",	8},
  {TOKEN_NUM_SURF,	"NUM_SURF",	8},
  {TOKEN_TITLE,		"TITLE",	5},
  {TOKEN_DESC,		"DESC",		4},
  {TOKEN_MODEL,		"MODEL",	5},
  {TOKEN_TIMESTAMP,	"TIMESTAMP",	9},
  {TOKEN_INVALID,	NULL,	-1},
};


static int		next_line		(char *data, int *index);
static int		find_token		(char *data, int *index);
static int		next_word		(char *data, int *index);
static char*		get_name		(char *data, int *index);
static void		put_name		(char *str, FILE *file);
static GrCSegment*	read_segment		(GrCourse *course,
    						 char *data,
						 int *index);
static void		create_boundary		(GrCSegment *seg);
static void		initialize_surface	(GrCSegment *seg);


static void
gr_csegment_free (GrCSegment *seg)
{
  gr_boundary_free (seg->boundary);
  free (seg->verts);
  free (seg->surfs);
}

static void
gr_course_free (GrCourse *course)
{
  int i;

  free (course->title);
  free (course->desc);
  free (course->model);
  free (course->timestamp);
  free (course->grids);
  free (course->clines);
  free (course->mats);

  for (i=0; i<course->num_segs; i++) {
    free (course->segs[i]);
  }
  free (course->segs);

  free (course);
}

GrCourse*
gr_course_new_from_file (FILE *file)
{
  size_t pos;
  size_t len;
  char *buf;
  GrCourse *course;

  pos = ftell (file);
  fseek (file, 0, SEEK_END);
  len = ftell (file) - pos;
  fseek (file, pos, SEEK_SET);

  buf = gr_new (char, len+1);
  if (!buf) {
    return NULL;
  }
  fread (buf, 1, len, file);
  buf[len] = '\0';

  course = gr_course_new_from_data (buf);
  free (buf);

  return course;
}

GrCourse*
gr_course_new_from_data (char *data)
{
  GrCourse *course = NULL;
  GrControlLine *cline;
  int i = 0, j, k;
  int token;
  float x[6];
  float y[6];
  float f;

  if (strncmp (data, "GRACER COURSE/1.0\n", 18))
    goto ERROR;

  course = gr_new0 (GrCourse, 1);
  if (!course)
    return NULL;
  gr_FREE_FUNC (course, gr_course_free);

  next_line (data, &i);
  while (data[i] != '\0') {
    token = find_token (data, &i);
    switch (token) {
    case TOKEN_TITLE:
      next_word (data, &i);
      course->title = get_name (data, &i);
      break;

    case TOKEN_DESC:
      next_word (data, &i);
      course->desc = get_name (data, &i);
      break;

    case TOKEN_MODEL:
      next_word (data, &i);
      course->model = get_name (data, &i);
      break;

    case TOKEN_TIMESTAMP:
      next_word (data, &i);
      course->timestamp = get_name (data, &i);
      break;

    case TOKEN_NUM_GRID:
      next_word (data, &i);
      sscanf (data+i, "%d", &course->num_grids);
      course->grids = gr_new (GrGrid, course->num_grids);
      next_line (data, &i);
      for (j=0; j<course->num_grids; j++) {
	sscanf (data+i, "GRID %d %d %f %f %f",
	    	&course->grids[j].flag,
	    	&course->grids[j].segment,
	    	&course->grids[j].x,
	    	&course->grids[j].y,
	    	&course->grids[j].dir);
	next_line (data, &i);
      }
      break;

    case TOKEN_NUM_CLINE:
      next_word (data, &i);
      sscanf (data+i, "%d", &course->num_clines);
      course->clines = gr_new (GrControlLine, course->num_clines);
      next_line (data, &i);
      for (j=0; j<course->num_clines; j++) {
	cline = course->clines + j;

	sscanf (data+i, "CLINE %d %d",
	    	&cline->segment,
	    	&cline->flag);
	next_line (data, &i);
	for (k=0; k<6; k++) {
	  sscanf (data+i, "%f %f", &x[k], &y[k]);
	  next_line (data, &i);
	}
	cline->region_a[0].c.x = x[0];
	cline->region_a[0].c.y = y[0];
	cline->region_a[1].c.x = x[1];
	cline->region_a[1].c.y = y[1];
	cline->region_a[2].c.x = x[3];
	cline->region_a[2].c.y = y[3];
	cline->region_a[3].c.x = x[2];
	cline->region_a[3].c.y = y[2];

	cline->region_b[0].c.x = x[2];
	cline->region_b[0].c.y = y[2];
	cline->region_b[1].c.x = x[3];
	cline->region_b[1].c.y = y[3];
	cline->region_b[2].c.x = x[5];
	cline->region_b[2].c.y = y[5];
	cline->region_b[3].c.x = x[4];
	cline->region_b[3].c.y = y[4];

	/* calcurate boundary rectangle */
	cline->xmax = cline->xmin = x[0];
	cline->ymax = cline->ymin = y[0];
	for (k=1; k<6; k++) {
	  if (cline->xmax < x[k]) {
	    cline->xmax = x[k];
	  }
	  if (cline->xmin > x[k]) {
	    cline->xmin = x[k];
	  }
	  if (cline->ymax < y[k]) {
	    cline->ymax = y[k];
	  }
	  if (cline->ymin > y[k]) {
	    cline->ymin = y[k];
	  }
	}

	/* rotate 90 degree */
	for (k=0; k<4; k++) {
	  gr_vertex_sub_xy (cline->rot_a+k,
			    cline->region_a + (k+1) % 4,
			    cline->region_a + k);
	  f = cline->rot_a[k].c.x;
	  cline->rot_a[k].c.x = - cline->rot_a[k].c.y;
	  cline->rot_a[k].c.y = f;

	  gr_vertex_sub_xy (cline->rot_b+k,
			    cline->region_b + (k+1) % 4,
			    cline->region_b + k);
	  f = cline->rot_b[k].c.x;
	  cline->rot_b[k].c.x = - cline->rot_b[k].c.y;
	  cline->rot_b[k].c.y = f;
	}
      }

      break;

    case TOKEN_NUM_MAT:
      next_word (data, &i);
      sscanf (data+i, "%d", &course->num_mats);
      course->mats = gr_new (GrCMaterial, course->num_mats);
      next_line (data, &i);
      for (j=0; j<course->num_mats; j++) {
	sscanf (data+i, "MAT %d %f %f %f",
	    	&course->mats[j].flag,
	    	&course->mats[j].friction,
	    	&course->mats[j].exp,
	    	&course->mats[j].mu);
	next_line (data, &i);
      }
      break;

    case TOKEN_NUM_SEG:
      next_word (data, &i);
      sscanf (data+i, "%d", &course->num_segs);
      course->segs = gr_new (GrCSegment *, course->num_segs);
      next_line (data, &i);
      for (j=0; j<course->num_mats; j++) {
	course->segs[j] = read_segment (course, data, &i);
      }
      break;
    }
  }

  for (i=0; i<course->num_segs; i++) {
    initialize_surface (course->segs[i]);
    create_boundary (course->segs[i]);
  }

  return course;

ERROR:
  if (course) {
    gr_DECREF (course);
  }
  return NULL;
}


static GrCSurface *
gr_course_find_seg_surface (GrCourse *course,
    			    float x, float y,
			    int segment_cur)
{
  GrSList *list;
  GrBoundary *yboundary;
  GrCSegment *seg;
  int i, j;
  GrCSurface *surf;
  GrVertex v;

  v.c.x = x;
  v.c.y = y;

  seg = course->segs[segment_cur];

  for (i=100; i>0; i--) {
    yboundary = (GrBoundary *) gr_boundary_check (seg->boundary, x, &i);
    if (!yboundary)
      return NULL;

    for (j=100; j>0; j--) {
      list = (GrSList *) gr_boundary_check (yboundary, y, &j);

      if (!list)
	break;

      while (list) {
	surf = (GrCSurface *) list->data;

	if (gr_vertex_check_inner_xy (&v, 3, surf->v))
	  return surf;

	list = list->next;
      }
    }
  }

  return NULL;
}

GrCSurface*
gr_course_find_surface (GrCourse *course,
		        float x, float y,
		        int segment_cur,
		        int *segment_ret)
{
  GrCSurface *surf;
  int seg;
  int i;

  if (course == NULL)
    return NULL;

  surf = gr_course_find_seg_surface (course, x, y, segment_cur);
  if (surf) {
    *segment_ret = segment_cur;
    return surf;
  }
  for (i=1; i<course->num_segs/2; i++) {
    seg = (segment_cur + i) % course->num_segs;
    surf = gr_course_find_seg_surface (course, x, y, seg);
    if (surf) {
      *segment_ret = seg;
      return surf;
    }
    seg = (segment_cur - i) % course->num_segs;
    surf = gr_course_find_seg_surface (course, x, y, seg);
    if (surf) {
      *segment_ret = seg;
      return surf;
    }
  }

  return NULL;
}

void
gr_course_write_file (GrCourse *course, FILE *file)
{
  int i, j;
  GrControlLine *cline;

  fputs ("GRACER COURSE/1.0\n", file);

  fputs ("TITLE ", file);
  put_name (course->title, file);
  fputc('\n', file);

  fputs ("DESC ", file);
  put_name (course->desc, file);
  fputc('\n', file);

  fputs ("MODEL ", file);
  put_name (course->model, file);
  fputc('\n', file);

  fputs ("TIMESTAMP ", file);
  put_name (course->timestamp, file);
  fputc('\n', file);

  fprintf (file, "NUM_GRID %d\n", course->num_grids);
  for (i=0; i<course->num_grids; i++) {
    fprintf (file, "GRID %d %d %f %f %f\n",
	     course->grids[i].flag,
	     course->grids[i].segment,
	     course->grids[i].x,
	     course->grids[i].y,
	     course->grids[i].dir);
  }
  fprintf (file, "NUM_CLINE %d\n", course->num_clines);
  for (i=0; i<course->num_clines; i++) {
    cline = course->clines + i;

    fprintf (file, "CLINE %d %d\n", cline->flag, cline->segment);
    fprintf (file, "%f %f\n", cline->region_a[0].c.x, cline->region_a[0].c.y);
    fprintf (file, "%f %f\n", cline->region_a[1].c.x, cline->region_a[1].c.y);
    fprintf (file, "%f %f\n", cline->region_a[3].c.x, cline->region_a[3].c.y);
    fprintf (file, "%f %f\n", cline->region_a[2].c.x, cline->region_a[2].c.y);
    fprintf (file, "%f %f\n", cline->region_b[3].c.x, cline->region_b[3].c.y);
    fprintf (file, "%f %f\n", cline->region_b[2].c.x, cline->region_b[2].c.y);
  }
  fprintf (file, "NUM_MAT %d\n", course->num_mats);
  for (i=0; i<course->num_mats; i++) {
    fprintf (file, "MAT %d %f %f %f\n",
	     course->mats[i].flag,
	     course->mats[i].friction,
	     course->mats[i].exp,
	     course->mats[i].mu);
  }
  fprintf (file, "NUM_SEG %d\n", course->num_segs);
  for (i=0; i<course->num_segs; i++) {
    fprintf (file, "SEG %d %f %f %f %f %f %f\n",
	     course->segs[i]->flag,
	     course->segs[i]->max.c.x,
	     course->segs[i]->min.c.x,
	     course->segs[i]->max.c.y,
	     course->segs[i]->min.c.y,
	     course->segs[i]->max.c.z,
	     course->segs[i]->min.c.z);
    fprintf (file, "NUM_VERT %d\n", course->segs[i]->num_verts);
    for (j=0; j<course->segs[i]->num_verts; j++) {
      fprintf (file, "%f %f %f\n",
	       course->segs[i]->verts[j].c.x,
	       course->segs[i]->verts[j].c.y,
	       course->segs[i]->verts[j].c.z);
    }
    fprintf (file, "NUM_SURF %d\n", course->segs[i]->num_surfs);
    for (j=0; j<course->segs[i]->num_surfs; j++) {
      fprintf (file, "%d %d %d %d %d\n",
	       course->segs[i]->surfs[j].flag,
	       course->segs[i]->surfs[j].mat,
	       course->segs[i]->surfs[j].p[0],
	       course->segs[i]->surfs[j].p[1],
	       course->segs[i]->surfs[j].p[2]);
    }
  }
}

static int
find_token (char *data, int *index)
{
  int i;
  int j;
  char c;

  i = next_word (data, index);

  if (data[i] != '\0') {
    for (j=0; tokens[j].str != NULL; j++) {
      if (!strncmp (data+i, tokens[j].str, tokens[j].len) &&
	  (c = data[i+tokens[j].len],
	   c == ' ' || c == '\t' || c == '\n' || c == '\0'))
      {
	i += tokens[j].len;
	*index = i;
	return tokens[j].type;
      }
    }
  }

  return TOKEN_INVALID;
}

static int
next_line (char *data, int *index)
{
  int i = *index;

  for (; data[i] != '\0' && data[i] != '\n'; i++) {}
  if (data[i] != 0) {
    i++;
  }
  *index = i;

  return i;
}


static int
next_word (char *data, int *index)
{
  int i = *index;

  while (data[i] == ' ' || data[i] == '\t' || data[i] == '\n') {
    i++;
  }

  *index = i;

  return i;
}

static char *
get_name (char *data, int *index)
{
  char *name = NULL;
  char *tmp;
  int i = *index;
  int len;

  if (data[i] == '"') {
    i++;
    tmp = strchr (data+i, '"');
    len = tmp - (data + i);
    if (len > 0) {
      name = gr_new (char, len+1);
      strncpy (name, data+i, len);
      name[len] = '\0';
    }
    i += len + 1;
  } else {
    tmp = strpbrk (data+i, " \t");
    len = tmp - (data + i);
    name = gr_new (char, len+1);
    strncpy (name, data+i, len);
    name[len] = '\0';
    i += len;
  }

  *index = i;
  return name;
}

static GrCSegment *
read_segment (GrCourse *course, char *data, int *i)
{
  GrCSegment *seg;
  int j;

  seg = gr_new (GrCSegment, 1);
  memset (seg, 0, sizeof(GrCSegment));

  sscanf (data+*i, "SEG %d %f %f %f %f %f %f",
	  &seg->flag,
	  &seg->max.c.x, &seg->min.c.x,
	  &seg->max.c.y, &seg->min.c.y,
	  &seg->max.c.z, &seg->min.c.z);
  next_line (data, i);

  sscanf (data+*i, "NUM_VERT %d", &seg->num_verts);
  next_line (data, i);
  seg->verts = gr_new (GrVertex, seg->num_verts);
  for (j=0; j<seg->num_verts; j++) {
    sscanf (data+*i, "%f %f %f",
	    &seg->verts[j].c.x, &seg->verts[j].c.y, &seg->verts[j].c.z);
    next_line (data, i);
  }

  sscanf (data+*i, "NUM_SURF %d", &seg->num_surfs);
  next_line (data, i);
  seg->surfs = gr_new (GrCSurface, seg->num_surfs);
  for (j=0; j<seg->num_surfs; j++) {
    sscanf (data+*i, "%d %d %d %d %d",
	    &seg->surfs[j].flag,
	    &seg->surfs[j].mat,
	    &seg->surfs[j].p[0],
	    &seg->surfs[j].p[1],
	    &seg->surfs[j].p[2]);
    seg->surfs[j].v[0] = seg->verts[seg->surfs[j].p[0]];
    seg->surfs[j].v[1] = seg->verts[seg->surfs[j].p[1]];
    seg->surfs[j].v[2] = seg->verts[seg->surfs[j].p[2]];
    next_line (data, i);
  }

  return seg;
}

static void
put_name (char *str, FILE *file)
{
  int i;
  fputc ('"', file);
  if (str) {
    for (i=0; str[i] != '\0'; i++) {
      if (str[i] == '"' || str[i] == '\\') {
	fputc ('\\', file);
      }
      fputc (str[i], file);
    }
  }
  fputc ('"', file);
}

static void
create_boundary (GrCSegment *seg)
{
  int i, j;
  float val;
  float xmx, xmn, ymx, ymn, zmx, zmn;
  GrSList **list;
  GrBoundary **b;

  seg->boundary = gr_boundary_new (100, seg->min.c.x - 0.1, seg->max.c.x + 0.1,
      				   (GrBoundaryDestroyCB) gr_boundary_free);
  assert (seg->boundary);

  for (i=0; i<seg->num_surfs; i++) {
    xmx = xmn = seg->verts[seg->surfs[i].p[0]].c.x;
    ymx = ymn = seg->verts[seg->surfs[i].p[0]].c.y;
    zmx = zmn = seg->verts[seg->surfs[i].p[0]].c.z;
    for (j=1; j<3; j++) {
      val = seg->verts[seg->surfs[i].p[j]].c.x;
      if (xmx < val) xmx = val;
      if (xmn > val) xmn = val;

      val = seg->verts[seg->surfs[i].p[j]].c.y;
      if (ymx < val) ymx = val;
      if (ymn > val) ymn = val;

      val = seg->verts[seg->surfs[i].p[j]].c.z;
      if (zmx < val) zmx = val;
      if (zmn > val) zmn = val;
    }

    b = (GrBoundary **) gr_boundary_register (seg->boundary, xmx, xmn);
    assert (b);

    if (!(*b)) {
      *b = gr_boundary_new (100, seg->min.c.y - 0.1, seg->max.c.y + 0.1,
	  		    (GrBoundaryDestroyCB) gr_boundary_free);
      assert (*b);
    }

    b = (GrBoundary **) gr_boundary_register (*b, ymx, ymn);
    assert (b);

    if (!(*b)) {
      *b = gr_boundary_new (100, seg->min.c.z - 0.1, seg->max.c.z + 0.1,
	  		    (GrBoundaryDestroyCB) gr_slist_free);
      assert (*b);
    }

    list = (GrSList **) gr_boundary_register (*b, zmn, zmx);
    assert (list);

    *list = gr_slist_prepend (*list, (void *) i);
  }
}

static void
initialize_surface (GrCSegment *seg)
{
  int i, j;
  GrVertex v[3], tmp;
  GrCSurface *surf;

  for (i=0; i<seg->num_surfs; i++) {
    surf = seg->surfs + i;

    v[0] = seg->verts[surf->p[0]];
    v[1] = seg->verts[surf->p[1]];
    v[2] = seg->verts[surf->p[2]];
    gr_plane_init (&surf->plane, v+0, v+1, v+2);

    for (j=0; j<3; j++) {
      gr_matrix_mult_vertex (&surf->plane.m, v+j, surf->v+j);
    }
    for (j=0; j<3; j++) {
      gr_vertex_sub (&tmp, surf->v+((j+1) % 3), surf->v+j);
      gr_vertex_cross (surf->rv+j, gr_vertex_unit_z, &tmp);
    }
  }
}

int
gr_course_check_control_line (GrCourse *course, GrCLineInfo *cinfo)
{
  int i, j;
  GrVertex v;
  GrControlLine *cline;
  float product = 0;
  GrCLineType new_res;

  if (!course || !cinfo)
    return 0;

  new_res = (cinfo->res & GR_CLINE_MASK) >> 2;

  for (i=0; i<course->num_clines; i++) {
    if (cinfo->current.pos.c.x < course->clines[i].xmin ||
	cinfo->current.pos.c.x > course->clines[i].xmax ||
	cinfo->current.pos.c.y < course->clines[i].ymin ||
	cinfo->current.pos.c.y > course->clines[i].ymax)
      continue;

    cline = course->clines + i;

    /* CHECK REGION A */
    for (j=0; j<4; j++) {
      gr_vertex_sub_xy (&v, &cinfo->current.pos, cline->region_a + j);
      product = gr_vertex_product_xy (&v, cline->rot_a + j);
      if (product < 0) {
	break;
      }
    }
    if (j == 4) {
      cinfo->res = new_res | GR_CLINE_CURRENT_A;
      cinfo->current.cline = i;
      return 1;
    }

    /* CHECK REGION B */
    for (j=0; j<4; j++) {
      gr_vertex_sub_xy (&v, &cinfo->current.pos, cline->region_b + j);
      product = gr_vertex_product_xy (&v, cline->rot_b + j);
      if (product < 0) {
	break;
      }
    }
    if (j == 4) {
      cinfo->res = new_res | GR_CLINE_CURRENT_B;
      cinfo->current.cline = i;

      if (new_res & GR_CLINE_PREV_A) {
	/* calc cross ratio */

	gr_vertex_sub_xy (&v, &cinfo->prev.pos, cline->region_b);
	cinfo->cross_ratio = product
		      / (product - gr_vertex_product_xy (&v, cline->rot_b));
      }

      return 1;
    }
  }

  cinfo->res = new_res | GR_CLINE_CURRENT_C;
  return 1;
}


void
gr_course_foreach_surface (GrCourse *course,
    			   GrVertex *center,
			   float r,
			   GrCSurfaceCallback callback,
			   void *data)
{
  GrBoundary *xboundary;
  GrBoundary *yboundary;
  GrBoundary *zboundary;
  GrCSegment *segment;
  int i, j, k, seg, surf;
  float xmax, xmin;
  float ymax, ymin;
  float zmax, zmin;
  GrSList *ylist = NULL;
  GrSList *zlist = NULL;
  GrSList *list = NULL;
  GrSList *slist = NULL;

  assert (course);
  assert (center);
  assert (callback);

  xmax = center->c.x + r;
  xmin = center->c.x - r;
  ymax = center->c.y + r;
  ymin = center->c.y - r;
  zmax = center->c.z + r;
  zmin = center->c.z - r;

  for (seg=0; seg<course->num_segs; seg++) {
    segment = course->segs[seg];
    if (xmax < segment->min.c.x || xmin > segment->max.c.x ||
        ymax < segment->min.c.y || ymin > segment->max.c.y ||
        zmax < segment->min.c.z || zmin > segment->max.c.z) {
      continue;
    }
    xboundary = segment->boundary;

    for (i=100; i>0; i--) {
      ylist = gr_boundary_check_region (xboundary, xmin, xmax, &i);

      while (ylist) {
	yboundary = (GrBoundary *) ylist->data;

	for (j=100; j>0; j--) {
	  zlist = gr_boundary_check_region (yboundary, ymin, ymax, &j);

	  while (zlist) {
	    zboundary = (GrBoundary *) zlist->data;

	    for (k=100; k>0; k--) {
	      list = gr_boundary_check_region (zboundary, zmin, zmax, &k);

	      while (list) {
		slist = list->data;
		while (slist) {
		  surf = (int) slist->data;

		  if ((*callback) (course, seg, surf, data)) {
		    goto FINISH;
		  }
		  slist = slist->next;
		}

		list = gr_slist_remove (list);
	      }
	    }
	    zlist = gr_slist_remove (zlist);
	  }
	}
	ylist = gr_slist_remove (ylist);
      }
    }
  }

FINISH:
  if (list)
    gr_slist_free (list);
  if (ylist)
    gr_slist_free (ylist);
  if (zlist)
    gr_slist_free (zlist);
}

void
gr_course_foreach_surface_xy (GrCourse *course,
			      GrVertex *pos,
			      GrCSurfaceXYCallback callback,
			      void *data)
{
  GrBoundary *xboundary;
  GrBoundary *yboundary;
  GrBoundary *zboundary;
  GrCSegment *segment;
  GrCSurface *surface;
  int i, j, k, seg, surf;
  GrSList *list = NULL, *slist;
  GrVertex cross;

  assert (course);
  assert (callback);

  for (seg=0; seg<course->num_segs; seg++) {
    segment = course->segs[seg];
    if (pos->c.x < segment->min.c.x || pos->c.x > segment->max.c.x ||
        pos->c.y < segment->min.c.y || pos->c.y > segment->max.c.y) {
      continue;
    }
    xboundary = segment->boundary;

    for (i=100; i>0; i--) {
      yboundary = gr_boundary_check (xboundary, pos->c.x, &i);

      if (!yboundary)
	return;

      for (j=100; j>0; j--) {
	zboundary = gr_boundary_check (yboundary, pos->c.y, &j);

	if (!zboundary)
	  return;

	for (k=100; k>0; k--) {
	  list = gr_boundary_check_region (zboundary,
					   segment->min.c.z,
					   segment->max.c.z, &k);

	  while (list) {
	    slist = list->data;
	    while (slist) {
	      surf = (int) slist->data;
	      surface = segment->surfs + surf;

	      if (gr_plane_get_crossing (&surface->plane, &cross,
					 pos, gr_vertex_unit_z) &&
	          gr_course_surface_check_inner (surface, &cross)) {
		if ((*callback) (course, seg, surf, cross.c.z, data)) {
		  goto FINISH;
		}
	      }
	      slist = slist->next;
	    }

	    list = gr_slist_remove (list);
	  }
	}
      }
    }
  }

FINISH:
  if (list)
    gr_slist_free (list);
}

int
gr_course_surface_check_inner (GrCSurface *surf, GrVertex *p)
{
  int i;
  GrVertex tmp;

  for (i=0; i<3; i++) {
    gr_vertex_sub (&tmp, p, surf->v+i);
    if (gr_vertex_product_xy (&tmp, surf->rv+i) < -0.05)
      return 0;
  }

  return 1;
}