#include "environment.h"

Environment::Environment() {
  env_polygon = NULL;
  extendable_pairs = new List();
  internal_polygons = new List();
  best_state = NULL;
}

Environment::Environment(Polygon *p) {
  env_polygon = p;
  extendable_pairs = new List();     
  internal_polygons = new List();
  best_state = NULL;
}

Environment::~Environment() {
  if (env_polygon) free(env_polygon);
  if (extendable_pairs) delete extendable_pairs;
  if (internal_polygons) delete internal_polygons;
  best_state = NULL;
}
 


/*
 * PursueEvader
 *
 * This function tries to find the evader by searching the state space.
 * On success, it returns 1. On failure, it returns 0.
 * On success, the resulting path is stored in the state.
 */

List *Environment::PursueEvader(State *state, void *m_start_state,
				Polygon *start_polygon) {

  List *result_list;

  

  result_list = new List();
  best_state = m_start_state;

  if (PursueStep(state, m_start_state, start_polygon, result_list)) {

    return result_list;

  } else {

    return 0;

  }
}





/*
 * PursueStep
 *
 * This is a recursive function which takes a step in the pursuit
 * algorithm.
 */

int Environment::PursueStep(State *state, void *elem, Polygon *p,
			    List *result_list) {

  REGISTER("Environment::PursueStep");

  Vertex *temp_vert;
  Segment *segs_to_cross;
  void *new_elem;
  List *steps;
  List *polys;
  void *step_elem;
  Polygon *poly_elem;

  temp_vert = new Vertex();
  temp_vert->do_copy(p->GetCenter());

  result_list->Append(temp_vert);

  if (state->Done(elem)) {
    UNREGISTER();
    return 1;
  }

  steps = new List();
  polys = new List();

  segs_to_cross = (Segment *)p->SegList()->First();
  while(segs_to_cross) {
    if (segs_to_cross->neighbor) {

      if ((new_elem = state->Move(segs_to_cross->neighbor, elem))) {
	
	if (state->Value(new_elem) < state->Value(best_state)) {
	  best_polygon = segs_to_cross->neighbor;
	  best_state = new_elem;
	}

	poly_elem = (Polygon *)polys->First();
	step_elem = steps->First();
	while(step_elem) {
	  if (state->Value(new_elem) > state->Value(step_elem))
	    break;
	  step_elem = steps->Next(step_elem);
	  poly_elem = (Polygon *)polys->Next(poly_elem);
	}
	
	
	if (step_elem) {
	  steps->InsertAfter(new_elem, step_elem);
	  polys->InsertAfter(segs_to_cross->neighbor, poly_elem);
	} else { 
	  steps->Append(new_elem);
	  polys->Append(segs_to_cross->neighbor);
	}
	
      }
    }
    segs_to_cross = (Segment *)p->SegList()->Next(segs_to_cross);
  }

  
  // Now, propagate out to them, smallest first.
  step_elem = steps->First();
  poly_elem = (Polygon *)polys->First();
  while(step_elem) {
    state->print(step_elem);

    if (PursueStep(state, step_elem, poly_elem, result_list)) {
      UNREGISTER();
      return 1;
    }

    step_elem = steps->Next(step_elem);
    poly_elem = (Polygon *)polys->Next(poly_elem);
  }
  
  result_list->Delete(temp_vert);
  delete temp_vert;
  delete steps;
  delete polys;

  UNREGISTER();
  return 0;
}



/*
 * CanSee
 *
 * This function returns true if two vertices can see each other.
 * It is used to determine the visibility regions from each
 * polygon.
 */

int Environment::CanSee(Vertex *v1, Vertex *v2) {

  REGISTER("Environment::CanSee");

  Segment *temp_seg;
  Segment *sight_seg;

  sight_seg = new Segment(v1, v2);

  temp_seg = (Segment *)env_polygon->SegList()->First();
  while(temp_seg) {
    
    if (sight_seg->IsIntersection(temp_seg, 1)) { 
      delete sight_seg;
      UNREGISTER();
      return 0;
    }
    temp_seg = (Segment *)env_polygon->SegList()->Next(temp_seg);
  }

  delete sight_seg;

  UNREGISTER();
  return 1;
}


/*
 * ComputeVisibility
 *
 * This function computes the visibility for each polygon. What it does
 * is determine if, from the polygon's center, you can see each of the
 * internal segments in the environment polygon. Add each of these to the
 * starting polygon's visible_segments list
 *
 * Next, it runs through all the internal polygons. For each, it determines
 * if all of the polygon's internal segments are visible. If so, the
 * entire polygon is visible, so add it to the starting polygon's
 * visible_polygons list.
 */

