#include "roadmap.h"


Roadmap::Roadmap() {
  roadmap = NULL;
  roadmap_size = 0;
}

Roadmap::~Roadmap() {
  int i;

  for (i = 0; i < roadmap_size; i++) {
    if (roadmap[i]) free(roadmap[i]);
  }

  if (roadmap) free(roadmap);
}


void Roadmap::do_copy(Roadmap *r) {

  int i;
  int n_next;
  Polygon **temp_next;
  fixedpt *temp_dist;

  roadmap = (roadmap_elem **)malloc(sizeof(roadmap_elem *) * r->Size());

  for (i = 0; i < r->Size(); i++) {
    n_next = r->NumNext(i);
    temp_dist = r->AllDistances(i);
    temp_next = r->AllNext(i);

    roadmap[i] = (roadmap_elem *)malloc(sizeof(roadmap_elem));

    roadmap[i]->distance = (fixedpt *)malloc(sizeof(fixedpt) * n_next);
    memcpy(roadmap[i]->distance, temp_dist, sizeof(fixedpt) * n_next);

    roadmap[i]->next = (Polygon **)malloc(sizeof(Polygon *) * n_next);
    memcpy(roadmap[i]->next, temp_next, sizeof(Polygon *) * n_next);

    roadmap[i]->num_next = n_next;
    roadmap[i]->my_polygon = r->PolygonNum(i);
  }
}




void Roadmap::copy_polygons(Roadmap *r) {

  int i;

  roadmap = (roadmap_elem **)malloc(sizeof(roadmap_elem *) * r->Size());

  for (i = 0; i < r->Size(); i++) {
    roadmap[i] = (roadmap_elem *)malloc(sizeof(roadmap_elem));
    roadmap[i]->num_next = 0;
    roadmap[i]->next = NULL;
    roadmap[i]->distance = NULL;
    roadmap[i]->my_polygon = r->PolygonNum(i);
  }

  roadmap_size = r->Size();
}




void Roadmap::Insert(Polygon *p) {

  REGISTER("Roadmap::Insert");
  
  roadmap_elem **old_map;
  int new_roadmap_size;
  roadmap_elem *new_elem;

  old_map = roadmap;

  new_roadmap_size = roadmap_size + 1;
  roadmap = (roadmap_elem **)malloc(sizeof(roadmap_elem *) * new_roadmap_size);

  if (old_map) {
    memcpy(roadmap, old_map, roadmap_size * (sizeof(roadmap_elem *)));
    free(old_map);
  } 

  new_elem = (roadmap_elem *)malloc(sizeof(roadmap_elem));
  new_elem->next = NULL;
  new_elem->my_polygon = p;
  new_elem->num_next = 0;
  new_elem->distance = NULL;

  roadmap[roadmap_size] = new_elem;
  roadmap_size = new_roadmap_size;

  UNREGISTER();
}


void Roadmap::Link(Polygon *p1, Polygon *p2) {

  REGISTER("Roadmap::Link");

  roadmap_elem *e[2];
  int i, new_num_next;
  Polygon **new_next;
  fixedpt *new_dist;

  if (p1 == p2) {
    ERROR("Attempted recursive Roadmap Link!");
  }

  e[0] = e[1] = NULL;

  for (i = 0; i < roadmap_size; i++) {
    if (roadmap[i]->my_polygon == p1) {
      e[0] = roadmap[i];
    } 
    if (roadmap[i]->my_polygon == p2) {
      e[1] = roadmap[i];
    }
  }

  if (e[0]->next) {
    for (i = 0; i < e[0]->num_next; i++) {      
      if (e[0]->next[i] == e[1]->my_polygon) {
	UNREGISTER();
	return;
      }
    }
  }
  
  if (!e[0] || !e[1]) ERROR("Didn't find one of the polygons!");  
  // Didn't find one or both of the polygons.

  new_num_next = e[0]->num_next + 1;

  new_next = (Polygon **)malloc(sizeof(Polygon *) * new_num_next);
  if (e[0]->next) {
    memcpy(new_next, e[0]->next, sizeof(Polygon *) * e[0]->num_next);
    free(e[0]->next);
  }

  new_dist = (fixedpt *)malloc(sizeof(fixedpt) * new_num_next);
  if (e[0]->next) {
    memcpy(new_dist, e[0]->distance, sizeof(fixedpt) * e[0]->num_next);
    free(e[0]->distance);
  }
  
  e[0]->next = new_next;
  e[0]->num_next = new_num_next;
  e[0]->distance = new_dist;

  e[0]->distance[new_num_next-1] = 
    e[0]->my_polygon->GetCenter()->Distance(p2->GetCenter());

  e[0]->next[new_num_next-1] = p2;

  UNREGISTER();

}


