From b845342e48f170a65c997f415edb872948332ed1 Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Sun, 5 Feb 2017 01:08:52 -0600
Subject: [PATCH] Began implementation of computation graph. Have tested with 3
 nodes so far, which work.

---
 quad/computation_graph/Makefile               | 220 ++++++++++++++++++
 .../computation_graph/src/computation_graph.c | 118 ++++++++++
 .../computation_graph/src/computation_graph.h |  54 +++++
 quad/computation_graph/src/main.c             |  25 ++
 quad/computation_graph/src/node_add.h         |  12 +
 quad/computation_graph/src/node_add2.c        |  12 +
 quad/computation_graph/src/node_mult.c        |  12 +
 quad/computation_graph/src/node_mult.h        |  12 +
 8 files changed, 465 insertions(+)
 create mode 100644 quad/computation_graph/Makefile
 create mode 100644 quad/computation_graph/src/computation_graph.c
 create mode 100644 quad/computation_graph/src/computation_graph.h
 create mode 100644 quad/computation_graph/src/main.c
 create mode 100644 quad/computation_graph/src/node_add.h
 create mode 100644 quad/computation_graph/src/node_add2.c
 create mode 100644 quad/computation_graph/src/node_mult.c
 create mode 100644 quad/computation_graph/src/node_mult.h