void Environment::ComputeVisibility() {
  REGISTER("Environment::ComputeVisibility");

  Polygon *p;
  Vertex *center;
  Segment *int_seg;
  Segment *visible_seg;

  // First, determine which segments can be seen.

  p = (Polygon *)internal_polygons->First();
  while(p) {
    center = p->GetCenter();

    int_seg = (Segment *)env_polygon->internal_segments->First();
    while(int_seg) {
      if (CanSee(center, int_seg->GetVertex(0)) &&
	  CanSee(center, int_seg->GetVertex(1)))
	p->visible_segments->Append(int_seg);

      int_seg = (Segment *)env_polygon->internal_segments->Next(int_seg);
    }

    p = (Polygon *)internal_polygons->Next(p);
  } 


  // Now, determine which full polygons can be seen.
  int not_found, found;
  Polygon *source_p;

  source_p = (Polygon *)internal_polygons->First();
  while(source_p) {

    p = (Polygon *)internal_polygons->First();
    while(p) {

      not_found = 0;
      int_seg = (Segment *)p->SegList()->First();
      while(int_seg) {
	
	found = 0;
	visible_seg = (Segment *)source_p->visible_segments->First();
	while(visible_seg) {
	  
	  
	  // Note the neighbor line. This is because if the seg neighbors
	  // nothing (i.e. it's on the boundary of the environment), that
	  // seg's visibility doesn't matter.
	  
	  if (visible_seg->Equals(int_seg) || 
	      (int_seg->neighbor == NULL && 
	       CanSee(source_p->GetCenter(), int_seg->GetVertex(0)) &&
	       CanSee(source_p->GetCenter(), int_seg->GetVertex(1)))) {
	    found = 1;
	    break;
	  }
	  
	  visible_seg = (Segment *)source_p->visible_segments->Next(visible_seg);
	}
	
	if (!found) {
	  not_found = 1;
	  break;
	}
	
	int_seg = (Segment *)p->SegList()->Next(int_seg);
      }
      
      if (!not_found) {
	source_p->visible_polygons->Append(p);
      }
    
      p = (Polygon *)internal_polygons->Next(p); 
    }
    
    source_p = (Polygon *)internal_polygons->Next(source_p);
  }

  UNREGISTER();
}



/*
 * PlanPath
 *
 * This function plans a path between any pair of vertices in the
 * environment polygon using the roadmap generated for that particular
 * robot.
 */


List *Environment::PlanPath(int robot, Vertex *to) {

  REGISTER("Environment::PlanPath");

  Polygon *start_polygon;
  Polygon *end_polygon;
  List *result_list;

  result_list = new List();

  start_polygon = vertex_in_which_polygon(robot_starts[robot]);
  end_polygon = vertex_in_which_polygon(to);

  if (PathStep(robot, start_polygon, end_polygon, result_list)) {
    result_list->InsertBefore(robot_starts[robot], 
			      (Vertex *)result_list->First());
    result_list->Append(to);
    UNREGISTER();
    return result_list;
  } else {
    ERROR("Couldn't plan path!");
  }
  UNREGISTER();
  return NULL;
}


int Environment::PathStep(int robot, Polygon *curr_polygon, 
			  Polygon *dest_polygon, 
			  List *result_list) {

  REGISTER("Environment::PathStep");

  int i, num_next;
  Polygon **nexts;
  Vertex *temp_vert;

  temp_vert = new Vertex();
  temp_vert->do_copy(curr_polygon->GetCenter());

  result_list->Append(temp_vert);

  if (curr_polygon == dest_polygon) {
    UNREGISTER();
    return 1;
  }

  nexts = shortest_path_roadmaps[robot]->AllNext(curr_polygon, &num_next);
  for (i = 0; i < num_next; i++) {
    if (PathStep(robot, nexts[i], dest_polygon, result_list)) {
      UNREGISTER();
      return 1;
    }
  }
  
  result_list->Delete(temp_vert);
  delete temp_vert;
  UNREGISTER();
  return 0;
}


int Environment::InPolygon(Segment *s, Polygon *p) {
  REGISTER("Environment::InPolygon");
  
  Segment *temp;

  if (!p || !s) {
    UNREGISTER();
    return 0;
  }

  temp = (Segment *)p->SegList()->First();
  while(temp) {
    if (temp->Equals(s)) {
      UNREGISTER();
      return 1;
    }
    temp = (Segment *)p->SegList()->Next(temp);
  }
   
  UNREGISTER();

  return 0;
}




/*
 * GenerateCells
 *
 * This function decomposes the polygonal environment into cells
 * which maintain constant edge visibility. 
 * 
 * This is done by first extending all edge segments until they 
 * run into another edge.
 *
 * Then, certain other pairs of vertices are also extended.
 *
 * Finally, the environment edges, together with the newly found
 * edges, are used to create cells which can then be traversed.
 */

void Environment::GenerateCells(int m_num_robots) {
  REGISTER("Environment::GenerateCells");
  
  num_robots = m_num_robots;
  robot_starts = (Vertex **)malloc(sizeof(Vertex *) * num_robots);

  precompute_external_hits();
  generate_extension_queue();
  generate_internal_segments();
  link_roadmap();

  shortest_path_roadmaps = (Roadmap **)malloc(sizeof(Roadmap *) * num_robots);

  UNREGISTER();
}




