diff --git a/y2023/control_loops/python/BUILD b/y2023/control_loops/python/BUILD
index a2ea6d8..9b1746a 100644
--- a/y2023/control_loops/python/BUILD
+++ b/y2023/control_loops/python/BUILD
@@ -35,7 +35,8 @@
     name = "graph_edit",
     srcs = [
         "graph_edit.py",
-        "graph_generate.py",
+        "graph_paths.py",
+        "graph_tools.py",
     ],
     legacy_create_init = False,
     target_compatible_with = ["@platforms//cpu:x86_64"],
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index 7b6179c..7caf8bc 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -11,21 +11,17 @@
 gi.require_version('Gtk', '3.0')
 from gi.repository import Gdk, Gtk
 import cairo
-import graph_generate
-from graph_generate import XYSegment, AngleSegment, to_theta, to_xy, alpha_blend
-from graph_generate import back_to_xy_loop, subdivide_theta, to_theta_loop
-from graph_generate import l1, l2, joint_center
+from graph_tools import XYSegment, AngleSegment, to_theta, to_xy, alpha_blend, draw_lines
+from graph_tools import back_to_xy_loop, subdivide_theta, to_theta_loop, px
+from graph_tools import l1, l2, joint_center
+from graph_paths import segments
 
-from frc971.control_loops.python.basic_window import OverrideMatrix, identity, quit_main_loop, set_color
+from frc971.control_loops.python.basic_window import quit_main_loop, set_color
 
 import shapely
 from shapely.geometry import Polygon
 
 
-def px(cr):
-    return OverrideMatrix(cr, identity)
-
-
 def draw_px_cross(cr, length_px):
     """Draws a cross with fixed dimensions in pixel space."""
     with px(cr):
@@ -81,15 +77,6 @@
 t2_max = numpy.pi * 3.0 / 4.0
 
 
-# Draw lines to cr + stroke.
-def draw_lines(cr, lines):
-    cr.move_to(lines[0][0], lines[0][1])
-    for pt in lines[1:]:
-        cr.line_to(pt[0], pt[1])
-    with px(cr):
-        cr.stroke()
-
-
 # Rotate a rasterized loop such that it aligns to when the parameters loop
 def rotate_to_jump_point(points):
     last_pt = points[0]
@@ -491,5 +478,5 @@
 
 
 silly = Silly()
-silly.segments = graph_generate.segments
+silly.segments = segments
 basic_window.RunApp()
