Add node to computation graph by ID

Added ability to create nodes with arbitrary IDs. This should only be used if necessary, preferring graph_add_node over graph_add_node_id, because it does not use hashing, and must allocate an array up to the size of the ID you requested.

The ability to add nodes by ID is necessary for the ground station to use the existing graph infrastructure for showing nodes. We could have left the requirement such that after all nodes are added, they must be consecutive, because that is what the quadcopter will return. I am not a fan of this, though, because it seems like something that will bite teams in the future if they don't know about that unexpected behavior.

quad/src/computation_graph/computation_graph.c

@@ -9,11 +9,19 @@
 // Array to store input values for passing to the execute function of each node
 static double exec_input_vals[GRAPH_MAX_INPUTS];
 
+// Macro functions for setting and clearing single bits in int array
+// From http://www.mathcs.emory.edu/~cheung/Courses/255/Syllabus/1-C-intro/bit-array.html
+#define setBit(A,k)     ( A[(k / (8*sizeof(&A)))] |=  (1 << (k % (8*sizeof(&A)))) )
+#define clearBit(A,k)   ( A[(k / (8*sizeof(&A)))] &= ~(1 << (k % (8*sizeof(&A)))) )
+#define testBit(A,k)    ( A[(k / (8*sizeof(&A)))] &   (1 << (k % (8*sizeof(&A)))) )
+
 struct computation_graph *create_graph() {
     struct computation_graph *the_graph = malloc(sizeof(struct computation_graph));
     if (!the_graph) {return NULL;}
+    // Allocate space for a single node in the graph
     the_graph->nodes = malloc(sizeof(struct graph_node));
-    if (!the_graph->nodes) { return NULL; }
+    the_graph->node_existence = malloc(sizeof(int));
+    if (!the_graph->nodes || !the_graph->node_existence) { return NULL; }
     the_graph->n_nodes = 0;
     the_graph->size = 1;
     return the_graph;
@@ -46,12 +54,14 @@ static void reset_node_rec(struct computation_graph* graph, int node_id, int dep
 }
 
 int reset_node(struct computation_graph* graph, int node_id) {
-    if (node_id >= graph->n_nodes) {
+    if (!graph_node_exists(graph, node_id)) {
         return -1;
     }
     int i;
-    for (i = 0; i < graph->n_nodes; i++) {
-        graph->nodes[i].processed_state = UNPROCESSED;
+    for (i = 0; i < graph->size; i++) {
+        if (graph_node_exists(graph, i)) {
+            graph->nodes[i].processed_state = UNPROCESSED;
+        }
     }
     reset_node_rec(graph, node_id, 0);
     return 0;
@@ -59,12 +69,13 @@ int reset_node(struct computation_graph* graph, int node_id) {
 
 int graph_set_source(struct computation_graph *graph,
                      int dest_node_id, int dest_input, int src_node_id, int src_output) {
-    if (dest_node_id >= graph->n_nodes || src_node_id >= graph->n_nodes) {
+    if (!graph_node_exists(graph, dest_node_id) || !graph_node_exists(graph, src_node_id)) {
         return -1;
     }
     struct graph_node *dest_node = &graph->nodes[dest_node_id];
     struct graph_node *src_node = &graph->nodes[src_node_id];
-    if (dest_input >= dest_node->type->n_inputs || src_output >= src_node->type->n_outputs) {
+    if (dest_input >= dest_node->type->n_inputs || src_output >= src_node->type->n_outputs ||
+        dest_input < 0 || src_output < 0) {
         return -1;
     }
 
@@ -82,7 +93,7 @@ int graph_set_source(struct computation_graph *graph,
 }
 
 struct node_src graph_get_source(struct computation_graph *graph, int node_id, int input_id) {
-    if (node_id >= graph->n_nodes || node_id < 0 ||
+    if (!graph_node_exists(graph, node_id) ||
         input_id >= graph->nodes[node_id].type->n_inputs || input_id < 0) {
         return (struct node_src) {.controller_id = -1, .controller_output = -1};
     }
@@ -95,16 +106,33 @@ int graph_add_node(struct computation_graph *graph,
                    void *state) {
     assert(type->n_inputs <= GRAPH_MAX_INPUTS);
     int new_id = graph->n_nodes;
-    if (new_id >= graph->size) {
-        int new_capacity = graph->n_nodes == 0 ? 1 : graph->n_nodes * 2;
-        struct graph_node *node_arr = realloc(graph->nodes, sizeof(struct graph_node) * new_capacity);
-        if (!node_arr) {
-            return -1;
+    return graph_add_node_id(graph, new_id, name, type, state);
+}
+
+int graph_add_node_id(struct computation_graph *graph,
+                   int id,
+                   const char *name,
+                   const struct graph_node_type *type,
+                   void *state) {
+    if (id >= graph->size) {
+        size_t old_size = graph->size;
+        size_t new_size = old_size == 0 ? 8 : id * 2; // Hold twice the given ID
+        struct graph_node *node_arr = realloc(graph->nodes, sizeof(struct graph_node) * new_size);
+        if (!node_arr) { return -1; }
+        // Number of integers needed to hold new_size bits
+        size_t new_exist_size = ceil((float)new_size / (8 * sizeof(int))); // ceil(new_size / (bits per int))
+        // Set the newly allocated memory to 0
+        size_t old_exist_size = ceil((float)old_size / (8 * sizeof(int)));
+        if (old_exist_size != new_exist_size) {
+            int* exist_arr = realloc(graph->node_existence, sizeof(int) * new_exist_size);
+            if (!exist_arr) {return -1;}
+            memset(exist_arr + old_exist_size, 0, (new_exist_size - old_exist_size) * sizeof(int));
+            graph->node_existence = exist_arr;
         }
-        graph->size = new_capacity;
+        graph->size = new_size;
         graph->nodes = node_arr;
     }
-    struct graph_node *new_node = &graph->nodes[new_id];
+    struct graph_node *new_node = &graph->nodes[id];
     new_node->name = strdup(name);
     new_node->type = type;
     new_node->state = state;
@@ -124,15 +152,16 @@ int graph_add_node(struct computation_graph *graph,
         new_node->input_srcs[i].controller_id = -1;
     }
     graph->n_nodes += 1;
+    setBit(graph->node_existence, id);
     // Reset block upon creation
     if (new_node->type->reset != NULL) {
         new_node->type->reset(new_node->state);
     }
-    return new_id;
+    return id;
 }
 
 int graph_set_param_val(struct computation_graph *graph, int node_id, int param_id, double value) {
-    if (node_id >= graph->n_nodes || param_id >= graph->nodes[node_id].type->n_params) {
+    if (!graph_node_exists(graph, node_id) || param_id >= graph->nodes[node_id].type->n_params) {
         return -1;
     }
     graph->nodes[node_id].param_values[param_id] = value;
@@ -141,27 +170,33 @@ int graph_set_param_val(struct computation_graph *graph, int node_id, int param_
 }
 
 double graph_get_param_val(const struct computation_graph *graph, int node_id, int param_id) {
-    if (node_id >= graph->n_nodes || param_id >= graph->nodes[node_id].type->n_params) {
+    if (!graph_node_exists(graph, node_id) || param_id >= graph->nodes[node_id].type->n_params) {
         return NAN;
     }
 	return graph->nodes[node_id].param_values[param_id];
 }
 
 double graph_get_output(const struct computation_graph *graph, int node_id, int output_id) {
-    if (node_id >= graph->n_nodes || output_id >= graph->nodes[node_id].type->n_outputs) {
+    if (!graph_node_exists(graph, node_id) || output_id >= graph->nodes[node_id].type->n_outputs) {
         return NAN;
     }
     return graph->nodes[node_id].output_values[output_id];
 }
 
+/*
+ * Assumptions: The node passed in is a valid ID (should be checked before passing)
+ * and all node sources are either valid node-output pairs, or the source node ID == -1
+ * These constraints should be satisfied by using the graph_set_source function, so long as
+ * a valid node ID is passed in to the first call of this function
+*/
 void graph_compute_node_rec(struct computation_graph *graph, int node_id, int depth) {
     if (depth >= GRAPH_MAX_DEPTH) {
-        assert(1 == 0); // TODO :Xil_Assert false
-        return;
-    }
-    if (node_id >= graph->n_nodes) {
+        assert(1 == 0);
         return;
     }
+    // if (!graph_node_exists(graph, node_id)) {
+    //     return;
+    // }
     struct graph_node *node = &graph->nodes[node_id];
     if (node->processed_state != UNPROCESSED) {
         return;
@@ -197,17 +232,19 @@ void graph_compute_node_rec(struct computation_graph *graph, int node_id, int de
 
 void graph_compute_nodes(struct computation_graph *graph, int* node_ids, int n_nodes) {
     int i;
-    for (i = 0; i < graph->n_nodes; i++) {
+    for (i = 0; i < graph->size; i++) {
+        // Note: Do not access malloc'd members in here without first checking if node is valid
         graph->nodes[i].processed_state = UNPROCESSED;
     }
     for (i = 0; i < n_nodes; i++) {
     	int node_id = node_ids[i];
-    	if (node_id < graph->n_nodes) {
+    	if (graph_node_exists(graph, node_id)) {
     	    graph_compute_node_rec(graph, node_id, 0);
     	}
     }
     // Clear all the updated flags for nodes that were actually executed
-    for (i = 0; i < graph->n_nodes; i++) {
+    for (i = 0; i < graph->size; i++) {
+        // Note: Do not access malloc'd members in here without first checking if node is valid
         struct graph_node* node = &graph->nodes[i];
         if (node->processed_state == PROCESSED) {
             node->updated = 0;
@@ -221,7 +258,8 @@ int export_dot(const struct computation_graph* graph, FILE* of, int print_output
 
     // Draw all the nodes and their inputs
     int i;
-    for (i = 0; i < graph->n_nodes; i++) {
+    for (i = 0; i < graph->size; i++) {
+        if (!graph_node_exists(graph, i)) {continue;}
         struct graph_node *node = &graph->nodes[i];
         // Create node
         fprintf(of, "\"%s\"[shape=record\nlabel=\"", node->name);
@@ -254,3 +292,10 @@ int export_dot(const struct computation_graph* graph, FILE* of, int print_output
     fprintf(of, "}"); // Close graph
     return 0;
 }
+
+int graph_node_exists(const struct computation_graph *graph, int node_id) {
+    if (node_id < 0 || node_id >= graph->size || !testBit(graph->node_existence, node_id)) {
+        return 0;
+    }
+    else {return 1;}
+}