/*
 * vertex_in_which_polygon
 *
 * This function assumes that the polygons are convex (the internal ones
 * are). It figures out which polygon a vertex is in. This is done by
 * connecting the vertex to a vertex in each polygon and then figuring out
 * if it triangulates into the polygon.
 */

Polygon *Environment::vertex_in_which_polygon(Vertex *v) {

  REGISTER("Environment::vertex_in_which_polygon");

  Polygon *temp_polygon;
  Segment *temp_seg = NULL;
  Segment *poly_seg;
  int found;

  temp_polygon = (Polygon *)internal_polygons->First();
  while(temp_polygon) {

    temp_seg = new Segment(v, temp_polygon->GetCenter());

    found = 0;
    poly_seg = (Segment *)temp_polygon->SegList()->First();
    while(poly_seg) {

      if (temp_seg->Intersection(poly_seg, 3)) {
	found = 1;
	break;
      } 

      poly_seg = (Segment *)temp_polygon->SegList()->Next(poly_seg);
    }
    
    if (!found) break;

    if (temp_seg) delete temp_seg;
    temp_seg = NULL;
    temp_polygon = (Polygon *)internal_polygons->Next(temp_polygon);
  }

  if (!temp_polygon) ERROR("Couldn't figure out which polygon a vertex is in!");

  if (temp_seg) delete temp_seg;

  UNREGISTER();
  return temp_polygon;

}



/* 
 * ComputeShortestPaths
 *
 * This function first figures out which polygon the robot is starting in.
 * It then uses Djikstra's algorighm to make a custom roadmap for this
 * robot.
 */


void Environment::ComputeShortestPaths(Vertex *start, int num) {

  REGISTER("Environment::ComputeShortestPaths");

  Polygon *start_polygon;
  Polygon **nexts;
  fixedpt *temp_distances;
  int num_next;

  Polygon **visited;
  Polygon **candidate;
  Polygon **link_froms;
  Polygon *curr_link_from;
  fixedpt *distances;

  int num_visited = 0;
  int num_candidate = 0;

  int i, j;
  int found;
  int best;
  fixedpt best_dist;

  Vertex *v_copy;
  v_copy = new Vertex();
  v_copy->do_copy(start);
  robot_starts[num] = v_copy;
  
  start_polygon = vertex_in_which_polygon(start);
  visited = (Polygon **)malloc(sizeof(Polygon *) * roadmap->Size());
  candidate = (Polygon **)malloc(sizeof(Polygon *) * roadmap->Size());
  distances = (fixedpt *)malloc(sizeof(fixedpt) * roadmap->Size());
  link_froms = (Polygon **)malloc(sizeof(Polygon *) * roadmap->Size());

  shortest_path_roadmaps[num] = new Roadmap();
  shortest_path_roadmaps[num]->copy_polygons(roadmap);
  
  candidate[0] = start_polygon;
  distances[0] = 0;
  link_froms[0] = NULL;
  num_candidate++;

  
  while(num_candidate) {

    best = -1;
    best_dist = LARGE_INT;
    for (i = 0; i < num_candidate; i++) {
      if (distances[i] < best_dist) {
	best = i;
	best_dist = distances[i];
      }
    }
     

    curr_link_from = link_froms[best];
    visited[num_visited] = candidate[best];
    candidate[best] = candidate[num_candidate-1];
    link_froms[best] = link_froms[num_candidate-1];
    distances[best] = distances[num_candidate-1];
    num_candidate--;

    nexts = roadmap->AllNext(visited[num_visited], &num_next);
    temp_distances = roadmap->AllDistances(visited[num_visited]);

    if (curr_link_from != NULL) {
      shortest_path_roadmaps[num]->Link(curr_link_from, visited[num_visited]);
    }

    for (i = 0; i < num_next; i++) {
      found = 0;

      // Check to be sure I haven't already visited this one.
      for (j = 0; j < num_visited; j++) {
	if (visited[j] == nexts[i]) {
	  found = 1;
	  break;
	}
      }

      // Check to be sure it's not already queued.
      for (j = 0; j < num_candidate; j++) {
	if (candidate[j] == nexts[i]) {
	  found = 1;
	  break;
	}
      }

      if (!found) {
	candidate[num_candidate] = nexts[i];
	distances[num_candidate] = temp_distances[i] + best_dist;
	link_froms[num_candidate] = visited[num_visited];
	num_candidate++;
      }
    }

    num_visited++;
  }

  free(visited);
  free(candidate);
  free(distances);
  free(link_froms);


#ifdef PART1_DEBUG
  printf("Links for Robot %d", num);
  shortest_path_roadmaps[num]->print();
#endif 

  UNREGISTER();

}



/*
 * link_roadmap
 *
 * This function creates the Roadmap structure, and links all the polygons
 * within it. Also, compute the polygons' centroids and do some other
 * bookkeeping tasks before part 1 is completed. 
 *
 */

