/**
 * Prints the graph by printing Node informations and Edge informations while iterating it
 * Testcase:Node:%d,%d\n	with the linkaddr.u8[0] and the depth
 * Testcase:Edge:%d,%d\n	with the linkaddr.u8[0] of the source and the linkaddr.u8[0] of the drain
 */
static void debug_k_hop_timer_event(void *ptr)
{
	printf("Print Graph for Node %d\n", linkaddr_node_addr.u8[0]);
	uint8_t count = 0;

	uint8_t i = 0;
	printf("Saved Nodes:%d\n", get_node_count());
	printf("Saved Edges:%d\n", get_edge_count());

	//print nodes
	p_hop_t *hops = get_hop_counts(&count);
	printf("Count Node hops:%d\n", count);
	//also print root
	printf("Testcase:Node:%d,%d\n", linkaddr_node_addr.u8[0], 0);
	for (i = 0; i < count; i++)
	{
		printf("Testcase:Node:%d,%d\n", hops[i].addr.u8[0], hops[i].hop_count);
	}
	free(hops);

	//print edges
	p_edge_t **edge_array = get_all_edges(&count);
	printf("Count Edge:%d\n", count);
	for (i = 0; i < count; i++)
	{
		printf("Testcase:Edge:%d,%d\n", edge_array[i]->src.u8[0], edge_array[i]->dst.u8[0]);
	}
}
// Core subdivision iteration loop
struct Mesh_Info *
iterate(struct rt_bot_internal *bot, struct Mesh_Info *prev_mesh)
{
    std::map<size_t, std::vector<size_t> >::iterator f_it;
    std::vector<size_t>::iterator l_it;

    struct Mesh_Info *starting_mesh;
    if (!prev_mesh) {
	starting_mesh = new Mesh_Info;
	mesh_info_init(bot, starting_mesh);
    } else {
	starting_mesh = prev_mesh;
    }
    struct Mesh_Info *mesh = new Mesh_Info;

    mesh->iteration_cnt = starting_mesh->iteration_cnt + 1;

    find_q_pts(starting_mesh);

    std::set<std::pair<size_t, size_t> > old_edges;
    std::set<std::pair<size_t, size_t> >::iterator e_it;
    get_all_edges(starting_mesh, &old_edges);

    std::set<std::pair<size_t, size_t> > outer_edges;
    std::set<size_t > outer_pts;
    std::set<size_t > outer_faces;
    get_boundaries(starting_mesh, &outer_pts, &outer_edges, &outer_faces);

    std::cout << "outer pt count: " << outer_pts.size() << "\n";
    std::cout << "outer edge count: " << outer_edges.size() << "\n";
    std::cout << "outer face count: " << outer_faces.size() << "\n";

    // Relax old points here
    for (size_t pcnt = 0; pcnt < (size_t)starting_mesh->points_p0.Count(); pcnt++) {
	mesh->points_p0.Append(*starting_mesh->points_p0.At((int)pcnt));
	mesh->iteration_of_insert[pcnt] = starting_mesh->iteration_of_insert[pcnt];
    }

    for (f_it = starting_mesh->face_pts.begin(); f_it != starting_mesh->face_pts.end(); f_it++) {
	mesh->points_p0.Append(starting_mesh->points_q[(int)(*f_it).first]);
	mesh->iteration_of_insert[mesh->points_p0.Count()-1] = mesh->iteration_cnt;
	starting_mesh->index_in_next[(*f_it).first] = mesh->points_p0.Count()-1;
    }

    // Use the old faces to guide the insertion of the new.
    size_t face_cnt = 0;
    for (f_it = starting_mesh->face_pts.begin(); f_it != starting_mesh->face_pts.end(); f_it++) {
	std::set<std::pair<size_t, size_t> > face_old_edges;

	size_t q0 = starting_mesh->index_in_next[(*f_it).first];
	l_it = (*f_it).second.begin();

	face_old_edges.insert(std::make_pair((*l_it), (*(l_it+1))));
	face_old_edges.insert(std::make_pair((*(l_it+1)), (*(l_it+2))));
	face_old_edges.insert(std::make_pair((*(l_it+2)), (*l_it)));

	for (e_it = face_old_edges.begin(); e_it != face_old_edges.end(); e_it++) {
	    std::pair<size_t, size_t> edge = mk_edge((*e_it).first, (*e_it).second);

	    if (old_edges.find(edge) != old_edges.end()) {
		if (outer_edges.find(edge) == outer_edges.end()) {

		    std::set<size_t> curr_faces = starting_mesh->edges_to_faces[edge];

		    curr_faces.erase((*f_it).first);
		    size_t q1 = starting_mesh->index_in_next[*curr_faces.begin()];

                    if (outer_faces.find(q0) != outer_faces.end() && outer_faces.find(q1) != outer_faces.end()) {
			std::cout << "Got two edge faces\n";
                    }

		    mesh_add_face((*e_it).first, q1, q0, face_cnt, mesh);
		    face_cnt++;
		    mesh_add_face((*e_it).second, q0, q1, face_cnt, mesh);
		    face_cnt++;
		    old_edges.erase(edge);
		} else {
		    if (mesh->iteration_cnt % 2 == 0) {
			std::cout << "second iteration: " << q0 << "\n";
		    } else {
			mesh_add_face((*e_it).first, (*e_it).second, q0, face_cnt, mesh);
			face_cnt++;
			old_edges.erase(edge);
		    }
		}
	    }
	}
    }

    delete starting_mesh;
    return mesh;
}
EdgeSet MSConnectivityScore::get_connected_pairs() const {
  NNGraph g = find_threshold();
  EdgeSet edges = get_all_edges(g);
  return edges;
}