#include "state.h"

State::State(int m_num_polygons, List *m_internal_segments,
	     List *m_internal_polygons, int m_num_robots) {

  states_seen = new List();
  num_polygons = m_num_polygons;
  internal_segments = m_internal_segments;
  internal_polygons = m_internal_polygons;

  num_robots = m_num_robots;

  if (num_robots > 1) {
    robot_polygons = (Polygon **)malloc(sizeof(Polygon *) * (num_robots - 1));
    robot_poly_nums = (int *)malloc(sizeof(int) * (num_robots - 1));
    robot_poly_nums[0] = -1;
  }
}

State::~State() {
  if (states_seen) delete states_seen;
}



state_elem *State::new_elem() {
  REGISTER("State:new_elem");
  
  state_elem *new_elem;

  new_elem = (state_elem *)malloc(sizeof(state_elem));
  new_elem->location = NULL;
  new_elem->poly_states = (int *)malloc(sizeof(int) * num_polygons);

  UNREGISTER();
  return new_elem;
}


void *State::StartElem(Polygon *p) {
  REGISTER("State::StartElem");
  
  int i;
  state_elem *elem = new_elem();
  for (i = 0; i < num_polygons; i++) {
    elem->poly_states[i] = 1;
  }
  elem->location = p;

  UNREGISTER();
  return elem;
}



int State::get_poly_num(Polygon *p) {

  REGISTER("State::get_poly_num");

  Polygon *t;
  int i;

  t = (Polygon *)internal_polygons->First();
  i = 0;
  while(t) {

    if (p == t) break;

    t = (Polygon *)internal_polygons->Next(t);
    i++;
  }

  if (!t) ERROR("Couldn't get polygon number!");

  UNREGISTER();

  return i;
}



int State::Done(void *m_elem) {

  state_elem *elem;
  int i;

  elem = (state_elem *)m_elem;

  for (i = 0; i < num_polygons; i++) {
    if (elem->poly_states[i] == 1) return 0;
  }

  return 1;
}


void *State::Move(Polygon *p, void *m_prev) {
  REGISTER("State::Move");
  
  // First, compute the new state based on the previous state
  // and the polygon I'm entering.

  int found = 0;
  int found_propagator_seg = 0;
  state_elem *prev = (state_elem *)m_prev;

  state_elem *elem = new_elem();
  elem->location = p;
  memcpy(elem->poly_states, prev->poly_states, num_polygons * sizeof(int));

  List *polygon_list;
  Polygon *temp_polygon;
  Polygon *vis_polygon;

  Segment *polygon_seg;
  Segment *vis_seg;

  int r;
  Polygon *r_check;
  int r_found;
  

  // Make a list of all the polygons in the environment.

  polygon_list = new List();
  temp_polygon = (Polygon *)internal_polygons->First();
  while(temp_polygon) {
    polygon_list->Append(temp_polygon);
    temp_polygon = (Polygon *)internal_polygons->Next(temp_polygon);
  }


  /*
   * The idea here is to iterate through all the polygons in the environemnt,
   * looking for ones I can't see. For each one I can't see (all of), I
   * check for a segment in there that is a "propagator". This is a 
   * segment that I can't see that borders a polygon of value 1. 
   * If the polygon has a propagator, then it gets set to 1. 
   *
   * In addition, polygons on the other side of the ones the propagator
   * sets get appended to the list to make sure that the changes propagate
   * out.
   */

  /* For multiple robots, if the polygon that I want to change is guarded by
   * one of the robots then don't change it.
   */
  
  temp_polygon = (Polygon *)polygon_list->First();
  while(temp_polygon) {

    found = 0;
    found_propagator_seg = 0;

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

      if (vis_polygon == temp_polygon) {
	found = 1; 
	break;
      }

      vis_polygon = (Polygon *)p->visible_polygons->Next(vis_polygon);
    }


    if (!found) {
      
      polygon_seg = (Segment *)temp_polygon->SegList()->First();
      while(polygon_seg) {
	
	if (polygon_seg->neighbor) {

	  found = 0;
	  vis_seg = (Segment *)p->visible_segments->First();
	  while(vis_seg) {
	    
	    if (vis_seg->Equals(polygon_seg)) {
	      found = 1;
	      break;
	    }
	    
	    vis_seg = (Segment *)p->visible_segments->Next(vis_seg);
	  }
	  

	  // This means it's a seg in a polygon I can't see all of
	  // to where I can't see the seg. Check if its bit is set
	  // to 1.
	  if (!found) { 

	      if (elem->poly_states[get_poly_num(polygon_seg->neighbor)]) {
		found_propagator_seg = 1;
		break;
	      }
	    }
	  }
	
	if (found_propagator_seg) break;
      
	polygon_seg = (Segment *)temp_polygon->SegList()->Next(polygon_seg);
      }
    }
    
    
    // I found a seg that can propagate to this polygon. 
    // Set this polygon to 1.
   
    if (found_propagator_seg) {
      
      // Check to make sure this isn't a polygon guarded by
      // a robot.
      

      r_found = 0;

      for (r = 0; r < num_robots-1; r++) {
	r_check = (Polygon *)robot_polygons[r]->visible_polygons->First();
	while(r_check) {

	  if (r_check == temp_polygon) {
	    r_found = 1;
	    break;
	  }

	  r_check = (Polygon *)robot_polygons[r]->visible_polygons->Next(r_check);
	}
	  
      }

      if (!r_found)
	elem->poly_states[get_poly_num(temp_polygon)] = 1;
		  
	
    }
    
    temp_polygon = (Polygon *)polygon_list->Next(temp_polygon);
  }
  
  delete polygon_list;
  

  /*
   * Now, iterate through a list of all the polygons I can see.
   * Set them all to zero.
   */

  temp_polygon = (Polygon *)p->visible_polygons->First();
  while(temp_polygon) {
    
    elem->poly_states[get_poly_num(temp_polygon)] = 0;
    
    temp_polygon = (Polygon *)p->visible_polygons->Next(temp_polygon);
  }



  // Determine if I've been to this new state already.
  // If so, then return NULL.

  state_elem *se;

  if (prev) {

    se = (state_elem *)states_seen->First();
    while(se) {

      if (se->location == elem->location) {
	
	if (!memcmp(elem->poly_states, se->poly_states, 
		    sizeof(int)*num_polygons)) {
	  free(elem->poly_states);
	  free(elem);
	  UNREGISTER();

	  return NULL;
	  
	}
	
      }

      se = (state_elem *)states_seen->Next(se);
    }

  }

  // They're different, so append the new state to my states seen
  // list, and return the new state to caller.

  states_seen->Append(elem);

  UNREGISTER();

  return elem;
}



int State::Value(void *m_elem) {

  int i, val;
  state_elem *elem = (state_elem *)m_elem;

  val = 0;

  for (i = 0; i < num_polygons; i++) {
    val += elem->poly_states[i];
  }

  return val;
}




void State::print(void *m_elem) {

  state_elem *elem = (state_elem *)m_elem;
  int i;

  printf("P: %d ", (int)elem->location);

  for (i = 0; i < num_polygons; i++) {
    printf("%d ", elem->poly_states[i]);

  }

  printf("\n");
}