void Environment::link_roadmap() {

  Polygon *temp_polygon;
  Segment *temp_seg;

  roadmap = new Roadmap();

  temp_polygon = (Polygon *)internal_polygons->First();
  while(temp_polygon) {
    roadmap->Insert(temp_polygon);    
    temp_polygon->ComputeCenter();
    temp_polygon = (Polygon *)internal_polygons->Next(temp_polygon);
  }

  temp_polygon = (Polygon *)internal_polygons->First();
  while(temp_polygon) {

    temp_seg = (Segment *)temp_polygon->SegList()->First();
    while(temp_seg) {
      
      if (temp_seg->neighbor)
	roadmap->Link(temp_polygon, temp_seg->neighbor);

      temp_seg = (Segment *)temp_polygon->SegList()->Next(temp_seg);
    }

    temp_polygon = (Polygon *)internal_polygons->Next(temp_polygon);
  }

#ifdef PART1_DEBUG

  roadmap->print();

#endif
  
}


/*
 * split_polygon
 *
 * This function figures out which polygon the segment (v1, v2) splits.
 * Then, it generates two polygons, based on this split, and deletes
 * the original. Further, it creates a segment (v1, v2) and sticks
 * it into each polygon and into the internal_segments list.
 *
 * This function assumes that the two vertices both exist as vertices in
 * polygons (i.e., it assumes that the vertex of the hit has been added
 * to the segment it hit, if necessary).
 *
 * The reason this function must search for the appropriate polygon to
 * split is because of a particularly nasty special case set where the
 * hit occurs on an already-existing vertex. It turns out to be easier
 * just to search for the polygon in that case.
 */

void Environment::split_polygon(Polygon *orig, Polygon *sub1, Polygon *sub2, 
				Segment *s) {

  REGISTER("Environment::split_polygon");

  Segment *seg0, *seg1, *temp_seg, *reverse_seg;

  seg0 = (Segment *)orig->SegList()->First();
  while(1) {
    if (seg0->GetVertex(0)->Equals(s->GetVertex(0))) {
      break;
    }
    seg0 = (Segment *)orig->Next(seg0);
  }

  seg1 = (Segment *)orig->SegList()->First();
  while(1) {
    if (seg1->GetVertex(0)->Equals(s->GetVertex(1))) {
      break;
    }
    seg1 = (Segment *)orig->Next(seg1);
  }
  

  // So now seg0 holds the segment in the polygon which has vertex 0
  // as a jump-off point, and similarly for seg1 and its vertex 0.
  
  temp_seg = seg0;
  while(1) {
    sub1->SegList()->Append(temp_seg);
    temp_seg = (Segment *)orig->Next(temp_seg);
    if (temp_seg == seg1) break;
  }
  
  while(1) {
    sub2->SegList()->Append(temp_seg);
    temp_seg = (Segment *)orig->Next(temp_seg);
    if (temp_seg == seg0) break;
  }

  temp_seg = new Segment();
  temp_seg->do_copy(s);

  reverse_seg = new Segment();
  reverse_seg->do_copy(s);
  reverse_seg->Flip();


  // If the new segment points out of the same vertex the old one points
  // out of, then that means I need to reverse the new segment for that
  // polygon.

  if (temp_seg->GetVertex(0)->Equals(seg0->GetVertex(0))) {
    temp_seg->neighbor = sub1;
    reverse_seg->neighbor = sub2;
    sub2->SegList()->Append(temp_seg);
    sub1->SegList()->Append(reverse_seg);
  } else {
    temp_seg->neighbor = sub2;
    reverse_seg->neighbor = sub1;
    sub1->SegList()->Append(temp_seg);
    sub2->SegList()->Append(reverse_seg);
  }



  Segment *ts_neighbor;
  
  temp_seg = (Segment *)sub1->SegList()->First();
  while(temp_seg) {

    if (temp_seg->neighbor && !temp_seg->Equals(reverse_seg)) {
      ts_neighbor = (Segment *)temp_seg->neighbor->SegList()->First();
      while(ts_neighbor) {
	if (ts_neighbor->neighbor == orig) 
	  ts_neighbor->neighbor = sub1;
	ts_neighbor = (Segment *)temp_seg->neighbor->SegList()->
	  Next(ts_neighbor);
      }
      
    }
    
    temp_seg = (Segment *)sub1->SegList()->Next(temp_seg);
  }
  

  Segment *ts2;

  temp_seg = (Segment *)sub2->SegList()->First();
  while(temp_seg) {

    if (temp_seg->neighbor && !temp_seg->Equals(reverse_seg)) {
      ts_neighbor = (Segment *)temp_seg->neighbor->SegList()->First();
      while(ts_neighbor) {
	if (ts_neighbor->neighbor == orig)
	  ts_neighbor->neighbor = sub2;
	else if (ts_neighbor->neighbor == sub1) {

	  ts2 = (Segment *)sub2->SegList()->First();
	  while(ts2) {

	    if (ts_neighbor->Equals(ts2)) {
	      ts_neighbor->neighbor = sub2;
	      break;
	    } 

	    ts2 = (Segment *)sub2->SegList()->Next(ts2);
	  }
	}

	ts_neighbor = (Segment *)temp_seg->neighbor->SegList()->
	  Next(ts_neighbor);
      }
      
    }
    
    temp_seg = (Segment *)sub2->SegList()->Next(temp_seg);
  }


  UNREGISTER();
}



