#include "trifixion.h"

Trifixion::Trifixion() {
  env = new Environment();
  srand(time(NULL));
}

Trifixion::~Trifixion(){
  if (env) delete env;
}

void Trifixion::Start(FILE *input, FILE *output) {
  REGISTER("Trifixion::Start");
  
  int i;
  List *path;

  void *s_elem;
  Polygon *start_polygon;
  
  int robot_in_use;


  in = input;
  out = output;


  parse_input();

  env->GenerateCells(num_robots);  // This was part 1a.

  /*  This is no longer needed.

  for (i = 0; i < num_robots; i++) {
    env->ComputeShortestPaths(robot_starts[i], i);  // This was part 1b.
  }

  */

  env->ComputeVisibility();  // This was part 2.

  state = new State(env->InternalPolygons()->Size(),
		    env->GetPolygon()->internal_segments, 
		    env->InternalPolygons(), num_robots);

  state->num_robots = 1;

  i = 0;
  path = NULL;

  for (robot_in_use = 0; robot_in_use < num_robots; robot_in_use++) {

    start_polygon = env->vertex_in_which_polygon(robot_starts[i]);

    s_elem = state->StartElem(start_polygon);
      
    path = env->PursueEvader(state, s_elem, start_polygon);  // part 3.
    if (path) break;
      
    if (robot_in_use == num_robots-1) {
      printf("Failed.");
      UNREGISTER();

      return;
    }

    state->robot_polygons[robot_in_use] = env->best_polygon;

    state->num_robots++;
  }


  if (!path) {
    printf("Failed.\n");
    UNREGISTER();
    return;
  }

  Vertex *tv;

  if (state->num_robots > 1) {
    for (i = 0; i < state->num_robots - 1; i++) {
      printf("%f\n%f\n\n", state->robot_polygons[i]->GetCenter()->x,
	     state->robot_polygons[i]->GetCenter()->y);
      
    }

  }


  
  tv = (Vertex *)path->First();
  while(tv) {
    printf("%.5f\n%.5f\n", tv->x, tv->y);
    tv = (Vertex *)path->Next(tv);
  }
  printf("\n");
  fflush(stdout);

  UNREGISTER();
}


void Trifixion::read_input(fixedpt *f) {
  REGISTER("Trifixion::read_input");

  float temp;

  if (fscanf(in, "%f", &temp) <= 0) {
    ERROR("Couldn't get float out of input stream.");
  }

  *f = FIXEDPT(temp);

  UNREGISTER();
}


void Trifixion::read_input(int *i) {
  REGISTER("Trifixion::read_input");

  if (fscanf(in, "%d", i) <= 0) {
    ERROR("Couldn't get integer out of input stream.");
  }

  UNREGISTER();
}


/*
 * parse_input
 *
 * This function takes the simple input string and parses it into
 * an Environment object to prepare for path-planning.
 */

void Trifixion::parse_input() {

  REGISTER("Trifixion::parse_input");

  int i;
  fixedpt x, y;
  Polygon *p;
  Vertex *vertex1 = NULL; 
  Vertex *vertex2 = NULL;
  Vertex *first_vertex = NULL;
  Segment *new_seg;
  int num_vertices;

  p = new Polygon();

  // First read in the number of robots.
  read_input(&num_robots);

  if (num_robots <= 0 || num_robots > MAX_ROBOTS) {
    ERROR("Bad number of robots");
  }
  
  for (i = 0; i < num_robots; i++) {
    read_input(&x);
    read_input(&y);

    x += perturbation();
    y += perturbation();

    robot_starts[i] = new Vertex(x, y);
  }

  read_input(&num_vertices);

  // Read in all the vertices for the environment polygon. It's
  // up to me to make segments out of these and insert them
  // in the proper order into the environment polygon.

  for (i = 0; i < num_vertices; i++) {
    read_input(&x);
    read_input(&y);

    x += perturbation();
    y += perturbation();
    
    // Hold the first vertex so we can attach the last vertex to it.
    // Also, we can't make a segment until we have gotten two vertices.

    if (i == 0) {
      vertex1 = new Vertex(x, y);
      first_vertex = vertex1;
    } else {
      vertex2 = new Vertex(x, y);
      new_seg = new Segment(vertex1, vertex2);
      p->AddSegment(new_seg);
      vertex1 = vertex2;
    }
  }

  new_seg = new Segment(vertex1, first_vertex);
  p->AddSegment(new_seg);

  env->SetPolygon(p);

  UNREGISTER();
}



/*
 * perturbation
 *
 * This function returns a random perterbation that is very small, but is
 * sufficient to rid us of annoying measure-zero problems.
 */

fixedpt Trifixion::perturbation() {
  return (((rand() >> 8) % 1000) / 1000.0) * 0.001;
}