void pr_advance_to_next_child__mut(Pr_TraverseState* state) {
uint32_t next_slot = state->next_slot;
Node** cur_slots = state->current->slots;
ASSERT(!is_backwards(cur_slots[next_slot]), "At %s we are going backward instead of forward",
((Node*)follow_edge(cur_slots[next_slot]))->name);
Node* tmp_grand_pa = state->previous;
state->previous = state->current;
state->current = follow_edge(cur_slots[next_slot]);
state->previous->slots[next_slot] = set_flags_on_edge(tmp_grand_pa, SET_TRAVERSE_FLAG|SET_BACK_FLAG|state->tag_token);
state->next_slot = pr_next_untraversed_slot(state->current, state->tag_token, 0);
}
Bf_EdgeParams bf_backward_invert_edges__mut(Bf_TraverseState *state) {
Bf_EdgeParams params = {0};
Node *grand_pa = NULL;
// Invariant : (grand_pa / NULL) <-- (parent) (tail)
while(1) {
params = bf_get_node_edge_parameters(state->parent);
ASSERT(state->parent->count <= state->tail->count, "While going backwards children always hold a higher gen than parents");
ASSERT(is_backwards(state->parent->slots[params.inv_idx]), "There should always be a backward edge");
grand_pa = follow_edge(state->parent->slots[params.inv_idx]);
state->parent->count = params.min_gen ? params.min_gen : state->tail->count;
ASSERT(state->parent->count >= state->next_tail_gen, "While going backwards we never go under the target generation");
// we have found the closest ancestor of the node we are looking for
if (state->parent->count == state->next_tail_gen) break;
state->parent->slots[params.inv_idx] = state->tail;
state->tail = state->parent;
state->parent = grand_pa;
ASSERT(state->parent, "We should not go past the root while going backwards");
}
// The edge params of the node where the traversal must change direction
// Or the default value on a degenerate case : no need to backward to reach the next tail node
return params;
}
uint32_t pr_next_untraversed_slot(Node *node, size_t tag_token, uint32_t start_slot) {
uint32_t next_slot = start_slot;
for(; next_slot < SLOT_COUNT; ++next_slot) {
Node *child = follow_edge(node->slots[next_slot]);
if (!child) continue;
if (get_flags_on_edge(node->slots[next_slot], SET_TRAVERSE_FLAG)) continue;
// the edge should not point to an already visited node
if (pr_get_visit_tag_value(child) == tag_token) continue;
// Do not follow edges that loop back to itself
if (child == node) continue;
break;
}
ASSERT(next_slot == SLOT_COUNT || follow_edge(node->slots[next_slot]),
"Untraversed null edge at %s slot %u", node->name, next_slot);
return next_slot;
}
void bf_pivot_back_forward_edges__mut(Bf_TraverseState *state, Bf_EdgeParams params) {
// degenerate case : no need to backward to reach the next tail node
if (!params.min_gen) return;
Node *grand_pa = follow_edge(state->parent->slots[params.inv_idx]);
state->parent->slots[params.inv_idx] = state->tail;
state->tail = state->parent->slots[params.min_idx];
state->parent->slots[params.min_idx] = to_backwards_edge(grand_pa);
ASSERT(state->tail->count == state->next_tail_gen, "The pivoted edge should have pointed to the next tail node");
}
void turn_to_line(robot_state &state, float degrees_approx)
{
std::cout << "Making " << degrees_approx << "* turn.\n";
if (fabs(degrees_approx) < 5.f) // the junction is straight, ascribe to numerical error
{
move(state, 1, 0);
while (state.line_state == LINE_JUNCTION)
update_sensor_values(state);
delay(50); // Just to make sure we're past the junction
}
else
{
float turn = degrees_approx < 0 ? 1 : -1;
// Turn in desired direction until we are off the line
move(state, -0.2, turn);
while (state.line_state != LINE_NONE_DETECTED)
update_sensor_values(state);
std::cout << "Left previous line\n";
// We've turned off the line -- now just a bit extra:
//delay(100);
// Go forward until we hit the new line.
move(state, 1, 0);
while (state.line_state == LINE_NONE_DETECTED)
update_sensor_values(state);
std::cout << "line acquired, using edge following to align...\n";
// Now use edge following to acquire line
while (state.line_state != LINE_STRAIGHT)
{
if (turn < 0)
follow_edge(state, 3, false);
else
follow_edge(state, 0, true);
update_sensor_values(state);
}
std::cout << "Realigned to line, resuming normal following\n";
}
}
uint32_t bf_backward_prune_exhausted_branch__mut(Bf_TraverseState *state) {
Bf_EdgeParams params = {0};
// Invariant : the tail node has no forward edges
while (!params.min_gen) {
ASSERT(state->parent, "We should not go past the root while pruning backwards");
params = bf_get_node_edge_parameters(state->parent);
ASSERT(is_backwards(state->parent->slots[params.inv_idx]), "There should always be a backward edge");
state->tail = state->parent;
state->parent = follow_edge(state->parent->slots[params.inv_idx]);
}
// we reached a node with ancestors in the visit queue, recalculate generation after prune
state->tail->count = params.min_gen;
// we keep the invariant that between parent and tail there is no edge
state->tail->slots[params.inv_idx] = NULL;
ASSERT(state->tail->count >= state->next_tail_gen, "When we cannot prune further we should be at a higher generation");
return state->tail->count;
}
GraphHandle restore_graph_from_buffer(PersistedGraph graph_buf) {
GraphHandle new_graph = {0};
new_graph.vertex_count = graph_buf.vertex_count;
new_graph.root = calloc(new_graph.vertex_count, sizeof(Node));
restore_graph_from_buffer_no_offset_adjust(graph_buf, new_graph);
// now we need to adjust the edge pointers to the new offset
size_t adjust = (size_t)new_graph.root - graph_buf.offset.as_int;
for (uint32_t i=0; i<new_graph.vertex_count; ++i)
for (uint32_t j=0; j<SLOT_COUNT; ++j) {
size_t edge = (size_t)(new_graph.root[i].slots[j]);
if (follow_edge((void*)edge) == 0) continue;
edge += adjust;
//LOG_TRACE("Move %p -> %p", new_graph.root[i].slots[j], (void*)edge);
new_graph.root[i].slots[j] = (void*)edge;
}
return new_graph;
}
void dump_graph_dot_format(GraphHandle graph, const char* filepath) {
FILE* dot_file = fopen(filepath, "w");
LOG_INFO("Dumping graph to %s", filepath);
ASSERT(dot_file, "Failed to open file");
fprintf(dot_file, "digraph {\n");
fprintf(dot_file, " node [ nodesep=1.5 ];\n");
fprintf(dot_file, " graph [ overlap=false; bgcolor=\"grey\" ];\n");
fprintf(dot_file, " edge [ weight=0.5 ];\n");
for(uint32_t i=0; i<graph.vertex_count; ++i) {
Node* node = (Node*)(graph.root + i);
LOG_TRACE("Printing %u : %s", i, node->name);
if (i == 0 || i == graph.vertex_count-1)
fprintf_node_dot_format(dot_file, node, "cyan");
else if (is_leaf_node(node))
fprintf_node_dot_format(dot_file, node, "gold");
else if (is_leaf_node_ignore_back(node))
fprintf_node_dot_format(dot_file, node, "orange");
else
fprintf_node_dot_format(dot_file, node, NULL);
for(uint32_t slot=0; slot < SLOT_COUNT; ++slot) {
Node* child = follow_edge(node->slots[slot]);
if (child)
if (is_backwards(node->slots[slot]))
fprintf_edge_dot_format(dot_file, node, child, "red");
else if (get_flags_on_edge(node->slots[slot], SET_TRAVERSE_FLAG))
fprintf_edge_dot_format(dot_file, node, child, "blue");
else if (get_flags_on_edge(node->slots[slot], SET_VISIT_FLAG))
fprintf_edge_dot_format(dot_file, node, child, "green4");
else
fprintf_edge_dot_format(dot_file, node, child, NULL);
else if (is_backwards(node->slots[slot]))
fprintf_edge_dot_format(dot_file, node, NULL, "red");
}
}
fprintf(dot_file, "}\n");
fclose(dot_file);
}
void pr_backward_invert_edges__mut(Pr_TraverseState *state) {
// Invariant : state.current MUST be rolled back
while (state->next_slot == SLOT_COUNT && state->current != state->poison) {
uint32_t back_slot = 0;
Node** prev_slots = state->previous->slots;
Node* tmp_last_cur = state->current;
pr_rollback_node_to_init_state__mut(state->current, state->tag_token);
for(; back_slot < SLOT_COUNT
&& !get_flags_on_edge(prev_slots[back_slot], SET_BACK_FLAG);
++back_slot);
ASSERT(get_flags_on_edge(prev_slots[back_slot], SET_TRAVERSE_FLAG),
"All back edge should have been traversed at : %s", state->previous->name);
ASSERT(back_slot < SLOT_COUNT, "Could not find any back edge on %s", state->previous->name);
state->current = state->previous;
state->previous = follow_edge(prev_slots[back_slot]);
state->current->slots[back_slot] = set_flags_on_edge(tmp_last_cur, SET_TRAVERSE_FLAG|state->tag_token);
state->next_slot = pr_next_untraversed_slot(state->current, state->tag_token, back_slot+1);
}
}