/*
 * generate_subpolygons
 *
 * This function extends the edges that need extending and generates a
 * list of internal subpolygons.
 */

void Environment::generate_subpolygons() {

  REGISTER("Environment::generate_subpolygons");


  UNREGISTER();
}




/* 
 * which_polygon
 *
 * This function figures out which polygon is spanned by the segment s.
 */

Polygon *Environment::which_polygon(Segment *s) {

  REGISTER("Environment::which_polygon");

  Polygon *temp_polygon;
  Segment *polygon_seg;
  int num_found0, num_found1;

  temp_polygon = (Polygon *)internal_polygons->First();
  while(temp_polygon) {

    num_found0 = num_found1 = 0;
    polygon_seg = (Segment *)temp_polygon->SegList()->First();
    while(polygon_seg) {

      if (polygon_seg->GetVertex(0)->Equals(s->GetVertex(0)))
	num_found0++;

      if (polygon_seg->GetVertex(0)->Equals(s->GetVertex(1)))
	num_found1++;

      polygon_seg = (Segment *)temp_polygon->SegList()->Next(polygon_seg);
    }

    // Check if both vertices of the splitting segment are found
    // in two segments of the polygon, each. Then I have the right
    // polygon, so return it.

    if (num_found0 > 0 && num_found1 > 0) {
      UNREGISTER();
      return temp_polygon;      
    }

    temp_polygon = (Polygon *)internal_polygons->Next(temp_polygon);
  }

  UNREGISTER();
  return NULL;
}



/* generate_internal_segments
 *
 * This function generates the internal segments for the environment 
 * polygon. This means that it splits any internal segments which cross.
 *
 */

