diff --git a/.gitignore b/.gitignore
index 358e24b..1ab68b6 100644
--- a/.gitignore
+++ b/.gitignore
@@ -138,6 +138,8 @@ gradle-app.setting
 
 # Ignore GraphHopper generated files
 src/main/resources/ny_capital_district-gh
+src/main/resources/galway_ny-gh
+
 
 # Ignore Google Optimization Tools libs
 /libs/
diff --git a/build.gradle b/build.gradle
index 9041ef4..dcb746e 100644
--- a/build.gradle
+++ b/build.gradle
@@ -16,4 +16,8 @@ dependencies {
     compile fileTree(dir: System.getenv('GUROBI_HOME') + '/lib', include: '*.jar')
     testCompile group: 'junit', name: 'junit', version: '4.12'
     compile group: 'com.graphhopper', name: 'graphhopper-reader-osm', version: '0.9.0'
+}
+
+test {
+    testLogging.showStandardStreams = true
 }
\ No newline at end of file
diff --git a/src/main/java/io/github/plastix/Constraints.java b/src/main/java/io/github/plastix/Constraints.java
new file mode 100644
index 0000000..14ecda3
--- /dev/null
+++ b/src/main/java/io/github/plastix/Constraints.java
@@ -0,0 +1,103 @@
+package io.github.plastix;
+
+import com.graphhopper.routing.util.AllEdgesIterator;
+import com.graphhopper.storage.Graph;
+import com.graphhopper.util.EdgeIterator;
+import gurobi.*;
+
+public class Constraints {
+
+    private Graph graph;
+    private GRBModel model;
+    private Vars vars;
+    private GraphUtils graphUtils;
+
+    private GRBLinExpr maxCostConstraint;
+    private GRBLinExpr objective;
+
+    public Constraints(Graph graph, GRBModel model, Vars vars, GraphUtils graphUtils) {
+        this.graph = graph;
+        this.model = model;
+        this.vars = vars;
+        this.graphUtils = graphUtils;
+    }
+
+    public void setupConstraints(int startNodeId, double maxCostMeters) throws GRBException {
+
+        // (1a)
+        // Objective maximizes total collected score of all roads
+        objective = new GRBLinExpr();
+
+        // (1b)
+        // Limit length of path
+        maxCostConstraint = new GRBLinExpr();
+
+        AllEdgesIterator edges = graph.getAllEdges();
+        while(edges.next()) {
+            double edgeScore = graphUtils.getArcScore(edges);
+            double edgeDist = edges.getDistance();
+
+            GRBVar forward = vars.getArcVar(edges, false);
+            GRBVar backward = vars.getArcVar(edges, true);
+
+            objective.addTerm(edgeScore, forward);
+            objective.addTerm(edgeScore, backward);
+            maxCostConstraint.addTerm(edgeDist, forward);
+            maxCostConstraint.addTerm(edgeDist, backward);
+
+            // (1j)
+            GRBLinExpr arcConstraint = new GRBLinExpr();
+            arcConstraint.addTerm(1, forward);
+            arcConstraint.addTerm(1, backward);
+            model.addConstr(arcConstraint, GRB.LESS_EQUAL, 1, "arc_constraint");
+        }
+
+        model.setObjective(objective, GRB.MAXIMIZE);
+        model.addConstr(maxCostConstraint, GRB.LESS_EQUAL, maxCostMeters, "max_cost");
+
+        int numNodes = graph.getNodes();
+        for(int i = 0; i < numNodes; i++) {
+            // (1d)
+            GRBLinExpr edgeCounts = new GRBLinExpr();
+            EdgeIterator incoming = graphUtils.incomingEdges(i);
+            while(incoming.next()) {
+                edgeCounts.addTerm(1, vars.getArcVar(incoming, true));
+            }
+
+            EdgeIterator outgoing = graphUtils.outgoingEdges(i);
+            while(outgoing.next()) {
+                GRBVar arc = vars.getArcVar(outgoing, false);
+                edgeCounts.addTerm(-1, arc);
+            }
+
+            model.addConstr(edgeCounts, GRB.EQUAL, 0, "edge_counts");
+
+            // (1e)
+            GRBLinExpr vertexVisits = new GRBLinExpr();
+            outgoing = graphUtils.outgoingEdges(i);
+            while(outgoing.next()) {
+                vertexVisits.addTerm(1, vars.getArcVar(outgoing, false));
+            }
+            vertexVisits.addTerm(-1, vars.getVertexVar(i));
+            model.addConstr(vertexVisits, GRB.EQUAL, 0, "vertex_visits");
+        }
+
+        // (1h)/(1i)
+        // Start vertex must be visited exactly once
+        GRBVar startNodeVar = vars.getVertexVar(startNodeId);
+        startNodeVar.set(GRB.DoubleAttr.LB, 1);
+        startNodeVar.set(GRB.DoubleAttr.UB, 1);
+
+        // Must set LazyConstraints parameter when using lazy constraints
+        model.set(GRB.IntParam.LazyConstraints, 1);
+        model.setCallback(new SubtourConstraint(vars, startNodeId, graphUtils));
+    }
+
+    public GRBLinExpr getMaxCostConstraint() {
+        return maxCostConstraint;
+    }
+
+    public GRBLinExpr getObjective() {
+        return objective;
+    }
+}
diff --git a/src/main/java/io/github/plastix/GraphUtils.java b/src/main/java/io/github/plastix/GraphUtils.java
index be5f2e1..6bb9897 100644
--- a/src/main/java/io/github/plastix/GraphUtils.java
+++ b/src/main/java/io/github/plastix/GraphUtils.java
@@ -1,29 +1,23 @@
 package io.github.plastix;
 
 import com.graphhopper.routing.util.DefaultEdgeFilter;