void Roadmap::Unlink(Polygon *p1, Polygon *p2) {
  
  REGISTER("Roadmap::Unlink");

  roadmap_elem *e[2];
  int i, j, k, new_num_next;
  Polygon **new_next;
  fixedpt *new_dist;

  e[0] = e[1] = NULL;

  for (i = 0; i < roadmap_size; i++) {
    if (roadmap[i]->my_polygon == p1) {
      e[0] = roadmap[i];
    } 
    if (roadmap[i]->my_polygon == p2) {
      e[1] = roadmap[i];
    }
  }
  
  if (!e[0] || !e[1]) ERROR("Didn't find one of the polygons!");  
  // Didn't find one or both of the polygons.

  new_num_next = e[0]->num_next - 1;
  new_next = (Polygon **)malloc(sizeof(Polygon *) * new_num_next);
  new_dist = (fixedpt *)malloc(sizeof(fixedpt) * new_num_next);
  for (j = 0, k = 0; j < e[i]->num_next; j++, k++) {
    if (e[0]->next[j] == e[1]->my_polygon) {
      k--;
    } else {
      new_dist[k] = e[0]->distance[j];
      new_next[k] = e[0]->next[j];
    }
  }

  free(e[0]->distance);
  free(e[0]->next);

  e[0]->distance = new_dist;
  e[0]->next = new_next;
  e[0]->num_next = new_num_next;
  
  
  UNREGISTER();
}


int Roadmap::NumNext(Polygon *p) {

  int i;

  for (i = 0; i < roadmap_size; i++) {
    if (roadmap[i]->my_polygon == p) {
      return roadmap[i]->num_next;
    }
  }

  return -1;
}



Polygon *Roadmap::Next(Polygon *p, int num) {
  
  int i;

  for (i = 0; i < roadmap_size; i++) {
    if (roadmap[i]->my_polygon == p) {
      if (roadmap[i]->num_next <= num) {
	return NULL;
      } else {
	return roadmap[i]->next[num];
      }
    }
  }

  return NULL;

}


fixedpt *Roadmap::AllDistances(Polygon *p) {
  int i;
  for (i = 0; i < roadmap_size; i++) {
    if (roadmap[i]->my_polygon == p) {
      return roadmap[i]->distance;
    }
  }

  return NULL;
}


Polygon **Roadmap::AllNext(Polygon *p, int *num) {

  int i;
  
  for (i = 0; i < roadmap_size; i++) {
    if (roadmap[i]->my_polygon == p) {
      *num = roadmap[i]->num_next;
      return roadmap[i]->next;
    }
  }
  
  return NULL;

}




void Roadmap::print() {

  int i;
  int j;

  printf("Roadmap:\n");

  for (i = 0; i < roadmap_size; i++) {

    printf("Polygon %d:\n", i);
    roadmap[i]->my_polygon->print();
    printf("\n");

    printf("Num links: %d\n\n", roadmap[i]->num_next);

    printf("Links to:\n\n");
    for (j = 0; j < roadmap[i]->num_next; j++) {
      printf("Link %d:\n", j);
      roadmap[i]->next[j]->print();       
    }
    
    printf("\n");
  }
}