void Environment::generate_internal_segments() {
  REGISTER("Environment::generate_internal_segments");

  vertex_pair *v_pair;
  vertex_pair *temp_pair;
  Vertex *vert, *new_env_vert;

  Segment *new_seg, *new_env_seg1, *new_env_seg2;
  int i, j;
  int dontnext = 0;


  // This first loop generates all the vertices on the environment
  // polygon which come from the internal segments cutting across it.

  v_pair = (vertex_pair *)extendable_pairs->First();
  while(v_pair) {
    
    for (i = 0; i < 2; i++) {

      if (v_pair->toward[i]) {

	if (i == 0) vert = v_pair->v1;
	else vert = v_pair->v2;

	new_env_vert = v_pair->seg->Intersection(v_pair->seg_hit[i], 0);
	new_env_vert->rank = 3;
	new_seg = new Segment(vert, new_env_vert);
	vert->rank++;

	env_polygon->internal_segments->Append(new_seg);
	
	new_env_seg1 = new Segment(v_pair->seg_hit[i]->GetVertex(0), 
				   new_env_vert);

	new_env_seg2 = new Segment(new_env_vert, 
				   v_pair->seg_hit[i]->GetVertex(1));
	
	env_polygon->SegList()->InsertAfter(new_env_seg1, v_pair->seg_hit[i]); 
	env_polygon->SegList()->InsertAfter(new_env_seg2, new_env_seg1);
	env_polygon->SegList()->Delete(v_pair->seg_hit[i]);

	// If any other internal segments hit the old one, switch them to the
	// new one.

	temp_pair = (vertex_pair *)extendable_pairs->First();
	while(temp_pair) {
	  if (temp_pair != v_pair) {
	    for (j = 0; j < 2; j++) {
	      if (temp_pair->toward[j]) {
		if (temp_pair->seg_hit[j] == v_pair->seg_hit[i]) {
		  
		  if (temp_pair->seg->IsIntersection(new_env_seg1, 0)) {
		    temp_pair->seg_hit[j] = new_env_seg1;
		  } else {
		    temp_pair->seg_hit[j] = new_env_seg2;
		  }

		}
	      }
	    }
	  }
	  temp_pair = (vertex_pair *)extendable_pairs->Next(temp_pair);
	}

	delete v_pair->seg_hit[i];
      }

    }

    v_pair = (vertex_pair *)extendable_pairs->Next(v_pair);
  }



#ifdef PART1_DEBUG
  env_polygon->print();
#endif

  Segment *seg1, *seg2;
  Segment *new_seg1, *new_seg2, *new_seg3, *new_seg4;
  Segment *temp_seg;
  Vertex *split_vert;

  Polygon *new_polygon1, *new_polygon2;
  Polygon *curr_polygon;
  
  // First, copy the environment polygon.

  new_polygon1 = new Polygon();
  new_polygon1->do_copy(env_polygon);
  new_polygon2 = NULL;
  
  internal_polygons->Append(new_polygon1);

#ifdef PART1_DEBUG
  printf("Internal Segments:\n");
  seg1 =  (Segment *)env_polygon->internal_segments->First();
  while(seg1) {
    seg1->print();
    seg1 = (Segment *)env_polygon->internal_segments->Next(seg1);
  }
  printf("\n");
#endif


  seg1 = (Segment *)env_polygon->internal_segments->First();
  while(seg1) {

    if (!InPolygon(seg1, new_polygon1) && !InPolygon(seg1, new_polygon2)) {

      curr_polygon = which_polygon(seg1);
      if (curr_polygon == NULL) {

#ifdef PART1_DEBUG
	printf("Split Seg:\n");
	seg1->print();
	
	printf("\n");
#endif
	seg1 = (Segment *)env_polygon->internal_segments->Next(seg1);
	continue;
      }
      
      
#ifdef PART1_DEBUG
      printf("Found a polygon to split.\n");
#endif
      

      new_polygon1 = new Polygon();
      new_polygon2 = new Polygon();

      // Split on seg1. That means I need to figure which segments are in
      // the outer polygon that need to be added to each of the new ones.
      
      split_polygon(curr_polygon, new_polygon1, new_polygon2, seg1);
      

#ifdef PART1_DEBUG
      printf("Splitting polygon:\n\n");
      printf("Split Seg:\n");
      seg1->print();
      
      
      printf("\nFull Polygon:\n");
      curr_polygon->print();

      printf("\nSplit 1:\n");
      new_polygon1->print();
    
      printf("\nSplit 2:\n");
      new_polygon2->print();
      
      printf("\n");
#endif

      internal_polygons->Delete(curr_polygon);
      delete curr_polygon;
      internal_polygons->Append(new_polygon1);
      internal_polygons->Append(new_polygon2);

    }


    seg2 = (Segment *)env_polygon->internal_segments->Next(seg1);
    while(seg2) {
      
      if ((split_vert = seg1->Intersection(seg2, 3)) != NULL) {

	split_vert->rank = 4;
	
	new_seg1 = new Segment(seg1->GetVertex(0), split_vert);
	new_seg2 = new Segment(split_vert, seg1->GetVertex(1));

	env_polygon->internal_segments->InsertAfter(new_seg1, seg1);
	env_polygon->internal_segments->InsertAfter(new_seg2, new_seg1);


	// Split the segment in the subpolygons.

	temp_seg = (Segment *)new_polygon1->SegList()->First();
	while(temp_seg) {
	  if (temp_seg->Equals(seg1)) break;
	  temp_seg = (Segment *)new_polygon1->SegList()->Next(temp_seg);
	}

#ifdef PART1_DEBUG
	seg1->print();
#endif

	if (!temp_seg) {

#ifdef PART1_DEBUG

	  seg2 = (Segment *)env_polygon->internal_segments->Next(seg1);
	  while(seg2) {

	    seg2->print();
	    seg2 = (Segment *)env_polygon->internal_segments->Next(seg2);
	  }

#endif
	  ERROR("Couldn't find seg to split in new_polygon1");

	}	

	new_env_seg1 = new Segment();
	new_env_seg1->do_copy(new_seg1);
	new_env_seg1->neighbor = new_polygon2;

	new_env_seg2 = new Segment();
	new_env_seg2->do_copy(new_seg2);
	new_env_seg2->neighbor = new_polygon2;

	
	// If this seg was flipped around, flip its subcomponents.
	if (!temp_seg->GetVertex(0)->Equals(seg1->GetVertex(0))) {
	  new_env_seg1->Flip();
	  new_env_seg2->Flip();
	  new_polygon1->SegList()->InsertAfter(new_env_seg2, temp_seg); 
	  new_polygon1->SegList()->InsertAfter(new_env_seg1, new_env_seg2);
	} else {
	
	  new_polygon1->SegList()->InsertAfter(new_env_seg1, temp_seg); 
	  new_polygon1->SegList()->InsertAfter(new_env_seg2, new_env_seg1);
	}

	new_polygon1->SegList()->Delete(temp_seg);
	

	// Now do the second new polygon.
	  
	temp_seg = (Segment *)new_polygon2->SegList()->First();
	while(temp_seg) {
	  if (temp_seg->Equals(seg1)) break;
	  temp_seg = (Segment *)new_polygon2->SegList()->Next(temp_seg);
	}
	
	if (!temp_seg) ERROR("Couldn't find seg to split in new_polygon2");
	
	new_env_seg1 = new Segment();
	new_env_seg1->do_copy(new_seg1);
	new_env_seg1->neighbor = new_polygon1;

	new_env_seg2 = new Segment();
	new_env_seg2->do_copy(new_seg2);
	new_env_seg2->neighbor = new_polygon1;
	
	if (!temp_seg->GetVertex(0)->Equals(seg1->GetVertex(0))) {
	  new_env_seg1->Flip();
	  new_env_seg2->Flip();
	  new_polygon2->SegList()->InsertAfter(new_env_seg2, temp_seg); 
	  new_polygon2->SegList()->InsertAfter(new_env_seg1, new_env_seg2);
	} else {
	  new_polygon2->SegList()->InsertAfter(new_env_seg1, temp_seg); 
	  new_polygon2->SegList()->InsertAfter(new_env_seg2, new_env_seg1);
	}

	new_polygon2->SegList()->Delete(temp_seg);


      // Done with this part. Now, propagate the split in the
      // second segment.

	
	new_seg3 = new Segment(seg2->GetVertex(0), split_vert);
	new_seg4 = new Segment(split_vert, seg2->GetVertex(1));

	
	env_polygon->internal_segments->InsertAfter(new_seg3, seg2);
	env_polygon->internal_segments->InsertAfter(new_seg4, new_seg3);

	temp_seg = seg1;

	seg1 = new_seg1;
	dontnext = 1;


	env_polygon->internal_segments->Delete(temp_seg);
	delete temp_seg;

	temp_seg = seg2;
	env_polygon->internal_segments->Delete(temp_seg);
	delete temp_seg;	
	
	seg2 = NULL;
    
      } else {
	seg2 = (Segment *)env_polygon->internal_segments->Next(seg2);
      }
    }

    if (!dontnext) {
      seg1 = (Segment *)env_polygon->internal_segments->Next(seg1);
    }
    else dontnext = 0;
  } 


#ifdef PART1_DEBUG

  seg1 = (Segment *)env_polygon->internal_segments->First();
  while(seg1) {
    seg1->print();
    seg1 = (Segment *)env_polygon->internal_segments->Next(seg1);
  } 
#endif


  UNREGISTER();
}