diff --git a/y2023/control_loops/python/graph_generate.py b/y2023/control_loops/python/graph_generate.py
deleted file mode 100644
index 046b9dd..0000000
--- a/y2023/control_loops/python/graph_generate.py
+++ /dev/null
@@ -1,798 +0,0 @@
-import numpy
-
-# joint_center in x-y space.
-joint_center = (-0.299, 0.299)
-
-# Joint distances (l1 = "proximal", l2 = "distal")
-l1 = 46.25 * 0.0254
-l2 = 43.75 * 0.0254
-
-
-# Convert from x-y coordinates to theta coordinates.
-# orientation is a bool. This orientation is circular_index mod 2.
-# where circular_index is the circular index, or the position in the
-# "hyperextension" zones. "cross_point" allows shifting the place where
-# it rounds the result so that it draws nicer (no other functional differences).
-def to_theta(pt, circular_index, cross_point=-numpy.pi):
-    orient = (circular_index % 2) == 0
-    x = pt[0]
-    y = pt[1]
-    x -= joint_center[0]
-    y -= joint_center[1]
-    l3 = numpy.hypot(x, y)
-    t3 = numpy.arctan2(y, x)
-    theta1 = numpy.arccos((l1**2 + l3**2 - l2**2) / (2 * l1 * l3))
-
-    if orient:
-        theta1 = -theta1
-    theta1 += t3
-    theta1 = (theta1 - cross_point) % (2 * numpy.pi) + cross_point
-    theta2 = numpy.arctan2(y - l1 * numpy.sin(theta1),
-                           x - l1 * numpy.cos(theta1))
-    return numpy.array((theta1, theta2))
-
-
-# Simple trig to go back from theta1, theta2 to x-y
-def to_xy(theta1, theta2):
-    x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
-    y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
-    orient = ((theta2 - theta1) % (2.0 * numpy.pi)) < numpy.pi
-    return (x, y, orient)
-
-
-def get_circular_index(theta):
-    return int(numpy.floor((theta[1] - theta[0]) / numpy.pi))
-
-
-def get_xy(theta):
-    theta1 = theta[0]
-    theta2 = theta[1]
-    x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
-    y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
-    return numpy.array((x, y))
-
-
-# Draw a list of lines to a cairo context.
-def draw_lines(cr, lines):
-    cr.move_to(lines[0][0], lines[0][1])
-    for pt in lines[1:]:
-        cr.line_to(pt[0], pt[1])
-
-
-max_dist = 0.01
-max_dist_theta = numpy.pi / 64
-xy_end_circle_size = 0.01
-theta_end_circle_size = 0.07
-
-
-# Subdivide in theta space.
-def subdivide_theta(lines):
-    out = []
-    last_pt = lines[0]
-    out.append(last_pt)
-    for n_pt in lines[1:]:
-        for pt in subdivide(last_pt, n_pt, max_dist_theta):
-            out.append(pt)
-        last_pt = n_pt
-
-    return out
-
-
-# subdivide in xy space.
-def subdivide_xy(lines, max_dist=max_dist):
-    out = []
-    last_pt = lines[0]
-    out.append(last_pt)
-    for n_pt in lines[1:]:
-        for pt in subdivide(last_pt, n_pt, max_dist):
-            out.append(pt)
-        last_pt = n_pt
-
-    return out
-
-
-def to_theta_with_ci(pt, circular_index):
-    return to_theta_with_circular_index(pt[0], pt[1], circular_index)
-
-
-# to_theta, but distinguishes between
-def to_theta_with_circular_index(x, y, circular_index):
-    theta1, theta2 = to_theta((x, y), circular_index)
-    n_circular_index = int(numpy.floor((theta2 - theta1) / numpy.pi))
-    theta2 = theta2 + ((circular_index - n_circular_index)) * numpy.pi
-    return numpy.array((theta1, theta2))
-
-
-# alpha is in [0, 1] and is the weight to merge a and b.
-def alpha_blend(a, b, alpha):
-    """Blends a and b.
-
-    Args:
-      alpha: double, Ratio.  Needs to be in [0, 1] and is the weight to blend a
-          and b.
-    """
-    return b * alpha + (1.0 - alpha) * a
-
-
-def normalize(v):
-    """Normalize a vector while handling 0 length vectors."""
-    norm = numpy.linalg.norm(v)
-    if norm == 0:
-        return v
-    return v / norm
-
-
-# CI is circular index and allows selecting between all the stats that map
-# to the same x-y state (by giving them an integer index).
-# This will compute approximate first and second derivatives with respect
-# to path length.
-def to_theta_with_circular_index_and_derivs(x, y, dx, dy,
-                                            circular_index_select):
-    a = to_theta_with_circular_index(x, y, circular_index_select)
-    b = to_theta_with_circular_index(x + dx * 0.0001, y + dy * 0.0001,
-                                     circular_index_select)
-    c = to_theta_with_circular_index(x - dx * 0.0001, y - dy * 0.0001,
-                                     circular_index_select)
-    d1 = normalize(b - a)
-    d2 = normalize(c - a)
-    accel = (d1 + d2) / numpy.linalg.norm(a - b)
-    return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
-
-
-def to_theta_with_ci_and_derivs(p_prev, p, p_next, c_i_select):
-    a = to_theta(p, c_i_select)
-    b = to_theta(p_next, c_i_select)
-    c = to_theta(p_prev, c_i_select)
-    d1 = normalize(b - a)
-    d2 = normalize(c - a)
-    accel = (d1 + d2) / numpy.linalg.norm(a - b)
-    return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
-
-
-# Generic subdivision algorithm.
-def subdivide(p1, p2, max_dist):
-    dx = p2[0] - p1[0]
-    dy = p2[1] - p1[1]
-    dist = numpy.sqrt(dx**2 + dy**2)
-    n = int(numpy.ceil(dist / max_dist))
-    return [(alpha_blend(p1[0], p2[0],
-                         float(i) / n), alpha_blend(p1[1], p2[1],
-                                                    float(i) / n))
-            for i in range(1, n + 1)]
-
-
-# convert from an xy space loop into a theta loop.
-# All segements are expected go from one "hyper-extension" boundary
-# to another, thus we must go backwards over the "loop" to get a loop in
-# x-y space.
-def to_theta_loop(lines, cross_point=-numpy.pi):
-    out = []
-    last_pt = lines[0]
-    for n_pt in lines[1:]:
-        for pt in subdivide(last_pt, n_pt, max_dist):
-            out.append(to_theta(pt, 0, cross_point))
-        last_pt = n_pt
-    for n_pt in reversed(lines[:-1]):
-        for pt in subdivide(last_pt, n_pt, max_dist):
-            out.append(to_theta(pt, 1, cross_point))
-        last_pt = n_pt
-    return out
-
-
-# Convert a loop (list of line segments) into
-# The name incorrectly suggests that it is cyclic.
-def back_to_xy_loop(lines):
-    out = []
-    last_pt = lines[0]
-    out.append(to_xy(last_pt[0], last_pt[1]))
-    for n_pt in lines[1:]:
-        for pt in subdivide(last_pt, n_pt, max_dist_theta):
-            out.append(to_xy(pt[0], pt[1]))
-        last_pt = n_pt
-
-    return out
-
-
-# Segment in angle space.
-class AngleSegment:
-
-    def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
-        """Creates an angle segment.
-
-        Args:
-          start: (double, double),  The start of the segment in theta1, theta2
-              coordinates in radians
-          end: (double, double),  The end of the segment in theta1, theta2
-              coordinates in radians
-        """
-        self.start = start
-        self.end = end
-        self.name = name
-        self.alpha_unitizer = alpha_unitizer
-        self.vmax = vmax
-
-    def __repr__(self):
-        return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
-
-    def DrawTo(self, cr, theta_version):
-        if theta_version:
-            cr.move_to(self.start[0], self.start[1] + theta_end_circle_size)
-            cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
-                   2.0 * numpy.pi)
-            cr.move_to(self.end[0], self.end[1] + theta_end_circle_size)
-            cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
-                   2.0 * numpy.pi)
-            cr.move_to(self.start[0], self.start[1])
-            cr.line_to(self.end[0], self.end[1])
-        else:
-            start_xy = to_xy(self.start[0], self.start[1])
-            end_xy = to_xy(self.end[0], self.end[1])
-            draw_lines(cr, back_to_xy_loop([self.start, self.end]))
-            cr.move_to(start_xy[0] + xy_end_circle_size, start_xy[1])
-            cr.arc(start_xy[0], start_xy[1], xy_end_circle_size, 0,
-                   2.0 * numpy.pi)
-            cr.move_to(end_xy[0] + xy_end_circle_size, end_xy[1])
-            cr.arc(end_xy[0], end_xy[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
-
-    def ToThetaPoints(self):
-        dx = self.end[0] - self.start[0]
-        dy = self.end[1] - self.start[1]
-        mag = numpy.hypot(dx, dy)
-        dx /= mag
-        dy /= mag
-
-        return [(self.start[0], self.start[1], dx, dy, 0.0, 0.0),
-                (self.end[0], self.end[1], dx, dy, 0.0, 0.0)]
-
-
-class XYSegment:
-    """Straight line in XY space."""
-
-    def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
-        """Creates an XY segment.
-
-        Args:
-          start: (double, double),  The start of the segment in theta1, theta2
-              coordinates in radians
-          end: (double, double),  The end of the segment in theta1, theta2
-              coordinates in radians
-        """
-        self.start = start
-        self.end = end
-        self.name = name
-        self.alpha_unitizer = alpha_unitizer
-        self.vmax = vmax
-
-    def __repr__(self):
-        return "XYSegment(%s, %s)" % (repr(self.start), repr(self.end))
-
-    def DrawTo(self, cr, theta_version):
-        if theta_version:
-            theta1, theta2 = self.start
-            circular_index_select = int(
-                numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
-            start = get_xy(self.start)
-            end = get_xy(self.end)
-
-            ln = [(start[0], start[1]), (end[0], end[1])]
-            draw_lines(cr, [
-                to_theta_with_circular_index(x, y, circular_index_select)
-                for x, y in subdivide_xy(ln)
-            ])
-            cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
-            cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
-                   2.0 * numpy.pi)
-            cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
-            cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
-                   2.0 * numpy.pi)
-        else:
-            start = get_xy(self.start)
-            end = get_xy(self.end)
-            cr.move_to(start[0], start[1])
-            cr.line_to(end[0], end[1])
-            cr.move_to(start[0] + xy_end_circle_size, start[1])
-            cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
-            cr.move_to(end[0] + xy_end_circle_size, end[1])
-            cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
-
-    def ToThetaPoints(self):
-        """ Converts to points in theta space via to_theta_with_circular_index_and_derivs"""
-        theta1, theta2 = self.start
-        circular_index_select = int(
-            numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
-        start = get_xy(self.start)
-        end = get_xy(self.end)
-
-        ln = [(start[0], start[1]), (end[0], end[1])]
-
-        dx = end[0] - start[0]
-        dy = end[1] - start[1]
-        mag = numpy.hypot(dx, dy)
-        dx /= mag
-        dy /= mag
-
-        return [
-            to_theta_with_circular_index_and_derivs(x, y, dx, dy,
-                                                    circular_index_select)
-            for x, y in subdivide_xy(ln, 0.01)
-        ]
-
-
-def spline_eval(start, control1, control2, end, alpha):
-    a = alpha_blend(start, control1, alpha)
-    b = alpha_blend(control1, control2, alpha)
-    c = alpha_blend(control2, end, alpha)
-    return alpha_blend(alpha_blend(a, b, alpha), alpha_blend(b, c, alpha),
-                       alpha)
-
-
-def subdivide_spline(start, control1, control2, end):
-    # TODO: pick N based on spline parameters? or otherwise change it to be more evenly spaced?
-    n = 100
-    for i in range(0, n + 1):
-        yield i / float(n)
-
-
-class SplineSegment:
-
-    def __init__(self,
-                 start,
-                 control1,
-                 control2,
-                 end,
-                 name=None,
-                 alpha_unitizer=None,
-                 vmax=None):
-        self.start = start
-        self.control1 = control1
-        self.control2 = control2
-        self.end = end
-        self.name = name
-        self.alpha_unitizer = alpha_unitizer
-        self.vmax = vmax
-
-    def __repr__(self):
-        return "SplineSegment(%s, %s, %s, %s)" % (repr(
-            self.start), repr(self.control1), repr(
-                self.control2), repr(self.end))
-
-    def DrawTo(self, cr, theta_version):
-        if theta_version:
-            c_i_select = get_circular_index(self.start)
-            start = get_xy(self.start)
-            control1 = get_xy(self.control1)
-            control2 = get_xy(self.control2)
-            end = get_xy(self.end)
-
-            draw_lines(cr, [
-                to_theta(spline_eval(start, control1, control2, end, alpha),
-                         c_i_select)
-                for alpha in subdivide_spline(start, control1, control2, end)
-            ])
-            cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
-            cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
-                   2.0 * numpy.pi)
-            cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
-            cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
-                   2.0 * numpy.pi)
-        else:
-            start = get_xy(self.start)
-            control1 = get_xy(self.control1)
-            control2 = get_xy(self.control2)
-            end = get_xy(self.end)
-
-            draw_lines(cr, [
-                spline_eval(start, control1, control2, end, alpha)
-                for alpha in subdivide_spline(start, control1, control2, end)
-            ])
-
-            cr.move_to(start[0] + xy_end_circle_size, start[1])
-            cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
-            cr.move_to(end[0] + xy_end_circle_size, end[1])
-            cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
-
-    def ToThetaPoints(self):
-        t1, t2 = self.start
-        c_i_select = get_circular_index(self.start)
-        start = get_xy(self.start)
-        control1 = get_xy(self.control1)
-        control2 = get_xy(self.control2)
-        end = get_xy(self.end)
-
-        return [
-            to_theta_with_ci_and_derivs(
-                spline_eval(start, control1, control2, end, alpha - 0.00001),
-                spline_eval(start, control1, control2, end, alpha),
-                spline_eval(start, control1, control2, end, alpha + 0.00001),
-                c_i_select)
-            for alpha in subdivide_spline(start, control1, control2, end)
-        ]
-
-
-def get_derivs(t_prev, t, t_next):
-    c, a, b = t_prev, t, t_next
-    d1 = normalize(b - a)
-    d2 = normalize(c - a)
-    accel = (d1 + d2) / numpy.linalg.norm(a - b)
-    return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
-
-
-class ThetaSplineSegment:
-
-    def __init__(self,
-                 start,
-                 control1,
-                 control2,
-                 end,
-                 name=None,
-                 alpha_unitizer=None,
-                 vmax=None):
-        self.start = start
-        self.control1 = control1
-        self.control2 = control2
-        self.end = end
-        self.name = name
-        self.alpha_unitizer = alpha_unitizer
-        self.vmax = vmax
-
-    def __repr__(self):
-        return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(
-            self.start), repr(self.control1), repr(
-                self.control2), repr(self.end))
-
-    def DrawTo(self, cr, theta_version):
-        if (theta_version):
-            draw_lines(cr, [
-                spline_eval(self.start, self.control1, self.control2, self.end,
-                            alpha)
-                for alpha in subdivide_spline(self.start, self.control1,
-                                              self.control2, self.end)
-            ])
-        else:
-            start = get_xy(self.start)
-            end = get_xy(self.end)
-
-            draw_lines(cr, [
-                get_xy(
-                    spline_eval(self.start, self.control1, self.control2,
-                                self.end, alpha))
-                for alpha in subdivide_spline(self.start, self.control1,
-                                              self.control2, self.end)
-            ])
-
-            cr.move_to(start[0] + xy_end_circle_size, start[1])
-            cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
-            cr.move_to(end[0] + xy_end_circle_size, end[1])
-            cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
-
-    def ToThetaPoints(self):
-        return [
-            get_derivs(
-                spline_eval(self.start, self.control1, self.control2, self.end,
-                            alpha - 0.00001),
-                spline_eval(self.start, self.control1, self.control2, self.end,
-                            alpha),
-                spline_eval(self.start, self.control1, self.control2, self.end,
-                            alpha + 0.00001))
-            for alpha in subdivide_spline(self.start, self.control1,
-                                          self.control2, self.end)
-        ]
-
-
-tall_box_x = 0.411
-tall_box_y = 0.125
-
-short_box_x = 0.431
-short_box_y = 0.082
-
-ready_above_box = to_theta_with_circular_index(tall_box_x,
-                                               tall_box_y + 0.08,
-                                               circular_index=-1)
-tall_box_grab = to_theta_with_circular_index(tall_box_x,
-                                             tall_box_y,
-                                             circular_index=-1)
-short_box_grab = to_theta_with_circular_index(short_box_x,
-                                              short_box_y,
-                                              circular_index=-1)
-
-# TODO(austin): Drive the front/back off the same numbers a bit better.
-front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
-front_middle3_box = to_theta_with_circular_index(0.700,
-                                                 2.125,
-                                                 circular_index=-1.000000)
-front_middle2_box = to_theta_with_circular_index(0.700,
-                                                 2.268,
-                                                 circular_index=-1)
-front_middle1_box = to_theta_with_circular_index(0.800,
-                                                 1.915,
-                                                 circular_index=-1)
-front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
-back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
-back_middle2_box = to_theta_with_circular_index(-0.700, 2.27, circular_index=0)
-back_middle1_box = to_theta_with_circular_index(-0.800, 1.93, circular_index=0)
-back_low_box = to_theta_with_circular_index(-0.87, 1.64, circular_index=0)
-
-back_extra_low_box = to_theta_with_circular_index(-0.87,
-                                                  1.52,
-                                                  circular_index=0)
-
-front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
-back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
-
-neutral = to_theta_with_circular_index(0.0, 0.33, circular_index=-1)
-
-up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
-
-front_switch_auto = to_theta_with_circular_index(0.750,
-                                                 2.20,
-                                                 circular_index=-1.000000)
-
-duck = numpy.array([numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
-
-starting = numpy.array([numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
-vertical_starting = numpy.array([numpy.pi / 2.0, -numpy.pi / 2.0])
-
-self_hang = numpy.array([numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
-partner_hang = numpy.array([numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
-
-above_hang = numpy.array([numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
-below_hang = numpy.array([numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
-
-up_c1 = to_theta((0.63, 1.17), circular_index=-1)
-up_c2 = to_theta((0.65, 1.62), circular_index=-1)
-
-front_high_box_c1 = to_theta((0.63, 1.04), circular_index=-1)
-front_high_box_c2 = to_theta((0.50, 1.60), circular_index=-1)
-
-front_middle2_box_c1 = to_theta((0.41, 0.83), circular_index=-1)
-front_middle2_box_c2 = to_theta((0.52, 1.30), circular_index=-1)
-
-front_middle1_box_c1 = to_theta((0.34, 0.82), circular_index=-1)
-front_middle1_box_c2 = to_theta((0.48, 1.15), circular_index=-1)
-
-#c1: (1.421433, -1.070254)
-#c2: (1.434384, -1.057803
-ready_above_box_c1 = numpy.array([1.480802, -1.081218])
-ready_above_box_c2 = numpy.array([1.391449, -1.060331])
-
-front_switch_c1 = numpy.array([1.903841, -0.622351])
-front_switch_c2 = numpy.array([1.903841, -0.622351])
-
-
-sparse_front_points = [
-    (front_high_box, "FrontHighBox"),
-    (front_middle2_box, "FrontMiddle2Box"),
-    (front_middle3_box, "FrontMiddle3Box"),
-    (front_middle1_box, "FrontMiddle1Box"),
-    (front_low_box, "FrontLowBox"),
-    (front_switch, "FrontSwitch"),
-]  # yapf: disable
-
-sparse_back_points = [
-    (back_high_box, "BackHighBox"),
-    (back_middle2_box, "BackMiddle2Box"),
-    (back_middle1_box, "BackMiddle1Box"),
-    (back_low_box, "BackLowBox"),
-    (back_extra_low_box, "BackExtraLowBox"),
-]  # yapf: disable
-
-def expand_points(points, max_distance):
-    """Expands a list of points to be at most max_distance apart
-
-    Generates the paths to connect the new points to the closest input points,
-    and the paths connecting the points.
-
-    Args:
-      points, list of tuple of point, name, The points to start with and fill
-          in.
-      max_distance, float, The max distance between two points when expanding
-          the graph.
-
-    Return:
-      points, edges
-    """
-    result_points = [points[0]]
-    result_paths = []
-    for point, name in points[1:]:
-        previous_point = result_points[-1][0]
-        previous_point_xy = get_xy(previous_point)
-        circular_index = get_circular_index(previous_point)
-
-        point_xy = get_xy(point)
-        norm = numpy.linalg.norm(point_xy - previous_point_xy)
-        num_points = int(numpy.ceil(norm / max_distance))
-        last_iteration_point = previous_point
-        for subindex in range(1, num_points):
-            subpoint = to_theta(alpha_blend(previous_point_xy, point_xy,
-                                            float(subindex) / num_points),
-                                circular_index=circular_index)
-            result_points.append(
-                (subpoint, '%s%dof%d' % (name, subindex, num_points)))
-            result_paths.append(
-                XYSegment(last_iteration_point, subpoint, vmax=6.0))
-            if (last_iteration_point != previous_point).any():
-                result_paths.append(XYSegment(previous_point, subpoint))
-            if subindex == num_points - 1:
-                result_paths.append(XYSegment(subpoint, point, vmax=6.0))
-            else:
-                result_paths.append(XYSegment(subpoint, point))
-            last_iteration_point = subpoint
-        result_points.append((point, name))
-
-    return result_points, result_paths
-
-
-front_points, front_paths = expand_points(sparse_front_points, 0.06)
-back_points, back_paths = expand_points(sparse_back_points, 0.06)
-
-points = [(ready_above_box, "ReadyAboveBox"),
-          (tall_box_grab, "TallBoxGrab"),
-          (short_box_grab, "ShortBoxGrab"),
-          (back_switch, "BackSwitch"),
-          (neutral, "Neutral"),
-          (up, "Up"),
-          (above_hang, "AboveHang"),
-          (below_hang, "BelowHang"),
-          (self_hang, "SelfHang"),
-          (partner_hang, "PartnerHang"),
-          (front_switch_auto, "FrontSwitchAuto"),
-          (starting, "Starting"),
-          (duck, "Duck"),
-          (vertical_starting, "VerticalStarting"),
-] + front_points + back_points  # yapf: disable
-
-duck_c1 = numpy.array([1.337111, -1.721008])
-duck_c2 = numpy.array([1.283701, -1.795519])
-
-ready_to_up_c1 = numpy.array([1.792962, 0.198329])
-ready_to_up_c2 = numpy.array([1.792962, 0.198329])
-
-front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
-front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
-
-# We need to define critical points so we can create paths connecting them.
-# TODO(austin): Attach velocities to the slow ones.
-ready_to_back_low_c1 = numpy.array([2.524325, 0.046417])
-
-neutral_to_back_low_c1 = numpy.array([2.381942, -0.070220])
-
-tall_to_back_low_c1 = numpy.array([2.603918, 0.088298])
-tall_to_back_low_c2 = numpy.array([1.605624, 1.003434])
-
-tall_to_back_high_c2 = numpy.array([1.508610, 0.946147])
-
-# If true, only plot the first named segment
-isolate = False
-
-long_alpha_unitizer = numpy.matrix([[1.0 / 17.0, 0.0], [0.0, 1.0 / 17.0]])
-
-neutral_to_back_c1 = numpy.array([0.702527, -2.618276])
-neutral_to_back_c2 = numpy.array([0.526914, -3.109691])
-
-named_segments = [
-    ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2,
-                       back_switch, "BackSwitch"),
-    ThetaSplineSegment(neutral,
-                       neutral_to_back_low_c1,
-                       tall_to_back_high_c2,
-                       back_high_box,
-                       "NeutralBoxToHigh",
-                       alpha_unitizer=long_alpha_unitizer),
-    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2,
-                       back_middle2_box, "NeutralBoxToMiddle2",
-                       long_alpha_unitizer),
-    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
-                       back_middle1_box, "NeutralBoxToMiddle1",
-                       long_alpha_unitizer),
-    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
-                       back_low_box, "NeutralBoxToLow", long_alpha_unitizer),
-    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
-                       tall_to_back_high_c2, back_high_box, "ReadyBoxToHigh",
-                       long_alpha_unitizer),
-    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
-                       tall_to_back_high_c2, back_middle2_box,
-                       "ReadyBoxToMiddle2", long_alpha_unitizer),
-    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
-                       tall_to_back_low_c2, back_middle1_box,
-                       "ReadyBoxToMiddle1", long_alpha_unitizer),
-    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
-                       tall_to_back_low_c2, back_low_box, "ReadyBoxToLow",
-                       long_alpha_unitizer),
-    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
-                       tall_to_back_high_c2, back_high_box, "ShortBoxToHigh",
-                       long_alpha_unitizer),
-    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
-                       tall_to_back_high_c2, back_middle2_box,
-                       "ShortBoxToMiddle2", long_alpha_unitizer),
-    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
-                       tall_to_back_low_c2, back_middle1_box,
-                       "ShortBoxToMiddle1", long_alpha_unitizer),
-    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
-                       tall_to_back_low_c2, back_low_box, "ShortBoxToLow",
-                       long_alpha_unitizer),
-    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
-                       tall_to_back_high_c2, back_high_box, "TallBoxToHigh",
-                       long_alpha_unitizer),
-    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
-                       tall_to_back_high_c2, back_middle2_box,
-                       "TallBoxToMiddle2", long_alpha_unitizer),
-    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
-                       back_middle1_box, "TallBoxToMiddle1",
-                       long_alpha_unitizer),
-    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
-                       back_low_box, "TallBoxToLow", long_alpha_unitizer),
-    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
-                  ready_above_box, "ReadyToNeutral"),
-    XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox", vmax=6.0),
-    XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox", vmax=6.0),
-    XYSegment(tall_box_grab, short_box_grab, "TallToShortBox", vmax=6.0),
-    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
-                  tall_box_grab, "TallToNeutral"),
-    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
-                  short_box_grab, "ShortToNeutral"),
-    SplineSegment(neutral, up_c1, up_c2, up, "NeutralToUp"),
-    SplineSegment(neutral, front_high_box_c1, front_high_box_c2,
-                  front_high_box, "NeutralToFrontHigh"),
-    SplineSegment(neutral, front_middle2_box_c1, front_middle2_box_c2,
-                  front_middle2_box, "NeutralToFrontMiddle2"),
-    SplineSegment(neutral, front_middle1_box_c1, front_middle1_box_c2,
-                  front_middle1_box, "NeutralToFrontMiddle1"),
-]
-
-unnamed_segments = [
-    SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2,
-                  front_switch_auto),
-    SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
-    SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
-    SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
-    ThetaSplineSegment(duck, duck_c1, duck_c2, neutral),
-    SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
-    XYSegment(ready_above_box, front_low_box),
-    XYSegment(ready_above_box, front_switch),
-    XYSegment(ready_above_box, front_middle1_box),
-    XYSegment(ready_above_box, front_middle2_box),
-    XYSegment(ready_above_box, front_middle3_box),
-    SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2,
-                  front_high_box),
-    AngleSegment(starting, vertical_starting),
-    AngleSegment(vertical_starting, neutral),
-    XYSegment(neutral, front_low_box),
-    XYSegment(up, front_high_box),
-    XYSegment(up, front_middle2_box),
-    XYSegment(front_middle3_box, up),
-    XYSegment(front_middle3_box, front_high_box),
-    XYSegment(front_middle3_box, front_middle2_box),
-    XYSegment(front_middle3_box, front_middle1_box),
-    XYSegment(up, front_middle1_box),
-    XYSegment(up, front_low_box),
-    XYSegment(front_high_box, front_middle2_box),
-    XYSegment(front_high_box, front_middle1_box),
-    XYSegment(front_high_box, front_low_box),
-    XYSegment(front_middle2_box, front_middle1_box),
-    XYSegment(front_middle2_box, front_low_box),
-    XYSegment(front_middle1_box, front_low_box),
-    XYSegment(front_switch, front_low_box),
-    XYSegment(front_switch, up),
-    XYSegment(front_switch, front_high_box),
-    AngleSegment(up, back_high_box),
-    AngleSegment(up, back_middle2_box),
-    AngleSegment(up, back_middle1_box),
-    AngleSegment(up, back_low_box),
-    XYSegment(back_high_box, back_middle2_box),
-    XYSegment(back_high_box, back_middle1_box),
-    XYSegment(back_high_box, back_low_box),
-    XYSegment(back_middle2_box, back_middle1_box),
-    XYSegment(back_middle2_box, back_low_box),
-    XYSegment(back_middle1_box, back_low_box),
-    AngleSegment(up, above_hang),
-    AngleSegment(above_hang, below_hang),
-    AngleSegment(up, below_hang),
-    AngleSegment(up, self_hang),
-    AngleSegment(up, partner_hang),
-] + front_paths + back_paths
-
-segments = []
-if isolate:
-    segments += named_segments[:isolate]
-else:
-    segments += named_segments + unnamed_segments
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
new file mode 100644
index 0000000..26488bd
--- /dev/null
+++ b/y2023/control_loops/python/graph_paths.py
@@ -0,0 +1,274 @@
+import numpy
+
+from graph_tools import *
+
+tall_box_x = 0.411
+tall_box_y = 0.125
+
+short_box_x = 0.431
+short_box_y = 0.082
+
+ready_above_box = to_theta_with_circular_index(tall_box_x,
+                                               tall_box_y + 0.08,
+                                               circular_index=-1)
+tall_box_grab = to_theta_with_circular_index(tall_box_x,
+                                             tall_box_y,
+                                             circular_index=-1)
+short_box_grab = to_theta_with_circular_index(short_box_x,
+                                              short_box_y,
+                                              circular_index=-1)
+
+# TODO(austin): Drive the front/back off the same numbers a bit better.
+front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
+front_middle3_box = to_theta_with_circular_index(0.700,
+                                                 2.125,
+                                                 circular_index=-1.000000)
+front_middle2_box = to_theta_with_circular_index(0.700,
+                                                 2.268,
+                                                 circular_index=-1)
+front_middle1_box = to_theta_with_circular_index(0.800,
+                                                 1.915,
+                                                 circular_index=-1)
+front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
+back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
+back_middle2_box = to_theta_with_circular_index(-0.700, 2.27, circular_index=0)
+back_middle1_box = to_theta_with_circular_index(-0.800, 1.93, circular_index=0)
+back_low_box = to_theta_with_circular_index(-0.87, 1.64, circular_index=0)
+
+back_extra_low_box = to_theta_with_circular_index(-0.87,
+                                                  1.52,
+                                                  circular_index=0)
+
+front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
+back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
+
+neutral = to_theta_with_circular_index(0.0, 0.33, circular_index=-1)
+
+up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
+
+front_switch_auto = to_theta_with_circular_index(0.750,
+                                                 2.20,
+                                                 circular_index=-1.000000)
+
+duck = numpy.array([numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
+
+starting = numpy.array([numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
+vertical_starting = numpy.array([numpy.pi / 2.0, -numpy.pi / 2.0])
+
+self_hang = numpy.array([numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
+partner_hang = numpy.array([numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
+
+above_hang = numpy.array([numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
+below_hang = numpy.array([numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
+
+up_c1 = to_theta((0.63, 1.17), circular_index=-1)
+up_c2 = to_theta((0.65, 1.62), circular_index=-1)
+
+front_high_box_c1 = to_theta((0.63, 1.04), circular_index=-1)
+front_high_box_c2 = to_theta((0.50, 1.60), circular_index=-1)
+
+front_middle2_box_c1 = to_theta((0.41, 0.83), circular_index=-1)
+front_middle2_box_c2 = to_theta((0.52, 1.30), circular_index=-1)
+
+front_middle1_box_c1 = to_theta((0.34, 0.82), circular_index=-1)
+front_middle1_box_c2 = to_theta((0.48, 1.15), circular_index=-1)
+
+#c1: (1.421433, -1.070254)
+#c2: (1.434384, -1.057803
+ready_above_box_c1 = numpy.array([1.480802, -1.081218])
+ready_above_box_c2 = numpy.array([1.391449, -1.060331])
+
+front_switch_c1 = numpy.array([1.903841, -0.622351])
+front_switch_c2 = numpy.array([1.903841, -0.622351])
+
+sparse_front_points = [
+    (front_high_box, "FrontHighBox"),
+    (front_middle2_box, "FrontMiddle2Box"),
+    (front_middle3_box, "FrontMiddle3Box"),
+    (front_middle1_box, "FrontMiddle1Box"),
+    (front_low_box, "FrontLowBox"),
+    (front_switch, "FrontSwitch"),
+]  # yapf: disable
+
+sparse_back_points = [
+    (back_high_box, "BackHighBox"),
+    (back_middle2_box, "BackMiddle2Box"),
+    (back_middle1_box, "BackMiddle1Box"),
+    (back_low_box, "BackLowBox"),
+    (back_extra_low_box, "BackExtraLowBox"),
+]  # yapf: disable
+
+front_points, front_paths = expand_points(sparse_front_points, 0.06)
+back_points, back_paths = expand_points(sparse_back_points, 0.06)
+
+points = [(ready_above_box, "ReadyAboveBox"),
+          (tall_box_grab, "TallBoxGrab"),
+          (short_box_grab, "ShortBoxGrab"),
+          (back_switch, "BackSwitch"),
+          (neutral, "Neutral"),
+          (up, "Up"),
+          (above_hang, "AboveHang"),
+          (below_hang, "BelowHang"),
+          (self_hang, "SelfHang"),
+          (partner_hang, "PartnerHang"),
+          (front_switch_auto, "FrontSwitchAuto"),
+          (starting, "Starting"),
+          (duck, "Duck"),
+          (vertical_starting, "VerticalStarting"),
+] + front_points + back_points  # yapf: disable
+
+duck_c1 = numpy.array([1.337111, -1.721008])
+duck_c2 = numpy.array([1.283701, -1.795519])
+
+ready_to_up_c1 = numpy.array([1.792962, 0.198329])
+ready_to_up_c2 = numpy.array([1.792962, 0.198329])
+
+front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
+front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
+
+# We need to define critical points so we can create paths connecting them.
+# TODO(austin): Attach velocities to the slow ones.
+ready_to_back_low_c1 = numpy.array([2.524325, 0.046417])
+
+neutral_to_back_low_c1 = numpy.array([2.381942, -0.070220])
+
+tall_to_back_low_c1 = numpy.array([2.603918, 0.088298])
+tall_to_back_low_c2 = numpy.array([1.605624, 1.003434])
+
+tall_to_back_high_c2 = numpy.array([1.508610, 0.946147])
+
+# If true, only plot the first named segment
+isolate = False
+
+long_alpha_unitizer = numpy.matrix([[1.0 / 17.0, 0.0], [0.0, 1.0 / 17.0]])
+
+neutral_to_back_c1 = numpy.array([0.702527, -2.618276])
+neutral_to_back_c2 = numpy.array([0.526914, -3.109691])
+
+named_segments = [
+    ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2,
+                       back_switch, "BackSwitch"),
+    ThetaSplineSegment(neutral,
+                       neutral_to_back_low_c1,
+                       tall_to_back_high_c2,
+                       back_high_box,
+                       "NeutralBoxToHigh",
+                       alpha_unitizer=long_alpha_unitizer),
+    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2,
+                       back_middle2_box, "NeutralBoxToMiddle2",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
+                       back_middle1_box, "NeutralBoxToMiddle1",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
+                       back_low_box, "NeutralBoxToLow", long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_high_c2, back_high_box, "ReadyBoxToHigh",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_high_c2, back_middle2_box,
+                       "ReadyBoxToMiddle2", long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_low_c2, back_middle1_box,
+                       "ReadyBoxToMiddle1", long_alpha_unitizer),
+    ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+                       tall_to_back_low_c2, back_low_box, "ReadyBoxToLow",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_high_box, "ShortBoxToHigh",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_middle2_box,
+                       "ShortBoxToMiddle2", long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_low_c2, back_middle1_box,
+                       "ShortBoxToMiddle1", long_alpha_unitizer),
+    ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+                       tall_to_back_low_c2, back_low_box, "ShortBoxToLow",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_high_box, "TallBoxToHigh",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
+                       tall_to_back_high_c2, back_middle2_box,
+                       "TallBoxToMiddle2", long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
+                       back_middle1_box, "TallBoxToMiddle1",
+                       long_alpha_unitizer),
+    ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
+                       back_low_box, "TallBoxToLow", long_alpha_unitizer),
+    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+                  ready_above_box, "ReadyToNeutral"),
+    XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox", vmax=6.0),
+    XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox", vmax=6.0),
+    XYSegment(tall_box_grab, short_box_grab, "TallToShortBox", vmax=6.0),
+    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+                  tall_box_grab, "TallToNeutral"),
+    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+                  short_box_grab, "ShortToNeutral"),
+    SplineSegment(neutral, up_c1, up_c2, up, "NeutralToUp"),
+    SplineSegment(neutral, front_high_box_c1, front_high_box_c2,
+                  front_high_box, "NeutralToFrontHigh"),
+    SplineSegment(neutral, front_middle2_box_c1, front_middle2_box_c2,
+                  front_middle2_box, "NeutralToFrontMiddle2"),
+    SplineSegment(neutral, front_middle1_box_c1, front_middle1_box_c2,
+                  front_middle1_box, "NeutralToFrontMiddle1"),
+]
+
+unnamed_segments = [
+    SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2,
+                  front_switch_auto),
+    SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
+    SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
+    SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
+    ThetaSplineSegment(duck, duck_c1, duck_c2, neutral),
+    SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
+    XYSegment(ready_above_box, front_low_box),
+    XYSegment(ready_above_box, front_switch),
+    XYSegment(ready_above_box, front_middle1_box),
+    XYSegment(ready_above_box, front_middle2_box),
+    XYSegment(ready_above_box, front_middle3_box),
+    SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2,
+                  front_high_box),
+    AngleSegment(starting, vertical_starting),
+    AngleSegment(vertical_starting, neutral),
+    XYSegment(neutral, front_low_box),
+    XYSegment(up, front_high_box),
+    XYSegment(up, front_middle2_box),
+    XYSegment(front_middle3_box, up),
+    XYSegment(front_middle3_box, front_high_box),
+    XYSegment(front_middle3_box, front_middle2_box),
+    XYSegment(front_middle3_box, front_middle1_box),
+    XYSegment(up, front_middle1_box),
+    XYSegment(up, front_low_box),
+    XYSegment(front_high_box, front_middle2_box),
+    XYSegment(front_high_box, front_middle1_box),
+    XYSegment(front_high_box, front_low_box),
+    XYSegment(front_middle2_box, front_middle1_box),
+    XYSegment(front_middle2_box, front_low_box),
+    XYSegment(front_middle1_box, front_low_box),
+    XYSegment(front_switch, front_low_box),
+    XYSegment(front_switch, up),
+    XYSegment(front_switch, front_high_box),
+    AngleSegment(up, back_high_box),
+    AngleSegment(up, back_middle2_box),
+    AngleSegment(up, back_middle1_box),
+    AngleSegment(up, back_low_box),
+    XYSegment(back_high_box, back_middle2_box),
+    XYSegment(back_high_box, back_middle1_box),
+    XYSegment(back_high_box, back_low_box),
+    XYSegment(back_middle2_box, back_middle1_box),
+    XYSegment(back_middle2_box, back_low_box),
+    XYSegment(back_middle1_box, back_low_box),
+    AngleSegment(up, above_hang),
+    AngleSegment(above_hang, below_hang),
+    AngleSegment(up, below_hang),
+    AngleSegment(up, self_hang),
+    AngleSegment(up, partner_hang),
+] + front_paths + back_paths
+
+segments = []
+if isolate:
+    segments += named_segments[:isolate]
+else:
+    segments += named_segments + unnamed_segments
\ No newline at end of file
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
new file mode 100644
index 0000000..3ddfb37
--- /dev/null
+++ b/y2023/control_loops/python/graph_tools.py
@@ -0,0 +1,533 @@
+import numpy
+import cairo
+
+from frc971.control_loops.python.basic_window import OverrideMatrix, identity
+
+# joint_center in x-y space.
+joint_center = (-0.299, 0.299)
+
+# Joint distances (l1 = "proximal", l2 = "distal")
+l1 = 46.25 * 0.0254
+l2 = 43.75 * 0.0254
+
+max_dist = 0.01
+max_dist_theta = numpy.pi / 64
+xy_end_circle_size = 0.01
+theta_end_circle_size = 0.07
+
+
+def px(cr):
+    return OverrideMatrix(cr, identity)
+
+
+# Convert from x-y coordinates to theta coordinates.
+# orientation is a bool. This orientation is circular_index mod 2.
+# where circular_index is the circular index, or the position in the
+# "hyperextension" zones. "cross_point" allows shifting the place where
+# it rounds the result so that it draws nicer (no other functional differences).
+def to_theta(pt, circular_index, cross_point=-numpy.pi):
+    orient = (circular_index % 2) == 0
+    x = pt[0]
+    y = pt[1]
+    x -= joint_center[0]
+    y -= joint_center[1]
+    l3 = numpy.hypot(x, y)
+    t3 = numpy.arctan2(y, x)
+    theta1 = numpy.arccos((l1**2 + l3**2 - l2**2) / (2 * l1 * l3))
+
+    if orient:
+        theta1 = -theta1
+    theta1 += t3
+    theta1 = (theta1 - cross_point) % (2 * numpy.pi) + cross_point
+    theta2 = numpy.arctan2(y - l1 * numpy.sin(theta1),
+                           x - l1 * numpy.cos(theta1))
+    return numpy.array((theta1, theta2))
+
+
+# Simple trig to go back from theta1, theta2 to x-y
+def to_xy(theta1, theta2):
+    x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
+    y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
+    orient = ((theta2 - theta1) % (2.0 * numpy.pi)) < numpy.pi
+    return (x, y, orient)
+
+
+def get_circular_index(theta):
+    return int(numpy.floor((theta[1] - theta[0]) / numpy.pi))
+
+
+def get_xy(theta):
+    theta1 = theta[0]
+    theta2 = theta[1]
+    x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
+    y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
+    return numpy.array((x, y))
+
+
+# Subdivide in theta space.
+def subdivide_theta(lines):
+    out = []
+    last_pt = lines[0]
+    out.append(last_pt)
+    for n_pt in lines[1:]:
+        for pt in subdivide(last_pt, n_pt, max_dist_theta):
+            out.append(pt)
+        last_pt = n_pt
+
+    return out
+
+
+# subdivide in xy space.
+def subdivide_xy(lines, max_dist=max_dist):
+    out = []
+    last_pt = lines[0]
+    out.append(last_pt)
+    for n_pt in lines[1:]:
+        for pt in subdivide(last_pt, n_pt, max_dist):
+            out.append(pt)
+        last_pt = n_pt
+
+    return out
+
+
+def to_theta_with_ci(pt, circular_index):
+    return to_theta_with_circular_index(pt[0], pt[1], circular_index)
+
+
+# to_theta, but distinguishes between
+def to_theta_with_circular_index(x, y, circular_index):
+    theta1, theta2 = to_theta((x, y), circular_index)
+    n_circular_index = int(numpy.floor((theta2 - theta1) / numpy.pi))
+    theta2 = theta2 + ((circular_index - n_circular_index)) * numpy.pi
+    return numpy.array((theta1, theta2))
+
+
+# alpha is in [0, 1] and is the weight to merge a and b.
+def alpha_blend(a, b, alpha):
+    """Blends a and b.
+
+    Args:
+      alpha: double, Ratio.  Needs to be in [0, 1] and is the weight to blend a
+          and b.
+    """
+    return b * alpha + (1.0 - alpha) * a
+
+
+def normalize(v):
+    """Normalize a vector while handling 0 length vectors."""
+    norm = numpy.linalg.norm(v)
+    if norm == 0:
+        return v
+    return v / norm
+
+
+# CI is circular index and allows selecting between all the stats that map
+# to the same x-y state (by giving them an integer index).
+# This will compute approximate first and second derivatives with respect
+# to path length.
+def to_theta_with_circular_index_and_derivs(x, y, dx, dy,
+                                            circular_index_select):
+    a = to_theta_with_circular_index(x, y, circular_index_select)
+    b = to_theta_with_circular_index(x + dx * 0.0001, y + dy * 0.0001,
+                                     circular_index_select)
+    c = to_theta_with_circular_index(x - dx * 0.0001, y - dy * 0.0001,
+                                     circular_index_select)
+    d1 = normalize(b - a)
+    d2 = normalize(c - a)
+    accel = (d1 + d2) / numpy.linalg.norm(a - b)
+    return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+def to_theta_with_ci_and_derivs(p_prev, p, p_next, c_i_select):
+    a = to_theta(p, c_i_select)
+    b = to_theta(p_next, c_i_select)
+    c = to_theta(p_prev, c_i_select)
+    d1 = normalize(b - a)
+    d2 = normalize(c - a)
+    accel = (d1 + d2) / numpy.linalg.norm(a - b)
+    return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+# Generic subdivision algorithm.
+def subdivide(p1, p2, max_dist):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    dist = numpy.sqrt(dx**2 + dy**2)
+    n = int(numpy.ceil(dist / max_dist))
+    return [(alpha_blend(p1[0], p2[0],
+                         float(i) / n), alpha_blend(p1[1], p2[1],
+                                                    float(i) / n))
+            for i in range(1, n + 1)]
+
+
+# convert from an xy space loop into a theta loop.
+# All segements are expected go from one "hyper-extension" boundary
+# to another, thus we must go backwards over the "loop" to get a loop in
+# x-y space.
+def to_theta_loop(lines, cross_point=-numpy.pi):
+    out = []
+    last_pt = lines[0]
+    for n_pt in lines[1:]:
+        for pt in subdivide(last_pt, n_pt, max_dist):
+            out.append(to_theta(pt, 0, cross_point))
+        last_pt = n_pt
+    for n_pt in reversed(lines[:-1]):
+        for pt in subdivide(last_pt, n_pt, max_dist):
+            out.append(to_theta(pt, 1, cross_point))
+        last_pt = n_pt
+    return out
+
+
+# Convert a loop (list of line segments) into
+# The name incorrectly suggests that it is cyclic.
+def back_to_xy_loop(lines):
+    out = []
+    last_pt = lines[0]
+    out.append(to_xy(last_pt[0], last_pt[1]))
+    for n_pt in lines[1:]:
+        for pt in subdivide(last_pt, n_pt, max_dist_theta):
+            out.append(to_xy(pt[0], pt[1]))
+        last_pt = n_pt
+
+    return out
+
+
+def spline_eval(start, control1, control2, end, alpha):
+    a = alpha_blend(start, control1, alpha)
+    b = alpha_blend(control1, control2, alpha)
+    c = alpha_blend(control2, end, alpha)
+    return alpha_blend(alpha_blend(a, b, alpha), alpha_blend(b, c, alpha),
+                       alpha)
+
+
+def subdivide_spline(start, control1, control2, end):
+    # TODO: pick N based on spline parameters? or otherwise change it to be more evenly spaced?
+    n = 100
+    for i in range(0, n + 1):
+        yield i / float(n)
+
+
+def get_derivs(t_prev, t, t_next):
+    c, a, b = t_prev, t, t_next
+    d1 = normalize(b - a)
+    d2 = normalize(c - a)
+    accel = (d1 + d2) / numpy.linalg.norm(a - b)
+    return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+# Draw lines to cr + stroke.
+def draw_lines(cr, lines):
+    cr.move_to(lines[0][0], lines[0][1])
+    for pt in lines[1:]:
+        cr.line_to(pt[0], pt[1])
+    with px(cr):
+        cr.stroke()
+
+
+# Segment in angle space.
+class AngleSegment:
+
+    def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
+        """Creates an angle segment.
+
+        Args:
+          start: (double, double),  The start of the segment in theta1, theta2
+              coordinates in radians
+          end: (double, double),  The end of the segment in theta1, theta2
+              coordinates in radians
+        """
+        self.start = start
+        self.end = end
+        self.name = name
+        self.alpha_unitizer = alpha_unitizer
+        self.vmax = vmax
+
+    def __repr__(self):
+        return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
+
+    def DrawTo(self, cr, theta_version):
+        if theta_version:
+            cr.move_to(self.start[0], self.start[1] + theta_end_circle_size)
+            cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+            cr.move_to(self.end[0], self.end[1] + theta_end_circle_size)
+            cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+            cr.move_to(self.start[0], self.start[1])
+            cr.line_to(self.end[0], self.end[1])
+        else:
+            start_xy = to_xy(self.start[0], self.start[1])
+            end_xy = to_xy(self.end[0], self.end[1])
+            draw_lines(cr, back_to_xy_loop([self.start, self.end]))
+            cr.move_to(start_xy[0] + xy_end_circle_size, start_xy[1])
+            cr.arc(start_xy[0], start_xy[1], xy_end_circle_size, 0,
+                   2.0 * numpy.pi)
+            cr.move_to(end_xy[0] + xy_end_circle_size, end_xy[1])
+            cr.arc(end_xy[0], end_xy[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+    def ToThetaPoints(self):
+        dx = self.end[0] - self.start[0]
+        dy = self.end[1] - self.start[1]
+        mag = numpy.hypot(dx, dy)
+        dx /= mag
+        dy /= mag
+
+        return [(self.start[0], self.start[1], dx, dy, 0.0, 0.0),
+                (self.end[0], self.end[1], dx, dy, 0.0, 0.0)]
+
+
+class XYSegment:
+    """Straight line in XY space."""
+
+    def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
+        """Creates an XY segment.
+
+        Args:
+          start: (double, double),  The start of the segment in theta1, theta2
+              coordinates in radians
+          end: (double, double),  The end of the segment in theta1, theta2
+              coordinates in radians
+        """
+        self.start = start
+        self.end = end
+        self.name = name
+        self.alpha_unitizer = alpha_unitizer
+        self.vmax = vmax
+
+    def __repr__(self):
+        return "XYSegment(%s, %s)" % (repr(self.start), repr(self.end))
+
+    def DrawTo(self, cr, theta_version):
+        if theta_version:
+            theta1, theta2 = self.start
+            circular_index_select = int(
+                numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
+            start = get_xy(self.start)
+            end = get_xy(self.end)
+
+            ln = [(start[0], start[1]), (end[0], end[1])]
+            draw_lines(cr, [
+                to_theta_with_circular_index(x, y, circular_index_select)
+                for x, y in subdivide_xy(ln)
+            ])
+            cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+            cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+            cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+            cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+        else:
+            start = get_xy(self.start)
+            end = get_xy(self.end)
+            cr.move_to(start[0], start[1])
+            cr.line_to(end[0], end[1])
+            cr.move_to(start[0] + xy_end_circle_size, start[1])
+            cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+            cr.move_to(end[0] + xy_end_circle_size, end[1])
+            cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+    def ToThetaPoints(self):
+        """ Converts to points in theta space via to_theta_with_circular_index_and_derivs"""
+        theta1, theta2 = self.start
+        circular_index_select = int(
+            numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
+        start = get_xy(self.start)
+        end = get_xy(self.end)
+
+        ln = [(start[0], start[1]), (end[0], end[1])]
+
+        dx = end[0] - start[0]
+        dy = end[1] - start[1]
+        mag = numpy.hypot(dx, dy)
+        dx /= mag
+        dy /= mag
+
+        return [
+            to_theta_with_circular_index_and_derivs(x, y, dx, dy,
+                                                    circular_index_select)
+            for x, y in subdivide_xy(ln, 0.01)
+        ]
+
+
+class SplineSegment:
+
+    def __init__(self,
+                 start,
+                 control1,
+                 control2,
+                 end,
+                 name=None,
+                 alpha_unitizer=None,
+                 vmax=None):
+        self.start = start
+        self.control1 = control1
+        self.control2 = control2
+        self.end = end
+        self.name = name
+        self.alpha_unitizer = alpha_unitizer
+        self.vmax = vmax
+
+    def __repr__(self):
+        return "SplineSegment(%s, %s, %s, %s)" % (repr(
+            self.start), repr(self.control1), repr(
+                self.control2), repr(self.end))
+
+    def DrawTo(self, cr, theta_version):
+        if theta_version:
+            c_i_select = get_circular_index(self.start)
+            start = get_xy(self.start)
+            control1 = get_xy(self.control1)
+            control2 = get_xy(self.control2)
+            end = get_xy(self.end)
+
+            draw_lines(cr, [
+                to_theta(spline_eval(start, control1, control2, end, alpha),
+                         c_i_select)
+                for alpha in subdivide_spline(start, control1, control2, end)
+            ])
+            cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+            cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+            cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+            cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+                   2.0 * numpy.pi)
+        else:
+            start = get_xy(self.start)
+            control1 = get_xy(self.control1)
+            control2 = get_xy(self.control2)
+            end = get_xy(self.end)
+
+            draw_lines(cr, [
+                spline_eval(start, control1, control2, end, alpha)
+                for alpha in subdivide_spline(start, control1, control2, end)
+            ])
+
+            cr.move_to(start[0] + xy_end_circle_size, start[1])
+            cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+            cr.move_to(end[0] + xy_end_circle_size, end[1])
+            cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+    def ToThetaPoints(self):
+        t1, t2 = self.start
+        c_i_select = get_circular_index(self.start)
+        start = get_xy(self.start)
+        control1 = get_xy(self.control1)
+        control2 = get_xy(self.control2)
+        end = get_xy(self.end)
+
+        return [
+            to_theta_with_ci_and_derivs(
+                spline_eval(start, control1, control2, end, alpha - 0.00001),
+                spline_eval(start, control1, control2, end, alpha),
+                spline_eval(start, control1, control2, end, alpha + 0.00001),
+                c_i_select)
+            for alpha in subdivide_spline(start, control1, control2, end)
+        ]
+
+
+class ThetaSplineSegment:
+
+    def __init__(self,
+                 start,
+                 control1,
+                 control2,
+                 end,
+                 name=None,
+                 alpha_unitizer=None,
+                 vmax=None):
+        self.start = start
+        self.control1 = control1
+        self.control2 = control2
+        self.end = end
+        self.name = name
+        self.alpha_unitizer = alpha_unitizer
+        self.vmax = vmax
+
+    def __repr__(self):
+        return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(
+            self.start), repr(self.control1), repr(
+                self.control2), repr(self.end))
+
+    def DrawTo(self, cr, theta_version):
+        if (theta_version):
+            draw_lines(cr, [
+                spline_eval(self.start, self.control1, self.control2, self.end,
+                            alpha)
+                for alpha in subdivide_spline(self.start, self.control1,
+                                              self.control2, self.end)
+            ])
+        else:
+            start = get_xy(self.start)
+            end = get_xy(self.end)
+
+            draw_lines(cr, [
+                get_xy(
+                    spline_eval(self.start, self.control1, self.control2,
+                                self.end, alpha))
+                for alpha in subdivide_spline(self.start, self.control1,
+                                              self.control2, self.end)
+            ])
+
+            cr.move_to(start[0] + xy_end_circle_size, start[1])
+            cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+            cr.move_to(end[0] + xy_end_circle_size, end[1])
+            cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+    def ToThetaPoints(self):
+        return [
+            get_derivs(
+                spline_eval(self.start, self.control1, self.control2, self.end,
+                            alpha - 0.00001),
+                spline_eval(self.start, self.control1, self.control2, self.end,
+                            alpha),
+                spline_eval(self.start, self.control1, self.control2, self.end,
+                            alpha + 0.00001))
+            for alpha in subdivide_spline(self.start, self.control1,
+                                          self.control2, self.end)
+        ]
+
+
+def expand_points(points, max_distance):
+    """Expands a list of points to be at most max_distance apart
+
+    Generates the paths to connect the new points to the closest input points,
+    and the paths connecting the points.
+
+    Args:
+      points, list of tuple of point, name, The points to start with and fill
+          in.
+      max_distance, float, The max distance between two points when expanding
+          the graph.
+
+    Return:
+      points, edges
+    """
+    result_points = [points[0]]
+    result_paths = []
+    for point, name in points[1:]:
+        previous_point = result_points[-1][0]
+        previous_point_xy = get_xy(previous_point)
+        circular_index = get_circular_index(previous_point)
+
+        point_xy = get_xy(point)
+        norm = numpy.linalg.norm(point_xy - previous_point_xy)
+        num_points = int(numpy.ceil(norm / max_distance))
+        last_iteration_point = previous_point
+        for subindex in range(1, num_points):
+            subpoint = to_theta(alpha_blend(previous_point_xy, point_xy,
+                                            float(subindex) / num_points),
+                                circular_index=circular_index)
+            result_points.append(
+                (subpoint, '%s%dof%d' % (name, subindex, num_points)))
+            result_paths.append(
+                XYSegment(last_iteration_point, subpoint, vmax=6.0))
+            if (last_iteration_point != previous_point).any():
+                result_paths.append(XYSegment(previous_point, subpoint))
+            if subindex == num_points - 1:
+                result_paths.append(XYSegment(subpoint, point, vmax=6.0))
+            else:
+                result_paths.append(XYSegment(subpoint, point))
+            last_iteration_point = subpoint
+        result_points.append((point, name))
+
+    return result_points, result_paths