#include <igraph.h>
#include <string.h>
#include <stdlib.h>

#define Calloc(n,t)    (t*) calloc( (size_t)(n), sizeof(t) )
#define Free(p)        (free( (void *)(p) ), (p) = NULL)

int igraph_free(void *p);

int igraph_free(void *p) {
  Free(p);
  return 0;
}

void igraph_i_neighborhood_free(igraph_vector_ptr_t *complist) {
  long int i;
  for (i=0; i<igraph_vector_ptr_size(complist); i++) {
    if (VECTOR(*complist)[i] != 0) {
      igraph_destroy(VECTOR(*complist)[i]);
      igraph_free(VECTOR(*complist)[i]);
    }
  }
}

int igraph_neighborhood(const igraph_t *graf, igraph_vector_ptr_t *res, int order){
  igraph_t *newg;
  igraph_dqueue_t q=IGRAPH_DQUEUE_NULL;
  igraph_vector_t tmp=IGRAPH_VECTOR_NULL;
  igraph_vector_t view;
  long int no_of_nodes=igraph_vcount(graf);
  int steps=0;
  long int i,j;
  char* already_processed;

  already_processed=Calloc(no_of_nodes, char);
  if (already_processed==0) {
    IGRAPH_ERROR("Failure in visiting subgraphs", IGRAPH_ENOMEM);
  }
  IGRAPH_FINALLY(igraph_free, already_processed);  
  IGRAPH_CHECK(igraph_dqueue_init(&q, 100));
  IGRAPH_VECTOR_INIT_FINALLY(&tmp, 0);
  IGRAPH_VECTOR_INIT_FINALLY(&view,0);
  igraph_vector_ptr_clear(res);
  IGRAPH_FINALLY(igraph_i_neighborhood_free, res);

  for(i=0;i<no_of_nodes;i++){
    IGRAPH_ALLOW_INTERRUPTION();
    memset(already_processed,0,no_of_nodes);
    already_processed[i]=1;
    igraph_vector_clear(&view);
    IGRAPH_CHECK(igraph_dqueue_push(&q,i));
    IGRAPH_CHECK(igraph_vector_push_back(&view, i));
    steps=1;
    while (!igraph_dqueue_empty(&q)) {
      long int actnode=(long int)igraph_dqueue_pop(&q);
      IGRAPH_CHECK(igraph_neighbors(graf, &tmp, actnode, IGRAPH_ALL));
      for (j=0; j<igraph_vector_size(&tmp); j++) {
	long int neighbor=VECTOR(tmp)[j];
	if(already_processed[neighbor]!=1){
	  IGRAPH_CHECK(igraph_vector_push_back(&view, neighbor));
	  if(steps<order){
	    IGRAPH_CHECK(igraph_dqueue_push(&q, neighbor));
	  }
	}
      }
      steps++;
      already_processed[actnode]=1;
    }
  
    newg=Calloc(1,igraph_t);
    if (newg==0) {
      IGRAPH_ERROR("Failure in visiting graph", IGRAPH_ENOMEM);
    }
    IGRAPH_CHECK(igraph_vector_ptr_push_back(res, newg));
    IGRAPH_FINALLY(igraph_destroy, newg);
    IGRAPH_CHECK(igraph_subgraph(graf, newg,igraph_vss_vector(&view)));
    IGRAPH_FINALLY_CLEAN(1);
  }

  igraph_dqueue_destroy(&q);
  igraph_vector_destroy(&view);
  igraph_vector_destroy(&tmp);
  igraph_free(already_processed);
  IGRAPH_FINALLY_CLEAN(4);

  return 0;
}

int main() {
  
  igraph_t graph;
  igraph_vector_ptr_t neighborhoods;
  long int i, j;
  
  igraph_tree(&graph, 15, 2, IGRAPH_TREE_UNDIRECTED);
  igraph_write_graph_edgelist(&graph, stdout);
  igraph_vector_ptr_init(&neighborhoods, 0);
  
  for (i=0; i<=4; i++) {
    printf("############### %li ###############\n", i);
    igraph_neighborhood(&graph, &neighborhoods, i);
    igraph_write_graph_edgelist(VECTOR(neighborhoods)[0], stdout);
    for (j=0; j<igraph_vector_ptr_size(&neighborhoods); j++) {
      igraph_destroy(VECTOR(neighborhoods)[j]);
    }
  }
  
  igraph_vector_ptr_destroy(&neighborhoods);
  igraph_destroy(&graph);
  
  return 0;
}