/* 
 * generate_extension_queue
 *
 * This function figures out which vertex pairs should be extended in
 * which directions. Also, for each pair of vertices
 * to be extended, it stores the location where that segment will
 * terminate.
 */

void Environment::generate_extension_queue() {
  REGISTER("Environment::generate_extension_queue");

  int j, k;
  int hits[2];
  fixedpt distance[2];
  fixedpt best_distance[2];
  Vertex *hit;
  Segment *my_seg;
  Vertex *my_verts[2];

  vertex_pair *v_pair;
  int closer, is_best;

  v_pair = (vertex_pair *)extendable_pairs->First();
  while(v_pair) {

    v_pair->seg_hit[0] = NULL;
    v_pair->seg_hit[1] = NULL;

    hits[0] = env_polygon->segment_in_triangle(v_pair->v1, v_pair->seg);
    hits[1] = env_polygon->segment_in_triangle(v_pair->v2, v_pair->seg);

    my_seg = v_pair->seg;
    my_verts[0] = v_pair->v1;
    my_verts[1] = v_pair->v2;
    best_distance[0] = best_distance[1] = FIXEDPT(LARGE_INT); // a big number.

    for (j = 0; j < num_env_segs; j++) {

      if ((hit = v_pair->external_hits[j]) != NULL) {


	// Now figure out where the extension will terminate, if it
	// terminates.

	for (k = 0; k < 2; k++) {
	  distance[k] = my_verts[k]->Distance(hit);
	  if (distance[k] < best_distance[k]) {
	    best_distance[k] = distance[k];
	    v_pair->termination[k] = hit;
	    
	  }
	}

	if (distance[0] < distance[1]) {
	  closer = 0;
	} else {
	  closer = 1;
	}

	if (distance[closer] == best_distance[closer]) {
	  is_best = 1;
	} else {
	  is_best = 0;
	}

	// I want to add up the number of other segments hit by each
	// extension in each direction. By definition, hits[0] is
	// the hits going from 0 toward 1, and vice versa for hits[1].

	if (is_best) {
	  v_pair->seg_hit[closer] = env_segs[j];
	 }
	
	hits[closer] ++;
      }

    }
    
    // If an extension hit an _odd_ number of other segments, then it's
    // an extension I want to consider, if its counterpart also
    // hit an odd number, or it's a member of a segment.

    for (k = 0; k < 2; k++) {
      if (hits[k] % 2 && v_pair->seg_hit[k] && 
	  ((hits[(k?0:1)] % 2 && v_pair->seg_hit[k?0:1]) || 
	   env_polygon->Contains(v_pair->v1, v_pair->v2))) {
	v_pair->toward[k] = 1;
      } else {
	v_pair->toward[k] = 0;
      }
    }

    v_pair = (vertex_pair *)extendable_pairs->Next(v_pair);
  }

#ifdef PART1_DEBUG
  printf("\nDefinite Boundary Hits:\n\n");

  {
    v_pair = (vertex_pair *)extendable_pairs->First();
    while(v_pair) {
      for (k = 0; k < 2; k++) {
	if (v_pair->toward[k]) {

	  if (v_pair->seg_hit[k] != HIT_ON_VERTEX) {
	    printf("V-Pair (%.3f, %.3f) - (%.3f, %.3f) => (%.3f, %.3f) - (%.3f, %.3f)\n",
		   FLOAT(v_pair->v1->x),
		   FLOAT(v_pair->v1->y),
		   FLOAT(v_pair->v2->x),
		   FLOAT(v_pair->v2->y),
		   FLOAT(v_pair->seg_hit[k]->GetVertex(0)->x),
		   FLOAT(v_pair->seg_hit[k]->GetVertex(0)->y),
		   FLOAT(v_pair->seg_hit[k]->GetVertex(1)->x),
		   FLOAT(v_pair->seg_hit[k]->GetVertex(1)->y)
		   );
	    fflush(stdout);
	  } else {
	    printf("V-Pair (%.3f, %.3f) - (%.3f, %.3f) did hit on vertex.\n",
		   FLOAT(v_pair->v1->x),
		   FLOAT(v_pair->v1->y),
		   FLOAT(v_pair->v2->x),
		   FLOAT(v_pair->v2->y)
		   );
	    fflush(stdout);
	  }
	}
      }
      v_pair = (vertex_pair *)extendable_pairs->Next(v_pair);
    }
  }
#endif /* PART1_DEBUG */

  UNREGISTER();

}