diff --git a/quad/computation_graph/Makefile b/quad/computation_graph/Makefile
new file mode 100644
index 000000000..bc379cc3c
--- /dev/null
+++ b/quad/computation_graph/Makefile
@@ -0,0 +1,220 @@
+#### PROJECT SETTINGS ####
+# The name of the executable to be created
+BIN_NAME := computation_graph
+# Compiler used
+CC ?= gcc
+# Extension of source files used in the project
+SRC_EXT = c
+# Path to the source directory, relative to the makefile
+SRC_PATH = ./src
+# Space-separated pkg-config libraries used by this project
+LIBS =
+# General compiler flags
+COMPILE_FLAGS = -std=c99 -Wall -Wextra -pedantic -g
+# Additional release-specific flags
+RCOMPILE_FLAGS = -D NDEBUG
+# Additional debug-specific flags
+DCOMPILE_FLAGS = -D DEBUG
+# Add additional include paths
+INCLUDES = -I $(SRC_PATH)
+# General linker settings
+LINK_FLAGS =
+# Additional release-specific linker settings
+RLINK_FLAGS =
+# Additional debug-specific linker settings
+DLINK_FLAGS =
+# Destination directory, like a jail or mounted system
+DESTDIR = /
+# Install path (bin/ is appended automatically)
+INSTALL_PREFIX = usr/local
+#### END PROJECT SETTINGS ####
+
+# Generally should not need to edit below this line
+
+# Obtains the OS type, either 'Darwin' (OS X) or 'Linux'
+UNAME_S:=$(shell uname -s)
+
+# Function used to check variables. Use on the command line:
+# make print-VARNAME
+# Useful for debugging and adding features
+print-%: ; @echo $*=$($*)
+
+# Shell used in this makefile
+# bash is used for 'echo -en'
+SHELL = /bin/bash
+# Clear built-in rules
+.SUFFIXES:
+# Programs for installation
+INSTALL = install
+INSTALL_PROGRAM = $(INSTALL)
+INSTALL_DATA = $(INSTALL) -m 644
+
+# Append pkg-config specific libraries if need be
+ifneq ($(LIBS),)
+	COMPILE_FLAGS += $(shell pkg-config --cflags $(LIBS))
+	LINK_FLAGS += $(shell pkg-config --libs $(LIBS))
+endif
+
+# Verbose option, to output compile and link commands
+export V := false
+export CMD_PREFIX := @
+ifeq ($(V),true)
+	CMD_PREFIX :=
+endif
+
+# Combine compiler and linker flags
+release: export CFLAGS := $(CFLAGS) $(COMPILE_FLAGS) $(RCOMPILE_FLAGS)
+release: export LDFLAGS := $(LDFLAGS) $(LINK_FLAGS) $(RLINK_FLAGS)
+debug: export CFLAGS := $(CFLAGS) $(COMPILE_FLAGS) $(DCOMPILE_FLAGS)
+debug: export LDFLAGS := $(LDFLAGS) $(LINK_FLAGS) $(DLINK_FLAGS)
+
+# Build and output paths
+release: export BUILD_PATH := build/release
+release: export BIN_PATH := bin/release
+debug: export BUILD_PATH := build/debug
+debug: export BIN_PATH := bin/debug
+install: export BIN_PATH := bin/release
+
+# Find all source files in the source directory, sorted by most
+# recently modified
+ifeq ($(UNAME_S),Darwin)
+	SOURCES = $(shell find $(SRC_PATH) -name '*.$(SRC_EXT)' | sort -k 1nr | cut -f2-)
+else
+	SOURCES = $(shell find $(SRC_PATH) -name '*.$(SRC_EXT)' -printf '%T@\t%p\n' \
+						| sort -k 1nr | cut -f2-)
+endif
+
+# fallback in case the above fails
+rwildcard = $(foreach d, $(wildcard $1*), $(call rwildcard,$d/,$2) \
+						$(filter $(subst *,%,$2), $d))
+ifeq ($(SOURCES),)
+	SOURCES := $(call rwildcard, $(SRC_PATH), *.$(SRC_EXT))
+endif
+
+# Set the object file names, with the source directory stripped
+# from the path, and the build path prepended in its place
+OBJECTS = $(SOURCES:$(SRC_PATH)/%.$(SRC_EXT)=$(BUILD_PATH)/%.o)
+# Set the dependency files that will be used to add header dependencies
+DEPS = $(OBJECTS:.o=.d)
+
+# Macros for timing compilation
+ifeq ($(UNAME_S),Darwin)
+	CUR_TIME = awk 'BEGIN{srand(); print srand()}'
+	TIME_FILE = $(dir $@).$(notdir $@)_time
+	START_TIME = $(CUR_TIME) > $(TIME_FILE)
+	END_TIME = read st < $(TIME_FILE) ; \
+		$(RM) $(TIME_FILE) ; \
+		st=$$((`$(CUR_TIME)` - $$st)) ; \
+		echo $$st
+else
+	TIME_FILE = $(dir $@).$(notdir $@)_time
+	START_TIME = date '+%s' > $(TIME_FILE)
+	END_TIME = read st < $(TIME_FILE) ; \
+		$(RM) $(TIME_FILE) ; \
+		st=$$((`date '+%s'` - $$st - 86400)) ; \
+		echo `date -u -d @$$st '+%H:%M:%S'`
+endif
+
+# Version macros
+# Comment/remove this section to remove versioning
+USE_VERSION := false
+# If this isn't a git repo or the repo has no tags, git describe will return non-zero
+ifeq ($(shell git describe > /dev/null 2>&1 ; echo $$?), 0)
+	USE_VERSION := true
+	VERSION := $(shell git describe --tags --long --dirty --always | \
+		sed 's/v\([0-9]*\)\.\([0-9]*\)\.\([0-9]*\)-\?.*-\([0-9]*\)-\(.*\)/\1 \2 \3 \4 \5/g')
+	VERSION_MAJOR := $(word 1, $(VERSION))
+	VERSION_MINOR := $(word 2, $(VERSION))
+	VERSION_PATCH := $(word 3, $(VERSION))
+	VERSION_REVISION := $(word 4, $(VERSION))
+	VERSION_HASH := $(word 5, $(VERSION))
+	VERSION_STRING := \
+		"$(VERSION_MAJOR).$(VERSION_MINOR).$(VERSION_PATCH).$(VERSION_REVISION)-$(VERSION_HASH)"
+	override CFLAGS := $(CFLAGS) \
+		-D VERSION_MAJOR=$(VERSION_MAJOR) \
+		-D VERSION_MINOR=$(VERSION_MINOR) \
+		-D VERSION_PATCH=$(VERSION_PATCH) \
+		-D VERSION_REVISION=$(VERSION_REVISION) \
+		-D VERSION_HASH=\"$(VERSION_HASH)\"
+endif
+
+# Standard, non-optimized release build
+.PHONY: release
+release: dirs
+ifeq ($(USE_VERSION), true)
+	@echo "Beginning release build v$(VERSION_STRING)"
+else
+	@echo "Beginning release build"
+endif
+	@$(START_TIME)
+	@$(MAKE) all --no-print-directory
+	@echo -n "Total build time: "
+	@$(END_TIME)
+
+# Debug build for gdb debugging
+.PHONY: debug
+debug: dirs
+ifeq ($(USE_VERSION), true)
+	@echo "Beginning debug build v$(VERSION_STRING)"
+else
+	@echo "Beginning debug build"
+endif
+	@$(START_TIME)
+	@$(MAKE) all --no-print-directory
+	@echo -n "Total build time: "
+	@$(END_TIME)
+
+# Create the directories used in the build
+.PHONY: dirs
+dirs:
+	@echo "Creating directories"
+	@mkdir -p $(dir $(OBJECTS))
+	@mkdir -p $(BIN_PATH)
+
+# Installs to the set path
+.PHONY: install
+install:
+	@echo "Installing to $(DESTDIR)$(INSTALL_PREFIX)/bin"
+	@$(INSTALL_PROGRAM) $(BIN_PATH)/$(BIN_NAME) $(DESTDIR)$(INSTALL_PREFIX)/bin
+
+# Uninstalls the program
+.PHONY: uninstall
+uninstall:
+	@echo "Removing $(DESTDIR)$(INSTALL_PREFIX)/bin/$(BIN_NAME)"
+	@$(RM) $(DESTDIR)$(INSTALL_PREFIX)/bin/$(BIN_NAME)
+
+# Removes all build files
+.PHONY: clean
+clean:
+	@echo "Deleting $(BIN_NAME) symlink"
+	@$(RM) $(BIN_NAME)
+	@echo "Deleting directories"
+	@$(RM) -r build
+	@$(RM) -r bin
+
+# Main rule, checks the executable and symlinks to the output
+all: $(BIN_PATH)/$(BIN_NAME)
+	@echo "Making symlink: $(BIN_NAME) -> $<"
+	@$(RM) $(BIN_NAME)
+	@ln -s $(BIN_PATH)/$(BIN_NAME) $(BIN_NAME)
+
+# Link the executable
+$(BIN_PATH)/$(BIN_NAME): $(OBJECTS)
+	@echo "Linking: $@"
+	@$(START_TIME)
+	$(CMD_PREFIX)$(CC) $(OBJECTS) $(LDFLAGS) -o $@
+	@echo -en "\t Link time: "
+	@$(END_TIME)
+
+# Add dependency files, if they exist
+-include $(DEPS)
+
+# Source file rules
+# After the first compilation they will be joined with the rules from the
+# dependency files to provide header dependencies
+$(BUILD_PATH)/%.o: $(SRC_PATH)/%.$(SRC_EXT)
+	@echo "Compiling: $< -> $@"
+	@$(START_TIME)
+	$(CMD_PREFIX)$(CC) $(CFLAGS) $(INCLUDES) -MP -MMD -c $< -o $@
+	@echo -en "\t Compile time: "
+	@$(END_TIME)
diff --git a/quad/computation_graph/src/computation_graph.c b/quad/computation_graph/src/computation_graph.c
new file mode 100644
index 000000000..f5b766174
--- /dev/null
+++ b/quad/computation_graph/src/computation_graph.c
@@ -0,0 +1,118 @@
+#include <stdlib.h>
+#include <string.h>
+#include "computation_graph.h"
+
+#define GRAPH_MAX_DEPTH 20
+
+struct computation_graph *create_graph() {
+    struct computation_graph *the_graph = malloc(sizeof(struct computation_graph));
+    the_graph->nodes = malloc(sizeof(struct graph_node));
+    the_graph->n_nodes = 0;
+    the_graph->size = 1;
+    return the_graph;
+}
+
+void reset_node(int node_id) {
+
+}
+
+int graph_set_source(struct computation_graph *graph,
+                     int dest_cntl_id, int dest_input, int src_cntl_id, int src_output) {
+    if (dest_cntl_id >= graph->n_nodes || src_cntl_id >= graph->n_nodes) {
+        return -1;
+    }
+    struct graph_node *dest_node = &graph->nodes[dest_cntl_id];
+    struct graph_node *src_node = &graph->nodes[src_cntl_id];
+    if (dest_input >= dest_node->n_inputs || src_output >= src_node->n_outputs) {
+        return -1;
+    }
+
+    dest_node->input_srcs[dest_input].controller_id = src_cntl_id;
+    dest_node->input_srcs[dest_input].controller_output = src_output;
+    reset_node(src_cntl_id);
+    return 0;
+}
+
+int graph_create_node(struct computation_graph *graph,
+                      char* name,
+                      int n_inputs,
+                      int n_outputs,
+                      execute_node_t exec_func,
+                      reset_node_t reset_func,
+                      void *state) {
+    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;
+        }
+        graph->size = new_capacity;
+        graph->nodes = node_arr;
+    }
+    struct graph_node *new_node = &graph->nodes[new_id];
+    new_node->name = strdup(name);
+    new_node->output_values = malloc(n_outputs * sizeof(double));
+    new_node->input_values = malloc(n_inputs * sizeof(double));
+    new_node->state = state;
+    new_node->execute = exec_func;
+    new_node->reset = reset_func;
+    new_node->n_inputs = n_inputs;
+    new_node->n_outputs = n_outputs;
+    new_node->input_srcs = malloc(n_inputs * sizeof(struct input_type));
+    int i;
+    for (i = 0; i < n_inputs; i++) {
+        new_node->input_srcs[i].controller_id = -1;
+    }
+    graph->n_nodes += 1;
+    return new_id;
+}
+
+int graph_set_input_val(struct computation_graph *graph, int node_id, int input_id, double value) {
+    if (node_id >= graph->n_nodes || input_id >= graph->nodes[node_id].n_inputs) {
+        return -1;
+    }
+    graph->nodes[node_id].input_values[input_id] = value;
+    return 0;
+}
+
+double graph_get_output(struct computation_graph *graph, int node_id, int output_id) {
+    if (node_id >= graph->n_nodes || output_id >= graph->nodes[node_id].n_outputs) {
+        return -1;
+    }
+    return graph->nodes[node_id].output_values[output_id];
+}
+
+void graph_compute_node_rec(struct computation_graph *graph, int node_id, int depth) {
+    if (depth >= GRAPH_MAX_DEPTH) {
+        return;
+    }
+    if (node_id >= graph->n_nodes) {
+        return;
+    }
+    struct graph_node *node = &graph->nodes[node_id];
+    if (node->processed_state == PROCESSED) {
+        return;
+    }
+    int input_id;
+    for (input_id = 0; input_id < node->n_inputs; input_id++) {
+        int src_cntl_id = node->input_srcs[input_id].controller_id;
+        if (src_cntl_id != -1) {
+            if (graph->nodes[src_cntl_id].processed_state == UNPROCESSED) {
+                graph_compute_node_rec(graph, src_cntl_id, depth + 1);
+            }
+            int src_output_id = node->input_srcs[input_id].controller_output;
+            node->input_values[input_id] = graph->nodes[src_cntl_id].output_values[src_output_id];
+        }
+    }
+    (*node->execute)(node->state, node->input_values, node->output_values);
+    node->processed_state = PROCESSED;
+}
+
+void graph_compute_node(struct computation_graph *graph, int node_id) {
+    int i;
+    for (i = 0; i < graph->n_nodes; i++) {
+        graph->nodes[i].processed_state = UNPROCESSED;
+    }
+    graph_compute_node_rec(graph, node_id, 0);
+}
diff --git a/quad/computation_graph/src/computation_graph.h b/quad/computation_graph/src/computation_graph.h
new file mode 100644
index 000000000..2f1cc1543
--- /dev/null
+++ b/quad/computation_graph/src/computation_graph.h
@@ -0,0 +1,54 @@
+#ifndef __COMPUTATION_GRAPH_H__
+#define __COMPUTATION_GRAPH_H__
+
+typedef void (*execute_node_t)(void *state, double *inputs, double *outputs);
+
+typedef void (*reset_node_t)(void *state);
+
+enum node_processed_state {
+    UNPROCESSED,
+    DISCOVERED,
+    PROCESSED
+};
+
+struct computation_graph {
+    int n_nodes;
+    int size;
+    struct graph_node *nodes;
+};
+
+struct graph_node {
+    char *name;
+    double *output_values;
+    double *input_values;
+    int n_inputs;
+    int n_outputs;
+    void *state;
+    execute_node_t execute;
+    reset_node_t reset;
+    int processed_state;
+    struct input_type {
+        int controller_id;
+        int controller_output;
+    } *input_srcs;
+};
+
+struct computation_graph *create_graph();
+
+int graph_set_source(struct computation_graph *graph, int dest_cntl, int dest_input, int src_cntl, int src_output);
+
+int graph_create_node(struct computation_graph *graph,
+                      char* name,
+                      int n_inputs,
+                      int n_outputs,
+                      execute_node_t exec_func,
+                      reset_node_t reset_func,
+                      void *state);
+
+double graph_get_output(struct computation_graph *graph, int node_id, int output_id);
+
+int graph_set_input_val(struct computation_graph *graph, int node_id, int input_id, double value);
+
+void graph_compute_node(struct computation_graph *graph, int node_id);
+
+#endif // __COMPUTATION_GRAPH_H__
diff --git a/quad/computation_graph/src/main.c b/quad/computation_graph/src/main.c
new file mode 100644
index 000000000..2080d95a8
--- /dev/null
+++ b/quad/computation_graph/src/main.c
@@ -0,0 +1,25 @@
+#include <stdio.h>
+#include "computation_graph.h"
+#include "node_add.h"
+#include "node_mult.h"
+
+int main() {
+    struct computation_graph *graph = create_graph();
+    int add2_id = graph_add_node_add2(graph, "add1");
+    graph_set_input_val(graph, add2_id, ADD2_SUMMAND1, 3);
+    graph_set_input_val(graph, add2_id, ADD2_SUMMAND2, 2);
+
+    int add3_id = graph_add_node_add2(graph, "add2");
+    graph_set_input_val(graph, add3_id, ADD2_SUMMAND2, 5);
+    graph_set_source(graph, add3_id, ADD2_SUMMAND1, add2_id, ADD2_SUM);
+
+    int mult1_id = graph_add_node_mult(graph, "add3");
+    graph_set_input_val(graph, mult1_id, MULT_MULTIPLICAND1, 4);
+    graph_set_source(graph, mult1_id, MULT_MULTIPLICAND2, add3_id, ADD2_SUM);
+
+
+    graph_compute_node(graph, mult1_id);
+    printf("Sum is %f\n", graph_get_output(graph, mult1_id, ADD2_SUM));
+    fflush(stdout);
+    return 0;
+}
diff --git a/quad/computation_graph/src/node_add.h b/quad/computation_graph/src/node_add.h
new file mode 100644
index 000000000..3f7183f1b
--- /dev/null
+++ b/quad/computation_graph/src/node_add.h
@@ -0,0 +1,12 @@
+#include "computation_graph.h"
+
+int graph_add_node_add2(struct computation_graph *graph, char* name);
+
+enum graph_node_add2_inputs {
+    ADD2_SUMMAND1,
+    ADD2_SUMMAND2
+};
+
+enum graph_node_add2_outputs {
+    ADD2_SUM
+};
diff --git a/quad/computation_graph/src/node_add2.c b/quad/computation_graph/src/node_add2.c
new file mode 100644
index 000000000..3619967e6
--- /dev/null
+++ b/quad/computation_graph/src/node_add2.c
@@ -0,0 +1,12 @@
+#include "node_add.h"
+#include <stdlib.h>
+
+static void add_nodes(void *state, double *inputs, double *outputs) {
+    outputs[ADD2_SUM] = inputs[ADD2_SUMMAND1] + inputs[ADD2_SUMMAND2];
+}
+
+static void reset(void *state) {}
+
+int graph_add_node_add2(struct computation_graph *graph, char* name) {
+    return graph_create_node(graph, name, 2, 1, &add_nodes, &reset, NULL);
+}
diff --git a/quad/computation_graph/src/node_mult.c b/quad/computation_graph/src/node_mult.c
new file mode 100644
index 000000000..b8db3f1a4
--- /dev/null
+++ b/quad/computation_graph/src/node_mult.c
@@ -0,0 +1,12 @@
+#include "node_mult.h"
+#include <stdlib.h>
+
+static void mult_nodes(void *state, double *inputs, double *outputs) {
+    outputs[MULT_PRODUCT] = inputs[MULT_MULTIPLICAND1] * inputs[MULT_MULTIPLICAND2];
+}
+
+static void reset(void *state) {}
+
+int graph_add_node_mult(struct computation_graph *graph, char* name) {
+    return graph_create_node(graph, name, 2, 1, &mult_nodes, &reset, NULL);
+}
diff --git a/quad/computation_graph/src/node_mult.h b/quad/computation_graph/src/node_mult.h
new file mode 100644
index 000000000..51d41f73d
--- /dev/null
+++ b/quad/computation_graph/src/node_mult.h
@@ -0,0 +1,12 @@
+#include "computation_graph.h"
+
+int graph_add_node_mult(struct computation_graph *graph, char* name);
+
+enum graph_node_mult_inputs {
+    MULT_MULTIPLICAND1,
+    MULT_MULTIPLICAND2
+};
+
+enum graph_node_mult_outputs {
+    MULT_PRODUCT
+};
-- 
GitLab