-import com.graphhopper.routing.util.EncodingManager;
 import com.graphhopper.routing.util.FlagEncoder;
+import com.graphhopper.routing.weighting.Weighting;
 import com.graphhopper.storage.Graph;
-import com.graphhopper.storage.index.LocationIndex;
-import com.graphhopper.storage.index.QueryResult;
 import com.graphhopper.util.EdgeExplorer;
 import com.graphhopper.util.EdgeIterator;
 import com.graphhopper.util.EdgeIteratorState;
 
 public class GraphUtils {
 
-    private Params params;
     private Graph graph;
-    private LocationIndex locationIndex;
     private FlagEncoder flagEncoder;
-    private BikePriorityWeighting weighting;
+    private Weighting weighting;
 
-    GraphUtils(Graph graph, LocationIndex locationIndex, EncodingManager encodingManager, Params params) {
+    public GraphUtils(Graph graph, FlagEncoder flagEncoder, Weighting weighting) {
         this.graph = graph;
-        this.locationIndex = locationIndex;
-        this.flagEncoder = encodingManager.getEncoder(params.getVehicle());
-        this.params = params;
-        weighting = new BikePriorityWeighting(flagEncoder);
+        this.flagEncoder = flagEncoder;
+        this.weighting = weighting;
     }
 
     public EdgeIterator outgoingEdges(int node) {
@@ -50,16 +44,6 @@ public double getArcScore(EdgeIteratorState edge) {
         return weighting.calcWeight(edge, false, edge.getEdge());
     }
 
-    public int getStartNode() {
-        QueryResult result = locationIndex.findClosest(params.getStartLat(), params.getStartLon(),
-                new DefaultEdgeFilter(flagEncoder));
-        if(!result.isValid()) {
-            throw new RuntimeException("Unable to find node at start lat/lon!");
-        }
-        return result.getClosestNode();
-
-    }
-
     public boolean isForward(EdgeIteratorState edge) {
         return edge.isForward(flagEncoder);
     }
diff --git a/src/main/java/io/github/plastix/Main.java b/src/main/java/io/github/plastix/Main.java
index 9010835..29b429b 100644
--- a/src/main/java/io/github/plastix/Main.java
+++ b/src/main/java/io/github/plastix/Main.java
@@ -1,13 +1,16 @@
 package io.github.plastix;
 
-import com.carrotsearch.hppc.IntHashSet;
 import com.graphhopper.GraphHopper;
 import com.graphhopper.reader.osm.GraphHopperOSM;
 import com.graphhopper.routing.util.AllEdgesIterator;
 import com.graphhopper.routing.util.EncodingManager;
+import com.graphhopper.routing.util.FlagEncoder;
+import com.graphhopper.routing.weighting.Weighting;
 import com.graphhopper.storage.Graph;
-import com.graphhopper.util.EdgeIterator;
-import gurobi.*;
+import gurobi.GRB;
+import gurobi.GRBEnv;
+import gurobi.GRBException;
+import gurobi.GRBModel;
 
 public class Main {
 
@@ -22,86 +25,16 @@ public class Main {
     // Solver variables
     private static GRBEnv env;
     private static GRBModel model;
+    private static Vars vars;
+    private static Constraints constraints;
 
     private static void setupSolver() throws GRBException {
         env = new GRBEnv("osm.log");
         model = new GRBModel(env);
-        Vars vars = new Vars(graph, model, graphUtils);
-
-        // (1a)
-        // Objective maximizes total collected score of all roads
-        GRBLinExpr objective = new GRBLinExpr();
-
-        // (1b)
-        // Limit length of path
-        GRBLinExpr maxCost = new GRBLinExpr();
-
-        AllEdgesIterator edges = graph.getAllEdges();
-        while(edges.next()) {
-            double edgeScore = graphUtils.getArcScore(edges);
-            double edgeDist = edges.getDistance();
-
-            GRBVar forward = vars.getArcVar(edges);
-            GRBVar backward = vars.getComplementArcVar(edges);
-
-            objective.addTerm(edgeScore, forward);
-            objective.addTerm(edgeScore, backward);
-            maxCost.addTerm(edgeDist, forward);
-            maxCost.addTerm(edgeDist, backward);
-
-            // (1j)
-            GRBLinExpr arcConstraint = new GRBLinExpr();
-            arcConstraint.addTerm(1, forward);
-            arcConstraint.addTerm(1, backward);
-            model.addConstr(arcConstraint, GRB.LESS_EQUAL, 1, "arc_constraint");
-        }
-
-        model.setObjective(objective, GRB.MAXIMIZE);
-        model.addConstr(maxCost, GRB.LESS_EQUAL, params.getMaxCost(), "max_cost");
-
-        int numNodes = graph.getNodes();
-        for(int i = 0; i < numNodes; i++) {
-            // (1d)
-            GRBLinExpr edgeCounts = new GRBLinExpr();
-            IntHashSet incomingIds = new IntHashSet();
-            EdgeIterator incoming = graphUtils.incomingEdges(i);
-            while(incoming.next()) {
-                incomingIds.add(incoming.getEdge());
-                edgeCounts.addTerm(1, vars.getArcVar(incoming));
-            }
-
-            EdgeIterator outgoing = graphUtils.outgoingEdges(i);
-            while(outgoing.next()) {
-                GRBVar arc = vars.getArcVar(outgoing);
-                // Check if we already recorded it as an incoming edge
-                if(incomingIds.contains(outgoing.getEdge())) {
-                    edgeCounts.remove(arc);
-                } else {
-                    edgeCounts.addTerm(-1, arc);
-                }
-            }
-
-            model.addConstr(edgeCounts, GRB.EQUAL, 0, "edge_counts");
-
-            // (1e)
-            GRBLinExpr vertexVisits = new GRBLinExpr();
-            outgoing = graphUtils.outgoingEdges(i);
-            while(outgoing.next()) {
-                vertexVisits.addTerm(1, vars.getArcVar(outgoing));
-            }
-            vertexVisits.addTerm(-1, vars.getVertexVar(i));
-            model.addConstr(vertexVisits, GRB.EQUAL, 0, "vertex_visits");
-        }
-
-        // (1h)/(1i)
-        // Start vertex must be visited exactly once
-        GRBVar startNode = vars.getVertexVar(START_NODE_ID);
-        startNode.set(GRB.DoubleAttr.LB, 1);
-        startNode.set(GRB.DoubleAttr.UB, 1);
-
-        // Must set LazyConstraints parameter when using lazy constraints
-        model.set(GRB.IntParam.LazyConstraints, 1);
-        model.setCallback(new SubtourConstraint(vars, START_NODE_ID, graphUtils));
+        vars = new Vars(graph, model, graphUtils);
+        constraints = new Constraints(graph, model, vars, graphUtils);
+        vars.addVarsToModel();
+        constraints.setupConstraints(START_NODE_ID, params.getMaxCost());
     }
 
     private static void runSolver() throws GRBException {
@@ -110,10 +43,16 @@ private static void runSolver() throws GRBException {
                 " (Node " + START_NODE_ID + ")");
 
 //        model.set(GRB.IntParam.LogToConsole, 0);
+        model.set(GRB.IntParam.Threads, params.getNumThreads());
         model.optimize();
 
-        env.dispose();
+        if(model.get(GRB.IntAttr.Status) == GRB.Status.OPTIMAL) {
+            System.out.println("Route score: " + constraints.getObjective().getValue());
+            System.out.println("Route distance: " + constraints.getMaxCostConstraint().getValue());
+        }
+
         model.dispose();
+        env.dispose();
         hopper.close();
     }
 
@@ -129,9 +68,11 @@ private static void loadOSM() {
         hopper.setCHEnabled(false);
         hopper.importOrLoad();
 
-        graph = hopper.getGraphHopperStorage().getBaseGraph();
-        graphUtils = new GraphUtils(graph, hopper.getLocationIndex(), encodingManager, params);
-        START_NODE_ID = graphUtils.getStartNode();
+        FlagEncoder flagEncoder = encodingManager.getEncoder(params.getVehicle());
+        Weighting weighting = new BikePriorityWeighting(flagEncoder);
+        graph = hopper.getGraphHopperStorage();
+        graphUtils = new GraphUtils(graph, flagEncoder, weighting);
+        START_NODE_ID = params.getStartNode(hopper.getLocationIndex(), flagEncoder);
 
         AllEdgesIterator edges = graph.getAllEdges();
         int nonTraversable = 0;
diff --git a/src/main/java/io/github/plastix/Params.java b/src/main/java/io/github/plastix/Params.java
index 7929d48..a01f0d2 100644
--- a/src/main/java/io/github/plastix/Params.java
+++ b/src/main/java/io/github/plastix/Params.java
@@ -1,5 +1,10 @@
 package io.github.plastix;
 
+import com.graphhopper.routing.util.DefaultEdgeFilter;
+import com.graphhopper.routing.util.FlagEncoder;
+import com.graphhopper.storage.index.LocationIndex;
+import com.graphhopper.storage.index.QueryResult;
+
 import java.io.FileInputStream;
 import java.io.FileNotFoundException;
 import java.io.IOException;
@@ -14,26 +19,32 @@ public class Params {
     private double START_LAT;
     private double START_LON;
     private String VEHICLE;
+    private int NUM_THREADS;
 
     public void loadParams() {
         Properties properties = new Properties();
         InputStream inputStream;
         try {
-            inputStream = new FileInputStream("src/main/resources/params.properties");
+            String paramPath = getClass().getResource("/params.properties").getPath();
+            inputStream = new FileInputStream(paramPath);
             properties.load(inputStream);
+            GRAPH_FILE = properties.getProperty("graphFile");
+            GRAPH_FOLDER = properties.getProperty("graphFolder");
+            START_LAT = Double.parseDouble(properties.getProperty("startLat"));
+            START_LON = Double.parseDouble(properties.getProperty("startLon"));
+            MAX_COST = Double.parseDouble(properties.getProperty("maxCost"));
+            VEHICLE = properties.getProperty("vehicle");
+            NUM_THREADS = Integer.parseInt(properties.getProperty("numThreads"));
         } catch(FileNotFoundException e) {
             System.out.println("No params file!");
             e.printStackTrace();
         } catch(IOException e) {
             System.out.println("Error reading params!");
             e.printStackTrace();
+        } catch(NumberFormatException e) {
+            System.out.println("Invalid parameter");
+            e.printStackTrace();
         }
-        GRAPH_FILE = properties.getProperty("graphFile");
-        GRAPH_FOLDER = properties.getProperty("graphFolder");
-        START_LAT = Double.parseDouble(properties.getProperty("startLat"));
-        START_LON = Double.parseDouble(properties.getProperty("startLon"));
-        MAX_COST = Double.parseDouble(properties.getProperty("maxCost"));
-        VEHICLE = properties.getProperty("vehicle");
     }
 
     public String getGraphFile() {
@@ -59,4 +70,17 @@ public double getStartLon() {
     public String getVehicle() {
         return VEHICLE;
     }
+
+    public int getNumThreads() {
+        return NUM_THREADS;
+    }
+
+    public int getStartNode(LocationIndex locationIndex, FlagEncoder flagEncoder) {
+        QueryResult result = locationIndex.findClosest(START_LAT, START_LON,
+                new DefaultEdgeFilter(flagEncoder));
+        if(!result.isValid()) {
+            throw new RuntimeException("Unable to find node at start lat/lon!");
+        }
+        return result.getClosestNode();
+    }
 }
diff --git a/src/main/java/io/github/plastix/SubtourConstraint.java b/src/main/java/io/github/plastix/SubtourConstraint.java
index f4ebd9f..7c97aef 100644
--- a/src/main/java/io/github/plastix/SubtourConstraint.java
+++ b/src/main/java/io/github/plastix/SubtourConstraint.java
@@ -5,10 +5,9 @@
 import com.carrotsearch.hppc.cursors.IntCursor;
 import com.graphhopper.util.EdgeExplorer;
 import com.graphhopper.util.EdgeIterator;
-import gurobi.GRB;
-import gurobi.GRBCallback;
-import gurobi.GRBException;
-import gurobi.GRBLinExpr;
+import gurobi.*;
+
+import java.util.Arrays;
 
 public class SubtourConstraint extends GRBCallback {
 
@@ -27,31 +26,43 @@ protected void callback() {
         try {
             if(where == GRB.CB_MIPSOL) { // Found an integer feasible solution
 
+                IntHashSet solutionVertices = getSolutionVertices();
                 IntHashSet visitedVertices = getReachableVertexSubset(START_NODE_ID);
-                int numVerticesInSolution = numVerticesInSolution();
+
+//                System.out.println("--- Callback ---");
+//                System.out.println("Verts in solution: " + numVerticesInSolution);
+//                System.out.println(solutionVertices);
+//                System.out.println("Reachable vertices: " + visitedVertices.size());
+//                printSolution();
 
                 // If the number of vertices we can reach from the start is not the number of vertices we
                 // visit in the entire solution, we have a disconnected tour
-                if(visitedVertices.size() != numVerticesInSolution) {
+                if(visitedVertices.size() < solutionVertices.size()) {
+                    solutionVertices.removeAll(visitedVertices);
 
                     // Add sub-tour elimination constraint
                     GRBLinExpr subtourConstraint = new GRBLinExpr();
                     int sumVertexVisits = 0;
                     int totalOutgoingEdges = 0;
 
-                    for(IntCursor cursor : visitedVertices) {
+                    double lhs = 0;
+                    for(IntCursor cursor : solutionVertices) {
                         int vertexId = cursor.value;
                         EdgeIterator outgoing = graphUtils.outgoingEdges(vertexId);
 
                         while(outgoing.next()) {
-                            subtourConstraint.addTerm(1, vars.getArcVar(outgoing));
+                            GRBVar var = vars.getArcVar(outgoing, false);
+                            if(!solutionVertices.contains(outgoing.getAdjNode())) {
+                                subtourConstraint.addTerm(1, var);
+                                lhs += getSolution(var);
+                            }
                             totalOutgoingEdges += 1;
                         }
-
                         sumVertexVisits += getSolution(vars.getVertexVar(vertexId));
                     }
 
                     double rhs = ((double) sumVertexVisits) / ((double) totalOutgoingEdges);
+//                    System.out.println("adding lazy constraint! " + lhs + " >= " + rhs);
                     addLazy(subtourConstraint, GRB.GREATER_EQUAL, rhs);
 
                 }
@@ -63,16 +74,17 @@ protected void callback() {
         }
     }
 
-    private int numVerticesInSolution() throws GRBException {
-        double[] values = getSolution(vars.getVertexVars());
+    private IntHashSet getSolutionVertices() throws GRBException {
+        IntHashSet result = new IntHashSet();
+        GRBVar[] verts = vars.getVertexVars();
+        double[] values = getSolution(verts);
 
-        int visited = 0;
-        for(double value : values) {
-            if(value > 0) {
-                visited++;
+        for(int i = 0; i < verts.length; i++) {
+            if(values[i] > 0) {
+                result.add(i);
             }
         }
-        return visited;
+        return result;
     }
 
     private IntHashSet getReachableVertexSubset(int startNode) throws GRBException {
@@ -88,7 +100,7 @@ private IntHashSet getReachableVertexSubset(int startNode) throws GRBException {
                 EdgeIterator iter = explorer.setBaseNode(current);
                 while(iter.next()) {
                     int connectedId = iter.getAdjNode();
-                    if(getSolution(vars.getArcVar(iter)) > 0) {
+                    if(getSolution(vars.getArcVar(iter, false)) > 0) {
                         stack.addLast(connectedId);
                     }
                 }
@@ -98,4 +110,23 @@ private IntHashSet getReachableVertexSubset(int startNode) throws GRBException {
 
         return explored;
     }
+
+    private void printSolution() throws GRBException {
+        GRBVar[] arcVars = vars.getArcVars();
+        double[] values = getSolution(arcVars);
+
+        StringBuilder arcString = new StringBuilder();
+
+        for(int i = 0; i < arcVars.length - 1; i++) {
+            arcString.append(values[i]);
+            arcString.append(", ");
+            arcString.append(arcVars[i].get(GRB.StringAttr.VarName));
+            if(i < arcVars.length - 2) {
+                arcString.append("\n");
+            }
+        }
+        System.out.println("Arcs: " + arcString.toString());
+        double[] verts = getSolution(vars.getVertexVars());
+        System.out.println("Verts: " + Arrays.toString(verts));
+    }
 }
diff --git a/src/main/java/io/github/plastix/Vars.java b/src/main/java/io/github/plastix/Vars.java
index 4ce29b1..aef978f 100644
--- a/src/main/java/io/github/plastix/Vars.java
+++ b/src/main/java/io/github/plastix/Vars.java
@@ -1,5 +1,10 @@
 package io.github.plastix;
 
+import com.carrotsearch.hppc.IntIntHashMap;
+import com.carrotsearch.hppc.IntIntMap;
+import com.carrotsearch.hppc.IntObjectHashMap;
+import com.carrotsearch.hppc.IntObjectMap;
+import com.carrotsearch.hppc.cursors.IntCursor;
 import com.graphhopper.routing.util.AllEdgesIterator;
 import com.graphhopper.storage.Graph;
 import com.graphhopper.util.EdgeIterator;
@@ -16,29 +21,24 @@ public class Vars {
     private GraphUtils graphUtils;
     private GRBModel model;
 
-    // arcs[arcId][0] = forwardArc
-    // arcs[arcId][1] = backwardArc
-    private GRBVar[][] arcs;
     private GRBVar[] verts;
-    private int[] arcBaseIds;
+    private IntObjectMap<GRBVar> forwardArcs;
+    private IntObjectMap<GRBVar> backwardArcs;
+    private IntIntMap arcBaseIds; // Records original "direction" of arc when processed.
 
-    Vars(Graph graph, GRBModel model, GraphUtils graphUtils) throws GRBException {
+    public Vars(Graph graph, GRBModel model, GraphUtils graphUtils) {
         this.graph = graph;
         this.model = model;
         this.graphUtils = graphUtils;
-        addVarsToModel();
+
+        backwardArcs = new IntObjectHashMap<>();
+        forwardArcs = new IntObjectHashMap<>();
+        arcBaseIds = new IntIntHashMap();
     }
 
-    private void addVarsToModel() throws GRBException {
+    public void addVarsToModel() throws GRBException {
         AllEdgesIterator edges = graph.getAllEdges();
-        int numEdges = edges.getMaxId();
         int numNodes = graph.getNodes();
-        arcBaseIds = new int[numEdges];
-
-        // Make a decision variable for every arc in our graph
-        // arcs[i] = 1 if arc is travelled, 0 otherwise
-        GRBVar[] arcVars = model.addVars(2 * numEdges, GRB.BINARY);
-        arcs = new GRBVar[numEdges][2];
 
         // Make a variable for every node in the graph
         // verts[i] = n the number of times vertex i is visited
@@ -47,17 +47,19 @@ private void addVarsToModel() throws GRBException {
         verts = model.addVars(null, null, null, types,
                 null, 0, numNodes);
 
-        int i = 0;
         while(edges.next()) {
             int edgeId = edges.getEdge();
             int baseNode = edges.getBaseNode();
-            arcBaseIds[edgeId] = baseNode;
+            arcBaseIds.put(edgeId, baseNode);
+            int adjNode = edges.getAdjNode();
 
-            GRBVar forward = arcVars[i++];
-            GRBVar backward = arcVars[i++];
+            // Make a decision variable for every arc in our graph
+            // arcs[i] = 1 if arc is travelled, 0 otherwise
+            GRBVar forward = model.addVar(0, 1, 0, GRB.BINARY, "forward_" + edgeId + "|" + baseNode + "->" + edges.getAdjNode());
+            GRBVar backward = model.addVar(0, 1, 0, GRB.BINARY, "backward_" + edgeId + "|" + baseNode + "->" + edges.getAdjNode());
 
-            arcs[edgeId][0] = forward;
-            arcs[edgeId][1] = backward;
+            forwardArcs.put(edgeId, forward);
+            backwardArcs.put(edgeId, backward);
 
             if(!graphUtils.isForward(edges)) {
                 forward.set(GRB.DoubleAttr.UB, 0);
@@ -69,23 +71,39 @@ private void addVarsToModel() throws GRBException {
         }
     }
 
-    private int getIndex(EdgeIterator edge) {
-        return arcBaseIds[edge.getEdge()] == edge.getBaseNode() ? 0 : 1;
+    private IntObjectMap<GRBVar> getMap(EdgeIterator edge, boolean reverse) {
+        int baseNode = edge.getBaseNode();
+        if(reverse) {
+            baseNode = edge.getAdjNode();
+        }
+        return arcBaseIds.get(edge.getEdge()) == baseNode ? forwardArcs : backwardArcs;
     }
 
-    public GRBVar getArcVar(EdgeIterator edge) {
-        return arcs[edge.getEdge()][getIndex(edge)];
-    }
 
-    public GRBVar getComplementArcVar(EdgeIterator edge) {
-        return arcs[edge.getEdge()][getIndex(edge) ^ 1];
+    public GRBVar getArcVar(EdgeIterator edge, boolean reverse) {
+        return getMap(edge, reverse).get(edge.getEdge());
     }
 
     public GRBVar getVertexVar(int id) {
+        if(id < 0 || id >= graph.getNodes()) {
+            throw new IllegalArgumentException(String.format("Invalid node id %d", id));
+        }
         return verts[id];
     }
 
     public GRBVar[] getVertexVars() {
         return verts;
     }
+
+    public GRBVar[] getArcVars() {
+        GRBVar[] result = new GRBVar[forwardArcs.values().size() * 2];
+
+        int j = 0;
+        for(IntCursor intCursor : forwardArcs.keys()) {
+            result[j++] = forwardArcs.get(intCursor.value);
+            result[j++] = backwardArcs.get(intCursor.value);
+        }
+
+        return result;
+    }
 }
diff --git a/src/main/resources/galway_ny.pbf b/src/main/resources/galway_ny.pbf
new file mode 100644
index 0000000..4a6b5a4
Binary files /dev/null and b/src/main/resources/galway_ny.pbf differ
diff --git a/src/main/resources/params.properties b/src/main/resources/params.properties
index 9dcd291..5c2c385 100644
--- a/src/main/resources/params.properties
+++ b/src/main/resources/params.properties
@@ -1,6 +1,7 @@
-graphFile=src/main/resources/ny_capital_district.pbf
-graphFolder=src/main/resources/ny_capital_district-gh/
+graphFile=src/main/resources/galway_ny.pbf
+graphFolder=src/main/resources/galway_ny-gh/
 vehicle=racingbike
 maxCost=40000
-startLat=43.009449
-startLon=-74.006824
\ No newline at end of file
+startLat=43.009339
+startLon=-74.009168
+numThreads=4
\ No newline at end of file
diff --git a/src/test/java/SimpleGraphTests.java b/src/test/java/SimpleGraphTests.java
new file mode 100644
index 0000000..57f2c8e
--- /dev/null
+++ b/src/test/java/SimpleGraphTests.java
@@ -0,0 +1,329 @@
+import com.carrotsearch.hppc.IntDoubleHashMap;
+import com.carrotsearch.hppc.IntDoubleMap;
+import com.graphhopper.routing.util.EncodingManager;
+import com.graphhopper.routing.util.FlagEncoder;
+import com.graphhopper.routing.util.HintsMap;
+import com.graphhopper.routing.util.RacingBikeFlagEncoder;
+import com.graphhopper.routing.weighting.Weighting;
+import com.graphhopper.storage.GraphBuilder;
+import com.graphhopper.storage.GraphHopperStorage;
+import com.graphhopper.util.EdgeIteratorState;
+import gurobi.*;
+import io.github.plastix.Constraints;
+import io.github.plastix.GraphUtils;
+import io.github.plastix.Vars;
+import org.junit.After;
+import org.junit.Before;
+import org.junit.Test;
+
+import java.util.Arrays;
+
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.fail;
+
+@SuppressWarnings("WeakerAccess")
+public class SimpleGraphTests {
+
+    static final double FP_PRECISION = 0.01;
+
+    GRBEnv env;
+    GRBModel model;
+    Vars vars;
+    Constraints constraints;
+
+    GraphHopperStorage graph;
+    GraphUtils graphUtils;
+    FlagEncoder flagEncoder;
+    IntDoubleMap weights;
+
+    @Before
+    public void setUp() throws Exception {
+        flagEncoder = new RacingBikeFlagEncoder();
+        EncodingManager encodingManager = new EncodingManager(flagEncoder);
+        GraphBuilder graphBuilder = new GraphBuilder(encodingManager)
+                .setStore(false);
+        graph = graphBuilder.create();
+        weights = new IntDoubleHashMap();
+
+        env = new GRBEnv();
+        model = new GRBModel(env);
+        graphUtils = new GraphUtils(graph, flagEncoder, new TestWeighting(weights));
+        vars = new Vars(graph, model, graphUtils);
+        constraints = new Constraints(graph, model, vars, graphUtils);
+
+        model.set(GRB.IntParam.LogToConsole, 0);
+    }
+
+    @After
+    public void tearDown() throws Exception {
+        env.dispose();
+        model.dispose();
+        graph.close();
+    }
+
+    private void addEdge(int a, int b, boolean bidirectional, double cost, double score) {
+        EdgeIteratorState edge = graph.edge(a, b, cost, bidirectional);
+        weights.put(edge.getEdge(), score);
+    }
+
+    private void runSolver(int startNode, int maxCost) throws GRBException {
+        vars.addVarsToModel();
+        constraints.setupConstraints(startNode, maxCost);
+        model.optimize();
+    }
+
+    @Test(expected = IllegalArgumentException.class)
+    public void emptyGraph() throws GRBException {
+        // Fails to run since we don't have a node ID (0)
+        runSolver(0, 1);
+    }
+
+    @Test
+    public void singleDirectedArcGraph() throws GRBException {
+        addEdge(0, 1, false, 1, 1);
+
+        runSolver(0, 2);
+        assertNoSolution();
+    }
+
+    @Test
+    public void singleUndirectedArcGraph() throws GRBException {
+        addEdge(0, 1, true, 1, 1);
+
+        runSolver(0, 2);
+        assertNoSolution();
+    }
+
+    @Test
+    public void disconnectedArcs() throws GRBException {
+        addEdge(0, 1, false, 1, 1);
+        addEdge(2, 3, false, 1, 1);
+
+        runSolver(0, 2);
+        assertNoSolution();
+    }
+
+    @Test
+    public void singleDirectedThreeCycle() throws GRBException {
+        addEdge(0, 1, false, 1, 1);
+        addEdge(1, 2, false, 1, 1);
+        addEdge(2, 0, false, 1, 1);
+
+        runSolver(0, 3);
+        assertHasSolution();
+        assertSolution(3, 3);
+    }
+
+    @Test
+    public void singleUndirectedThreeCycle() throws GRBException {
+        addEdge(0, 1, true, 1, 1);
+        addEdge(1, 2, true, 1, 1);
+        addEdge(2, 0, true, 1, 1);
+
+        runSolver(0, 3);
+        assertHasSolution();
+        assertSolution(3, 3);
+    }
+
+    @Test
+    public void singleUndirectedThreeCycle_limitedBudget() throws GRBException {
+        addEdge(0, 1, true, 1, 1);
+        addEdge(1, 2, true, 1, 1);
+        addEdge(2, 0, true, 1, 1);
+
+        runSolver(0, 2);
+        // We can't find a solution here since we aren't allowed to take a road backwards
+        assertNoSolution();
+    }
+
+
+    @Test
+    public void twoDisconnectedThreeCycles() throws GRBException {
+        addEdge(0, 1, true, 1, 1);
+        addEdge(1, 2, true, 1, 1);
+        addEdge(2, 0, true, 1, 1);
+
+        addEdge(3, 4, true, 1, 1);
+        addEdge(4, 5, true, 1, 1);
+        addEdge(5, 3, true, 1, 1);
+
+        runSolver(0, 3);
+        assertHasSolution();
+        assertSolution(3, 3);
+    }
+
+    @Test
+    public void multipleDisconnectedThreeCycles() throws GRBException {
+        int numThreeCycles = 10;
+        int nodeId = 0;
+
+        for(int i = 0; i < numThreeCycles; i++) {
+            addEdge(nodeId, nodeId + 1, false, 1, 1);
+            addEdge(nodeId + 1, nodeId + 2, false, 1, 1);
+            addEdge(nodeId + 2, nodeId, false, 1, 1);
+            nodeId += 3;
+        }
+
+        runSolver(0, 3 * numThreeCycles);
+        assertHasSolution();
+        assertSolution(3, 3);
+    }
+
+    @Test
+    public void twoConnectedThreeCycles() throws GRBException {
+        addEdge(0, 1, false, 1, 1);
+        addEdge(1, 2, false, 1, 1);
+        addEdge(2, 0, false, 1, 1);
+
+        addEdge(2, 3, true, 2, 2);
+
+        addEdge(3, 4, false, 1, 1);
+        addEdge(4, 5, false, 1, 1);
+        addEdge(5, 3, false, 1, 1);
+
+        runSolver(0, 6);
+        assertHasSolution();
+        assertSolution(3, 3);
+    }
+
+    @Test
+    public void sixCycleWithTwoThreeCycles() throws GRBException {
+        addEdge(0, 1, true, 1, 1);
+        addEdge(1, 2, true, 1, 1);
+        addEdge(2, 0, true, 1, 1);
+
+        addEdge(2, 3, true, 1, 1);
+        addEdge(1, 4, true, 1, 1);
+
+        addEdge(3, 4, true, 1, 1);
+        addEdge(4, 5, true, 1, 1);
+        addEdge(5, 3, true, 1, 1);
+
+        runSolver(0, 6);
+        assertHasSolution();
+        printSolution();
+        assertSolution(6, 6);
+    }
+
+
+    @Test
+    public void directedKFour() throws GRBException {
+        addEdge(0, 1, false, 1, 2);
+        addEdge(1, 2, false, 1, 2);
+        addEdge(2, 3, false, 1, 2);
+        addEdge(3, 0, false, 1, 2);
+        addEdge(0, 2, false, 1, 1);
+        addEdge(1, 3, false, 1, 1);
+
+        runSolver(0, 4);
+        assertHasSolution();
+        assertSolution(8, 4);
+    }
+
+    @Test
+    public void simpleDirectedCactusGraph() throws GRBException {
+        addEdge(0, 1, false, 1, 1);
+        addEdge(1, 2, false, 1, 1);
+        addEdge(2, 0, false, 1, 1);
+
+        addEdge(1, 3, false, 1, 1);
+        addEdge(3, 4, false, 1, 3);
+        addEdge(4, 1, false, 1, 1);
+
+        addEdge(2, 5, false, 1, 1);
+        addEdge(5, 6, false, 1, 1);
+        addEdge(6, 2, false, 1, 1);
+
+        runSolver(0, 6);
+        assertHasSolution();
+        assertSolution(8, 6);
+    }
+
+    @Test
+    public void threeNodeMultiEdgeGraph() throws GRBException {
+        addEdge(0, 1, false, 1, 1);
+        addEdge(2, 0, false, 1, 1);
+        for(int i = 0; i < 20; i++) {
+            addEdge(1, 2, true, 1, 1);
+        }
+
+        runSolver(0, 10);
+        assertHasSolution();
+        assertSolution(9, 9);
+    }
+
+    private void printSolution() throws GRBException {
+        System.out.println("---- Final Solution ----");
+        GRBVar[] arcVars = vars.getArcVars();
+        double[] values = model.get(GRB.DoubleAttr.X, arcVars);
+
+        StringBuilder arcString = new StringBuilder();
+
+        for(int i = 0; i < arcVars.length - 1; i++) {
+            arcString.append(values[i]);
+            arcString.append(", ");
+            arcString.append(arcVars[i].get(GRB.StringAttr.VarName));
+            arcString.append("\n");
+        }
+        System.out.println("Arcs: " + arcString.toString());
+        double[] verts = model.get(GRB.DoubleAttr.X, vars.getVertexVars());
+        System.out.println("Verts: " + Arrays.toString(verts));
+    }
+
+
+    private void assertHasSolution() throws GRBException {
+        if(model.get(GRB.IntAttr.Status) != GRB.Status.OPTIMAL) {
+            fail("Gurobi could not find an optimal solution!");
+        }
+    }
+
+    private void assertNoSolution() throws GRBException {
+        if(model.get(GRB.IntAttr.Status) != GRB.Status.INFEASIBLE) {
+            fail("Gurobi found an optimal solution!");
+        }
+    }
+
+    private void assertSolution(double score, double cost) throws GRBException {
+        assertEquals(score, constraints.getObjective().getValue(), FP_PRECISION);
+        assertEquals(cost, constraints.getMaxCostConstraint().getValue(), FP_PRECISION);
+    }
+
+    private static class TestWeighting implements Weighting {
+
+        private IntDoubleMap weights;
+
+        TestWeighting(IntDoubleMap weights) {
+            this.weights = weights;
+        }
+
+        @Override
+        public double getMinWeight(double distance) {
+            return 0;
+        }
+
+        @Override
+        public double calcWeight(EdgeIteratorState edgeState, boolean reverse, int prevOrNextEdgeId) {
+            return weights.get(edgeState.getEdge());
+        }
+
+        @Override
+        public long calcMillis(EdgeIteratorState edgeState, boolean reverse, int prevOrNextEdgeId) {
+            return 0;
+        }
+
+        @Override
+        public FlagEncoder getFlagEncoder() {
+            return null;
+        }
+
+        @Override
+        public String getName() {
+            return null;
+        }
+
+        @Override
+        public boolean matches(HintsMap map) {
+            return false;
+        }
+    }
+}