/* 
 * precompute_external_hits
 *
 * This function runs through all pairs of vertices in the environment polygon.
 *
 * If the pair of vertices forms a segment in the environment polygon, or
 * the two vertices can be connected without intersecting any segments
 * in the environment polygon, then we say they are "extendable".
 *
 * This function first computes which pairs of vertices are extendable.
 * Then, it computes all the nontrivial intersections between extending
 * the segments formed by these pairs of vertices and other segments in
 * the environment polygon.
 *
 * This data is used first to determine which segments should be extended
 * and then to determine where those extended segments should terminate.
 *
 */

void Environment::precompute_external_hits() {

  REGISTER("Environment::precompute_external_hits");

  int i, j, k;
  int found_int;
  vertex_pair *v_pair;
  Segment *temp_seg;

  num_env_segs = env_polygon->NumSegments();

#ifdef PART1_DEBUG
  printf("Potential boundary hits:\n");
#endif /* PART1_DEBUG */


  // Get a copy of all of the segments in the environment polygon
  // for use.

  env_segs = (Segment **)malloc(sizeof(Segment *) * num_env_segs);
  for (i = 0; i < num_env_segs; i++) {
    env_segs[i] = env_polygon->SegmentNum(i);
  }

  // Now, I want to get all of the vertices in the environment polygon.
  // To do this, it's sufficient to take the first vertex from each
  // segment.    

  env_verts = (Vertex **)malloc(sizeof(Vertex *) * num_env_segs);
  for (i = 0; i < num_env_segs; i++) {
    env_verts[i] = env_segs[i]->GetVertex(0);
  }

  // Now, figure out which pairs of non-consecutive vertices can see
  // each other. Consecutive vertices are part of the same segment, so
  // we process them automatically.

  for (i = 0; i < num_env_segs; i++) {
    for (j = i+1; j < num_env_segs; j++) {

      v_pair = (vertex_pair *)malloc(sizeof(vertex_pair));
      v_pair->v1 = env_verts[i];
      v_pair->v2 = env_verts[j];
      v_pair->external_hits = (Vertex **)malloc(sizeof(Vertex *) * num_env_segs);
      temp_seg = new Segment(env_verts[i], env_verts[j]);
      v_pair->seg = temp_seg;

      for (k = 0; k < num_env_segs; k++) {

	// If the current segment contains one of the vertices, then no
	// external hit is possible on this segment. Set ex_hits to NULL here.
	if (env_segs[k]->Contains(v_pair->v1) || 
	    env_segs[k]->Contains(v_pair->v2)) {       
	  v_pair->external_hits[k] = NULL;
	} else {
	  v_pair->external_hits[k] = 
	    v_pair->seg->Intersection(env_segs[k], 0);
	}
      }
	
      // Consecutive vertices:
      
      if ((j == i+1) || (i == 0 && j == num_env_segs-1)) {	  

	extendable_pairs->Append(v_pair);
      } 
      
      // Non-consecutive vertices:
      else {
	temp_seg = new Segment(env_verts[i], env_verts[j]);
	
	// Can these vertices see each other?
	
	found_int = 0;
	for (k = 0; k < num_env_segs; k++) {
	  if (temp_seg->IsIntersection(env_segs[k], 1)) {
	      found_int = 1;
	      break;
	  }
	}
	
	// No intersection found. This may be an extendable pair.
	if (!found_int) {
	  
	  extendable_pairs->Append(v_pair);   

	} else { 
	  delete temp_seg;
	  free(v_pair->external_hits);
	  free(v_pair);
	}
      } 
    }
  }

UNREGISTER();
}