diff --git a/y2018/control_loops/python/BUILD b/y2018/control_loops/python/BUILD
index 5a44834..eea64ad 100644
--- a/y2018/control_loops/python/BUILD
+++ b/y2018/control_loops/python/BUILD
@@ -96,3 +96,15 @@
         "//third_party/matplotlib-cpp",
     ],
 )
+
+py_binary(
+    name = "graph_edit",
+    srcs = [
+        "basic_window.py",
+        "graph_edit.py",
+        "graph_generate.py",
+    ],
+    # Sigh, these aren't respected.
+    default_python_version = "PY3",
+    srcs_version = "PY3",
+)
diff --git a/y2018/control_loops/python/basic_window.py b/y2018/control_loops/python/basic_window.py
index 7d93fe1..7caf299 100644
--- a/y2018/control_loops/python/basic_window.py
+++ b/y2018/control_loops/python/basic_window.py
@@ -8,108 +8,115 @@
 
 identity = cairo.Matrix()
 
+
 # Override the matrix of a cairo context.
 class OverrideMatrix(object):
-  def __init__(self, cr, matrix):
-    self.cr = cr
-    self.matrix = matrix
-  def __enter__(self):
-    self.cr.save()
-    self.cr.set_matrix(self.matrix)
+    def __init__(self, cr, matrix):
+        self.cr = cr
+        self.matrix = matrix
 
-  def __exit__(self, type, value, traceback):
-    self.cr.restore();
+    def __enter__(self):
+        self.cr.save()
+        self.cr.set_matrix(self.matrix)
+
+    def __exit__(self, type, value, traceback):
+        self.cr.restore()
+
 
 mainloop = GLib.MainLoop()
 
+
 def quit_main_loop(*args):
-  mainloop.quit()
+    mainloop.quit()
+
 
 def RunApp():
-  try:
-    mainloop.run();
-  except KeyboardInterrupt:
-    print('\nCtrl+C hit, quitting')
-    mainloop.quit()
+    try:
+        mainloop.run()
+    except KeyboardInterrupt:
+        print('\nCtrl+C hit, quitting')
+        mainloop.quit()
+
 
 # Create a GTK+ widget on which we will draw using Cairo
 class BaseWindow(Gtk.DrawingArea):
-  def method_connect(self, event, cb):
-    def handler(obj, *args):
-      cb(*args)
-    self.window.connect(event, handler)
+    def method_connect(self, event, cb):
+        def handler(obj, *args):
+            cb(*args)
 
-  # Draw in response to an expose-event
-  def __init__(self): 
-    super().__init__()
-    self.window = Gtk.Window()
-    self.window.set_title("DrawingArea")
-    self.window.connect("destroy", quit_main_loop)
-    self.window.set_events(Gdk.EventMask.BUTTON_PRESS_MASK |
-                        Gdk.EventMask.BUTTON_RELEASE_MASK |
-                        Gdk.EventMask.POINTER_MOTION_MASK |
-                        Gdk.EventMask.SCROLL_MASK |
-                        Gdk.EventMask.KEY_PRESS_MASK
-                        )
-    self.method_connect("key-press-event", self.do_key_press)
-    self.method_connect("button-press-event", self._do_button_press_internal)
-    self.method_connect("configure-event", self._do_configure)
+        self.window.connect(event, handler)
 
-    self.set_size_request(640, 400) #640 * 2, 1229)
-    self.window.add(self)
-    self.window.show_all()
-    self.center = (0, 0)
-    self.shape = (640, 400)
-    self.needs_redraw = False
+    # Draw in response to an expose-event
+    def __init__(self):
+        super(BaseWindow, self).__init__()
+        self.window = Gtk.Window()
+        self.window.set_title("DrawingArea")
+        self.window.connect("destroy", quit_main_loop)
+        self.window.set_events(Gdk.EventMask.BUTTON_PRESS_MASK
+                               | Gdk.EventMask.BUTTON_RELEASE_MASK
+                               | Gdk.EventMask.POINTER_MOTION_MASK
+                               | Gdk.EventMask.SCROLL_MASK
+                               | Gdk.EventMask.KEY_PRESS_MASK)
+        self.method_connect("key-press-event", self.do_key_press)
+        self.method_connect("button-press-event",
+                            self._do_button_press_internal)
+        self.method_connect("configure-event", self._do_configure)
 
-  def get_current_scale(self):
-    w_w, w_h = self.window_shape
-    w, h = self.shape
-    return min((w_w / w), (w_h / h))
+        self.set_size_request(640, 400)
+        self.window.add(self)
+        self.window.show_all()
+        self.center = (0, 0)
+        self.shape = (640, 400)
+        self.needs_redraw = False
 
-  def init_extents(self, center, shape):
-    self.center = center
-    self.shape = shape
+    def get_current_scale(self):
+        w_w, w_h = self.window_shape
+        w, h = self.shape
+        return min((w_w / w), (w_h / h))
 
-  # The gtk system creates cr which is a cairo_context_t (in the c docs), and then it 
-  # passes it as a function argument to the "draw" event.  do_draw is the default name.
-  def do_draw(self, cr):
-    cr.save()
-    cr.set_font_size(20)
-    cr.translate(self.window_shape[0] / 2, self.window_shape[1] / 2)
-    scale = self.get_current_scale()
-    cr.scale(scale, -scale)
-    cr.translate(-self.center[0], -self.center[1])
-    self.needs_redraw = False
-    self.handle_draw(cr)
-    cr.restore()
+    def init_extents(self, center, shape):
+        self.center = center
+        self.shape = shape
 
-  # Handle the expose-event by drawing
-  def handle_draw(self, cr):
-    pass
+    # The gtk system creates cr which is a cairo_context_t (in the c docs), and then it
+    # passes it as a function argument to the "draw" event.  do_draw is the default name.
+    def do_draw(self, cr):
+        cr.save()
+        cr.set_font_size(20)
+        cr.translate(self.window_shape[0] / 2, self.window_shape[1] / 2)
+        scale = self.get_current_scale()
+        cr.scale(scale, -scale)
+        cr.translate(-self.center[0], -self.center[1])
+        self.needs_redraw = False
+        self.handle_draw(cr)
+        cr.restore()
 
-  def do_key_press(self, event):
-    pass
+    # Handle the expose-event by drawing
+    def handle_draw(self, cr):
+        pass
 
-  def _do_button_press_internal(self, event):
-    o_x = event.x
-    o_y = event.y
-    x = event.x - self.window_shape[0] / 2
-    y = self.window_shape[1] / 2 - event.y
-    scale = self.get_current_scale()
-    event.x = x / scale + self.center[0]
-    event.y = y / scale + self.center[1]
-    self.do_button_press(event)
-    event.x = o_x
-    event.y = o_y
+    def do_key_press(self, event):
+        pass
 
-  def do_button_press(self, event):
-    pass
+    def _do_button_press_internal(self, event):
+        o_x = event.x
+        o_y = event.y
+        x = event.x - self.window_shape[0] / 2
+        y = self.window_shape[1] / 2 - event.y
+        scale = self.get_current_scale()
+        event.x = x / scale + self.center[0]
+        event.y = y / scale + self.center[1]
+        self.do_button_press(event)
+        event.x = o_x
+        event.y = o_y
 
-  def _do_configure(self, event):
-    self.window_shape = (event.width, event.height)
+    def do_button_press(self, event):
+        pass
 
-  def redraw(self):
-    if not self.needs_redraw:
-      self.needs_redraw = True
-      self.window.queue_draw()
+    def _do_configure(self, event):
+        self.window_shape = (event.width, event.height)
+
+    def redraw(self):
+        if not self.needs_redraw:
+            self.needs_redraw = True
+            self.window.queue_draw()
diff --git a/y2018/control_loops/python/graph_edit.py b/y2018/control_loops/python/graph_edit.py
index 3386579..183f7bf 100644
--- a/y2018/control_loops/python/graph_edit.py
+++ b/y2018/control_loops/python/graph_edit.py
@@ -1,5 +1,7 @@
+from __future__ import print_function
 import os
 import basic_window
+import random
 import gi
 import numpy
 gi.require_version('Gtk', '3.0')
@@ -15,83 +17,92 @@
 import shapely
 from shapely.geometry import Polygon
 
+
 def px(cr):
-  return OverrideMatrix(cr, identity)
+    return OverrideMatrix(cr, identity)
 
-# Draws a cross with fixed dimensions in pixel space.
+
 def draw_px_cross(cr, length_px):
-  with px(cr):
-    x,y = cr.get_current_point()
-    cr.move_to(x, y - length_px)
-    cr.line_to(x, y + length_px)
-    cr.stroke()
+    """Draws a cross with fixed dimensions in pixel space."""
+    with px(cr):
+        x, y = cr.get_current_point()
+        cr.move_to(x, y - length_px)
+        cr.line_to(x, y + length_px)
+        cr.stroke()
 
-    cr.move_to(x - length_px, y)
-    cr.line_to(x + length_px, y)
-    cr.stroke()
+        cr.move_to(x - length_px, y)
+        cr.line_to(x + length_px, y)
+        cr.stroke()
 
-# Distance between two points in angle space.
+
 def angle_dist_sqr(a1, a2):
-  return (a1[0] - a2[0]) ** 2 + (a1[1] - a2[1]) ** 2
+    """Distance between two points in angle space."""
+    return (a1[0] - a2[0])**2 + (a1[1] - a2[1])**2
+
 
 # Find the highest y position that intersects the vertical line defined by x.
 def inter_y(x):
-  return numpy.sqrt((l2 + l1) ** 2 - (x - joint_center[0]) ** 2) + joint_center[1]
+    return numpy.sqrt((l2 + l1)**2 -
+                      (x - joint_center[0])**2) + joint_center[1]
+
 
 # This is the x position where the inner (hyperextension) circle intersects the horizontal line
-derr = numpy.sqrt((l1 - l2) ** 2 - (joint_center[1] - 12.0) ** 2)
+derr = numpy.sqrt((l1 - l2)**2 - (joint_center[1] - 0.3048)**2)
+
 
 # Define min and max l1 angles based on vertical constraints.
 def get_angle(boundary):
-  h = numpy.sqrt((l1) ** 2 - (boundary - joint_center[0]) ** 2) + joint_center[1]
-  return numpy.arctan2(h, boundary - joint_center[0])
+    h = numpy.sqrt((l1)**2 - (boundary - joint_center[0])**2) + joint_center[1]
+    return numpy.arctan2(h, boundary - joint_center[0])
+
 
 # left hand side lines
 lines1 = [
-    (-32.525, inter_y(-32.525)),
-    (-32.525, 5.5),
-    (-23.025, 5.5),
-    (-23.025, 12.0),
-    (joint_center[0] - derr, 12.0),
+    (-0.826135, inter_y(-0.826135)),
+    (-0.826135, 0.1397),
+    (-23.025 * 0.0254, 0.1397),
+    (-23.025 * 0.0254, 0.3048),
+    (joint_center[0] - derr, 0.3048),
 ]
 
 # right hand side lines
-lines2 = [
-    (joint_center[0] + derr, 12.0),
-    (16.625, 12.0),
-    (16.625, 5.5),
-    (32.525, 5.5),
-    (32.525, inter_y(32.525))
-]
+lines2 = [(joint_center[0] + derr, 0.3048), (0.422275, 0.3048),
+          (0.422275, 0.1397), (0.826135, 0.1397), (0.826135,
+                                                   inter_y(0.826135))]
 
-t1_min = get_angle(32.525 - 4.0)
-t2_min = -7 / 4.0 * numpy.pi
+t1_min = get_angle((32.525 - 4.0) * 0.0254)
+t2_min = -7.0 / 4.0 * numpy.pi
 
-t1_max = get_angle(-32.525 + 4.0)
-t2_max = numpy.pi * 3 / 4.0
+t1_max = get_angle((-32.525 + 4.0) * 0.0254)
+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()
+    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]
-  for pt_i in range(1, len(points)):
-    pt = points[pt_i]
-    delta = last_pt[1] - pt[1]
-    if abs(delta) > numpy.pi:
-      print(delta)
-      return points[pt_i:] + points[:pt_i]
-    last_pt = pt
-  return points
+    last_pt = points[0]
+    for pt_i in range(1, len(points)):
+        pt = points[pt_i]
+        delta = last_pt[1] - pt[1]
+        if abs(delta) > numpy.pi:
+            print(delta)
+            return points[pt_i:] + points[:pt_i]
+        last_pt = pt
+    return points
+
 
 # shift points vertically by dy.
 def y_shift(points, dy):
-  return [(x, y + dy) for x, y in points]
+    return [(x, y + dy) for x, y in points]
+
 
 lines1_theta_part = rotate_to_jump_point(to_theta_loop(lines1, 0))
 lines2_theta_part = rotate_to_jump_point(to_theta_loop(lines2))
@@ -106,297 +117,318 @@
 
 p1 = Polygon(lines_theta)
 
-p2 = Polygon([(t1_min, t2_min), (t1_max, t2_min),
-  (t1_max, t2_max), (t1_min, t2_max)])
+p2 = Polygon([(t1_min, t2_min), (t1_max, t2_min), (t1_max, t2_max), (t1_min,
+                                                                     t2_max)])
 
 # Fully computed theta constrints.
 lines_theta = list(p1.intersection(p2).exterior.coords)
 
-print(", ".join("{%s, %s}" % (a,b) for a, b in lines_theta))
+print("Theta constraint.")
+print(", ".join("{%s, %s}" % (a, b) for a, b in lines_theta))
 
 lines1_theta_back = back_to_xy_loop(lines1_theta)
 lines2_theta_back = back_to_xy_loop(lines2_theta)
 
 lines_theta_back = back_to_xy_loop(lines_theta)
 
+
 # Get the closest point to a line from a test pt.
 def get_closest(prev, cur, pt):
-  dx_ang = (cur[0] - prev[0])
-  dy_ang = (cur[1] - prev[1])
+    dx_ang = (cur[0] - prev[0])
+    dy_ang = (cur[1] - prev[1])
 
-  d = numpy.sqrt(dx_ang ** 2 + dy_ang ** 2)
-  if (d < 0.000001):
-    return prev, numpy.sqrt((prev[0] - pt[0]) ** 2 + (prev[1] - pt[1]) ** 2)
+    d = numpy.sqrt(dx_ang**2 + dy_ang**2)
+    if (d < 0.000001):
+        return prev, numpy.sqrt((prev[0] - pt[0])**2 + (prev[1] - pt[1])**2)
+
+    pdx = -dy_ang / d
+    pdy = dx_ang / d
+
+    dpx = pt[0] - prev[0]
+    dpy = pt[1] - prev[1]
+
+    alpha = (dx_ang * dpx + dy_ang * dpy) / d / d
+
+    if (alpha < 0):
+        return prev, numpy.sqrt((prev[0] - pt[0])**2 + (prev[1] - pt[1])**2)
+    elif (alpha > 1):
+        return cur, numpy.sqrt((cur[0] - pt[0])**2 + (cur[1] - pt[1])**2)
+    else:
+        return (alpha_blend(prev[0], cur[0], alpha), alpha_blend(prev[1], cur[1], alpha)), \
+            abs(dpx * pdx + dpy * pdy)
 
 
-  pdx = -dy_ang / d
-  pdy = dx_ang / d
-
-  dpx = pt[0] - prev[0]
-  dpy = pt[1] - prev[1]
-
-  alpha = (dx_ang * dpx + dy_ang * dpy) / d / d
-
-  if (alpha < 0):
-    return prev, numpy.sqrt((prev[0] - pt[0]) ** 2 + (prev[1] - pt[1]) ** 2)
-  elif (alpha > 1):
-    return cur, numpy.sqrt((cur[0] - pt[0]) ** 2 + (cur[1] - pt[1]) ** 2)
-  else:
-    return (alpha_blend(prev[0], cur[0], alpha), alpha_blend(prev[1], cur[1], alpha)), \
-        abs(dpx * pdx + dpy * pdy)
-
-# 
+#
 def closest_segment(lines, pt):
-  c_pt, c_pt_dist = get_closest(lines[-1], lines[0], pt)
-  for i in range(1, len(lines)):
-    prev = lines[i - 1]
-    cur = lines[i]
-    c_pt_new, c_pt_new_dist = get_closest(prev, cur, pt)
-    if c_pt_new_dist < c_pt_dist:
-      c_pt = c_pt_new
-      c_pt_dist = c_pt_new_dist
-  return c_pt, c_pt_dist
+    c_pt, c_pt_dist = get_closest(lines[-1], lines[0], pt)
+    for i in range(1, len(lines)):
+        prev = lines[i - 1]
+        cur = lines[i]
+        c_pt_new, c_pt_new_dist = get_closest(prev, cur, pt)
+        if c_pt_new_dist < c_pt_dist:
+            c_pt = c_pt_new
+            c_pt_dist = c_pt_new_dist
+    return c_pt, c_pt_dist
+
 
 # Create a GTK+ widget on which we will draw using Cairo
 class Silly(basic_window.BaseWindow):
-  def __init__(self):
-    super().__init__()
+    def __init__(self):
+        super(Silly, self).__init__()
 
-    self.theta_version = True
-    self.reinit_extents()
+        self.theta_version = True
+        self.reinit_extents()
 
-    self.last_pos = (20, 20)
-    self.c_i_select = 0
-    self.click_bool = False
+        self.last_pos = (numpy.pi / 2.0, 1.0)
+        self.circular_index_select = -1
 
+        # Extra stuff for drawing lines.
+        self.segments = []
+        self.prev_segment_pt = None
+        self.now_segment_pt = None
 
-    # Extra stuff for drawing lines.
-    self.segs = []
-    self.prev_seg_pt = None
-    self.now_seg_pt = None
-
-  def reinit_extents(self):
-    if self.theta_version:
-      self.extents_x_min = -numpy.pi * 2
-      self.extents_x_max =  numpy.pi * 2
-      self.extents_y_min = -numpy.pi * 2
-      self.extents_y_max =  numpy.pi * 2
-    else:
-      self.extents_x_min = -40.0
-      self.extents_x_max =  40.0
-      self.extents_y_min = -4.0
-      self.extents_y_max =  110.0
-
-    self.init_extents((0.5*(self.extents_x_min+self.extents_x_max), 0.5*(self.extents_y_max+self.extents_y_min)),
-                      (1.0*(self.extents_x_max-self.extents_x_min), 1.0*(self.extents_y_max-self.extents_y_min)))
-
-  # Handle the expose-event by drawing
-  def handle_draw(self, cr):
-    # use "with px(cr): blah;" to transform to pixel coordinates.
-
-    # Fill the background color of the window with grey
-    cr.set_source_rgb(0.5, 0.5, 0.5)
-    cr.paint()
-
-    # Draw a extents rectangle
-    cr.set_source_rgb(1.0, 1.0, 1.0)
-    cr.rectangle(self.extents_x_min, self.extents_y_min,
-                 (self.extents_x_max-self.extents_x_min), self.extents_y_max-self.extents_y_min)
-    cr.fill()
-
-    if not self.theta_version:
-
-      # Draw a filled white rectangle.
-      cr.set_source_rgb(1.0, 1.0, 1.0)
-      cr.rectangle(-2.0, -2.0, 4.0, 4.0)
-      cr.fill()
-
-      cr.set_source_rgb(0.0, 0.0, 1.0)
-      cr.arc(joint_center[0], joint_center[1], l2 + l1, 0, 2 * numpy.pi)
-      with px(cr): cr.stroke()
-      cr.arc(joint_center[0], joint_center[1], l1 - l2, 0, 2 * numpy.pi)
-      with px(cr): cr.stroke()
-
-    else:
-      # Draw a filled white rectangle.
-      cr.set_source_rgb(1.0, 1.0, 1.0)
-      cr.rectangle(-numpy.pi, -numpy.pi, numpy.pi * 2, numpy.pi * 2)
-      cr.fill()
-
-    if self.theta_version:
-      cr.set_source_rgb(0.0, 0.0, 1.0)
-      for i in range(-6, 6):
-        cr.move_to(-40, -40 + i * numpy.pi)
-        cr.line_to(40, 40 + i * numpy.pi)
-      with px(cr): cr.stroke()
-
-
-    if not self.theta_version:
-      cr.set_source_rgb(0.2, 1.0, 0.2)
-      draw_lines(cr, lines2)
-
-    if self.theta_version:
-      cr.set_source_rgb(0.5, 0.5, 1.0)
-      draw_lines(cr, lines_theta)
-
-    else:
-      cr.set_source_rgb(0.5, 1.0, 1.0)
-      draw_lines(cr, lines1)
-      draw_lines(cr, lines2)
-
-      def set_color(cr, c_i):
-        if c_i == -2:
-          cr.set_source_rgb(0.0, 0.25, 1.0)
-        elif c_i == -1:
-          cr.set_source_rgb(0.5, 0.0, 1.0)
-        elif c_i == 0:
-          cr.set_source_rgb(0.5, 1.0, 1.0)
-        elif c_i == 1:
-          cr.set_source_rgb(0.0, 0.5, 1.0)
-        elif c_i == 2:
-          cr.set_source_rgb(0.5, 1.0, 0.5)
+    def reinit_extents(self):
+        if self.theta_version:
+            self.extents_x_min = -numpy.pi * 2
+            self.extents_x_max = numpy.pi * 2
+            self.extents_y_min = -numpy.pi * 2
+            self.extents_y_max = numpy.pi * 2
         else:
-          cr.set_source_rgb(1.0, 0.0, 0.0)
+            self.extents_x_min = -40.0 * 0.0254
+            self.extents_x_max = 40.0 * 0.0254
+            self.extents_y_min = -4.0 * 0.0254
+            self.extents_y_max = 110.0 * 0.0254
 
-      def get_ci(pt):
-        t1, t2 = pt
-        c_i = int(numpy.floor((t2 - t1) / numpy.pi))
-        return c_i
+        self.init_extents(
+            (0.5 * (self.extents_x_min + self.extents_x_max), 0.5 *
+             (self.extents_y_max + self.extents_y_min)),
+            (1.0 * (self.extents_x_max - self.extents_x_min), 1.0 *
+             (self.extents_y_max - self.extents_y_min)))
 
-      cr.set_source_rgb(0.0, 0.0, 1.0)
-      lines = subdivide_theta(lines_theta)
-      o_c_i = c_i = get_ci(lines[0])
-      p_xy = to_xy(lines[0][0], lines[0][1])
-      if c_i == self.c_i_select: cr.move_to(p_xy[0] + c_i * 0, p_xy[1])
-      for pt in lines[1:]:
-        p_xy = to_xy(pt[0], pt[1])
-        c_i = get_ci(pt)
-        if o_c_i == self.c_i_select: cr.line_to(p_xy[0] + o_c_i * 0, p_xy[1])
-        if c_i != o_c_i:
-          o_c_i = c_i
-          with px(cr): cr.stroke()
-          if c_i == self.c_i_select: cr.move_to(p_xy[0] + c_i * 0, p_xy[1])
+    # Handle the expose-event by drawing
+    def handle_draw(self, cr):
+        # use "with px(cr): blah;" to transform to pixel coordinates.
 
-      with px(cr): cr.stroke()
+        # Fill the background color of the window with grey
+        cr.set_source_rgb(0.5, 0.5, 0.5)
+        cr.paint()
 
-    if not self.theta_version:
-      t1, t2 = to_theta(self.last_pos[0], self.last_pos[1], (self.c_i_select % 2) == 0)
-      x, y = joint_center[0], joint_center[1]
-      cr.move_to(x, y)
+        # Draw a extents rectangle
+        cr.set_source_rgb(1.0, 1.0, 1.0)
+        cr.rectangle(self.extents_x_min, self.extents_y_min,
+                     (self.extents_x_max - self.extents_x_min),
+                     self.extents_y_max - self.extents_y_min)
+        cr.fill()
 
-      x += numpy.cos(t1) * l1
-      y += numpy.sin(t1) * l1
-      cr.line_to(x, y)
-      x += numpy.cos(t2) * l2
-      y += numpy.sin(t2) * l2
-      cr.line_to(x, y)
-      with px(cr): cr.stroke()
+        if not self.theta_version:
+            # Draw a filled white rectangle.
+            cr.set_source_rgb(1.0, 1.0, 1.0)
+            cr.rectangle(-2.0, -2.0, 4.0, 4.0)
+            cr.fill()
 
-      cr.move_to(self.last_pos[0], self.last_pos[1])
-      cr.set_source_rgb(0.0, 1.0, 0.2)
-      draw_px_cross(cr, 20)
+            cr.set_source_rgb(0.0, 0.0, 1.0)
+            cr.arc(joint_center[0], joint_center[1], l2 + l1, 0,
+                   2.0 * numpy.pi)
+            with px(cr):
+                cr.stroke()
+            cr.arc(joint_center[0], joint_center[1], l1 - l2, 0,
+                   2.0 * numpy.pi)
+            with px(cr):
+                cr.stroke()
+        else:
+            # Draw a filled white rectangle.
+            cr.set_source_rgb(1.0, 1.0, 1.0)
+            cr.rectangle(-numpy.pi, -numpy.pi, numpy.pi * 2.0, numpy.pi * 2.0)
+            cr.fill()
 
-    if self.theta_version:
-      cr.set_source_rgb(0.0, 1.0, 0.2)
+        if self.theta_version:
+            cr.set_source_rgb(0.0, 0.0, 1.0)
+            for i in range(-6, 6):
+                cr.move_to(-40, -40 + i * numpy.pi)
+                cr.line_to(40, 40 + i * numpy.pi)
+            with px(cr):
+                cr.stroke()
 
-      cr.set_source_rgb(0.0, 1.0, 0.2)
-      cr.move_to(self.last_pos[0], self.last_pos[1])
-      draw_px_cross(cr, 5)
+        if self.theta_version:
+            cr.set_source_rgb(0.5, 0.5, 1.0)
+            draw_lines(cr, lines_theta)
+        else:
+            cr.set_source_rgb(0.5, 1.0, 1.0)
+            draw_lines(cr, lines1)
+            draw_lines(cr, lines2)
 
-      c_pt, dist = closest_segment(lines_theta, self.last_pos)
-      print("dist:", dist, c_pt, self.last_pos)
-      cr.set_source_rgb(0.0, 1.0, 1.0)
-      cr.move_to(c_pt[0], c_pt[1])
-      draw_px_cross(cr, 5)
+            def set_color(cr, circular_index):
+                if circular_index == -2:
+                    cr.set_source_rgb(0.0, 0.25, 1.0)
+                elif circular_index == -1:
+                    cr.set_source_rgb(0.5, 0.0, 1.0)
+                elif circular_index == 0:
+                    cr.set_source_rgb(0.5, 1.0, 1.0)
+                elif circular_index == 1:
+                    cr.set_source_rgb(0.0, 0.5, 1.0)
+                elif circular_index == 2:
+                    cr.set_source_rgb(0.5, 1.0, 0.5)
+                else:
+                    cr.set_source_rgb(1.0, 0.0, 0.0)
 
-    cr.set_source_rgb(0.0, 0.5, 1.0)
-    for seg in self.segs:
-      seg.DrawTo(cr, self.theta_version)
-      with px(cr): cr.stroke()
+            def get_circular_index(pt):
+                theta1, theta2 = pt
+                circular_index = int(numpy.floor((theta2 - theta1) / numpy.pi))
+                return circular_index
 
-    cr.set_source_rgb(0.0, 1.0, 0.5)
-    seg = self.current_seg()
-    print(seg)
-    if seg:
-      seg.DrawTo(cr, self.theta_version)
-      with px(cr): cr.stroke()
+            cr.set_source_rgb(0.0, 0.0, 1.0)
+            lines = subdivide_theta(lines_theta)
+            o_circular_index = circular_index = get_circular_index(lines[0])
+            p_xy = to_xy(lines[0][0], lines[0][1])
+            if circular_index == self.circular_index_select:
+                cr.move_to(p_xy[0] + circular_index * 0, p_xy[1])
+            for pt in lines[1:]:
+                p_xy = to_xy(pt[0], pt[1])
+                circular_index = get_circular_index(pt)
+                if o_circular_index == self.circular_index_select:
+                    cr.line_to(p_xy[0] + o_circular_index * 0, p_xy[1])
+                if circular_index != o_circular_index:
+                    o_circular_index = circular_index
+                    with px(cr):
+                        cr.stroke()
+                    if circular_index == self.circular_index_select:
+                        cr.move_to(p_xy[0] + circular_index * 0, p_xy[1])
 
-  def cur_pt_in_theta(self):
-    if self.theta_version: return self.last_pos
-    t1, t2 = to_theta(self.last_pos[0], self.last_pos[1], (self.c_i_select % 2) == 0)
-    n_ci = int(numpy.floor((t2 - t1) / numpy.pi))
-    t2 = t2 + ((self.c_i_select - n_ci)) * numpy.pi
-    return (t1, t2)
+            with px(cr):
+                cr.stroke()
 
-  # Current seg based on which mode the drawing system is in.
-  def current_seg(self):
-    if self.prev_seg_pt and self.now_seg_pt:
-      if self.theta_version:
-        return AngleSegment(self.prev_seg_pt, self.now_seg_pt)
-      else:
-        return XYSegment(self.prev_seg_pt, self.now_seg_pt)
+        if not self.theta_version:
+            theta1, theta2 = to_theta(self.last_pos, self.circular_index_select)
+            x, y = joint_center[0], joint_center[1]
+            cr.move_to(x, y)
 
-  def do_key_press(self, event):
-    print("Gdk.KEY_" + Gdk.keyval_name(event.keyval))
-    print("Gdk.KEY_" + Gdk.keyval_name(Gdk.keyval_to_lower(event.keyval)) + " is the lower case key for this button press.")
-    if ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_q ):
-      print("Found q key and exiting.")
-      quit_main_loop()
-    elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_c ):
-      self.c_i_select += 1
-    elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_v ):
-      self.c_i_select -= 1
-    elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_f ):
-      self.click_bool = not self.click_bool
+            x += numpy.cos(theta1) * l1
+            y += numpy.sin(theta1) * l1
+            cr.line_to(x, y)
+            x += numpy.cos(theta2) * l2
+            y += numpy.sin(theta2) * l2
+            cr.line_to(x, y)
+            with px(cr):
+                cr.stroke()
 
-    elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_w ):
-      seg = self.current_seg();
-      if seg: self.segs.append(seg)
-      self.prev_seg_pt = self.now_seg_pt
+            cr.move_to(self.last_pos[0], self.last_pos[1])
+            cr.set_source_rgb(0.0, 1.0, 0.2)
+            draw_px_cross(cr, 20)
 
-    elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_r ):
-      self.prev_seg_pt = self.now_seg_pt
+        if self.theta_version:
+            cr.set_source_rgb(0.0, 1.0, 0.2)
 
-    elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_p ):
-      print(repr(self.segs))
-    elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_g ):
-      if self.segs:
-        print(repr(self.segs[0].ToThetaPoints()))
-    elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_e ):
-      best_pt = self.now_seg_pt
-      best_dist = 1e10
-      for seg in self.segs:
-        d = angle_dist_sqr(seg.st, self.now_seg_pt)
-        if (d < best_dist):
-          best_pt = seg.st
-          best_dist = d;
-        d = angle_dist_sqr(seg.ed, self.now_seg_pt)
-        if (d < best_dist):
-          best_pt = seg.ed
-          best_dist = d
-      self.now_seg_pt = best_pt
+            cr.set_source_rgb(0.0, 1.0, 0.2)
+            cr.move_to(self.last_pos[0], self.last_pos[1])
+            draw_px_cross(cr, 5)
 
-    elif ( Gdk.keyval_to_lower(event.keyval) == Gdk.KEY_t ):
-      if self.theta_version:
-        t1, t2 = self.last_pos
-        data = to_xy(t1, t2)
-        self.c_i_select = int(numpy.floor((t2 - t1) / numpy.pi))
-        self.last_pos = (data[0], data[1])
-      else:
-        self.last_pos = self.cur_pt_in_theta()
+            c_pt, dist = closest_segment(lines_theta, self.last_pos)
+            print("dist:", dist, c_pt, self.last_pos)
+            cr.set_source_rgb(0.0, 1.0, 1.0)
+            cr.move_to(c_pt[0], c_pt[1])
+            draw_px_cross(cr, 5)
 
-      self.theta_version = not self.theta_version
-      self.reinit_extents()
-    self.redraw()
+        cr.set_source_rgb(0.0, 0.5, 1.0)
+        for segment in self.segments:
+            color = [0, random.random(), 1]
+            random.shuffle(color)
+            cr.set_source_rgb(*color)
+            segment.DrawTo(cr, self.theta_version)
+            with px(cr):
+                cr.stroke()
 
-  def do_button_press(self, event):
-    print(event)
-    print(event.x, event.y, event.button)
-    self.last_pos = (event.x, event.y)
-    self.now_seg_pt = self.cur_pt_in_theta();
+        cr.set_source_rgb(0.0, 1.0, 0.5)
+        segment = self.current_seg()
+        if segment:
+            print(segment)
+            segment.DrawTo(cr, self.theta_version)
+            with px(cr):
+                cr.stroke()
 
-    self.redraw()
+    def cur_pt_in_theta(self):
+        if self.theta_version: return self.last_pos
+        return to_theta(self.last_pos, self.circular_index_select)
+
+    # Current segment based on which mode the drawing system is in.
+    def current_seg(self):
+        if self.prev_segment_pt and self.now_segment_pt:
+            if self.theta_version:
+                return AngleSegment(self.prev_segment_pt, self.now_segment_pt)
+            else:
+                return XYSegment(self.prev_segment_pt, self.now_segment_pt)
+
+    def do_key_press(self, event):
+        keyval = Gdk.keyval_to_lower(event.keyval)
+        print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+        if keyval == Gdk.KEY_q:
+            print("Found q key and exiting.")
+            quit_main_loop()
+        elif keyval == Gdk.KEY_c:
+            # Increment which arm solution we render
+            self.circular_index_select += 1
+            print(self.circular_index_select)
+        elif keyval == Gdk.KEY_v:
+            # Decrement which arm solution we render
+            self.circular_index_select -= 1
+            print(self.circular_index_select)
+        elif keyval == Gdk.KEY_w:
+            # Add this segment to the segment list.
+            segment = self.current_seg()
+            if segment: self.segments.append(segment)
+            self.prev_segment_pt = self.now_segment_pt
+
+        elif keyval == Gdk.KEY_r:
+            self.prev_segment_pt = self.now_segment_pt
+
+        elif keyval == Gdk.KEY_p:
+            # Print out the segments.
+            print(repr(self.segments))
+        elif keyval == Gdk.KEY_g:
+            # Generate theta points.
+            if self.segments:
+                print(repr(self.segments[0].ToThetaPoints()))
+        elif keyval == Gdk.KEY_e:
+            best_pt = self.now_segment_pt
+            best_dist = 1e10
+            for segment in self.segments:
+                d = angle_dist_sqr(segment.start, self.now_segment_pt)
+                if (d < best_dist):
+                    best_pt = segment.start
+                    best_dist = d
+                d = angle_dist_sqr(segment.end, self.now_segment_pt)
+                if (d < best_dist):
+                    best_pt = segment.end
+                    best_dist = d
+            self.now_segment_pt = best_pt
+
+        elif keyval == Gdk.KEY_t:
+            # Toggle between theta and xy renderings
+            if self.theta_version:
+                theta1, theta2 = self.last_pos
+                data = to_xy(theta1, theta2)
+                self.circular_index_select = int(
+                    numpy.floor((theta2 - theta1) / numpy.pi))
+                self.last_pos = (data[0], data[1])
+            else:
+                self.last_pos = self.cur_pt_in_theta()
+
+            self.theta_version = not self.theta_version
+            self.reinit_extents()
+        self.redraw()
+
+    def do_button_press(self, event):
+        self.last_pos = (event.x, event.y)
+        self.now_segment_pt = self.cur_pt_in_theta()
+        print('Clicked at theta: (%f, %f)' % (self.now_segment_pt[0],
+                                              self.now_segment_pt[1]))
+        if not self.theta_version:
+            print('Clicked at xy, circular index: (%f, %f, %f)' %
+                  (self.last_pos[0], self.last_pos[1],
+                   self.circular_index_select))
+
+        self.redraw()
+
 
 silly = Silly()
-silly.segs = graph_generate.segs
+silly.segments = graph_generate.segments
 basic_window.RunApp()
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index 232d1a7..034021f 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -1,204 +1,499 @@
 import numpy
 
 # joint_center in x-y space.
-joint_center = (-12.275, 11.775)
+joint_center = (-0.299, 0.299)
 
 # Joint distances (l1 = "proximal", l2 = "distal")
-l1 = 46.25
-l2 = 43.75
+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 c_i mod 2.
-# where c_i is the circular index, or the position in the
+# 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(x, y, orient, cross_point = -numpy.pi):
-  x -= joint_center[0]
-  y -= joint_center[1]
-  l3 = numpy.sqrt(x ** 2 + y ** 2)
-  t3 = numpy.arctan2(y, x)
-  t1 = numpy.arccos((l1 ** 2 + l3 ** 2 - l2 ** 2) / (2 * l1 * l3))
+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:
-    t1 = -t1
-  t1 += t3
-  t1 = (t1 - cross_point) % (2 * numpy.pi) + cross_point
-  t2 = numpy.arctan2(y - l1 * numpy.sin(t1), x - l1 * numpy.cos(t1))
-  return (t1, t2)
+    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(t1, t2):
-  x = numpy.cos(t1) * l1 + numpy.cos(t2) * l2 + joint_center[0]
-  y = numpy.sin(t1) * l1 + numpy.sin(t2) * l2 + joint_center[1]
-  orient = ((t2 - t1) % (2 * numpy.pi)) < numpy.pi
-  return (x, y, orient)
+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])
+    cr.move_to(lines[0][0], lines[0][1])
+    for pt in lines[1:]:
+        cr.line_to(pt[0], pt[1])
 
-max_dist = 1.0
+
+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
+    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
+    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
+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
+    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_ci(x, y, ci):
-  t1, t2 = to_theta(x, y, (ci % 2) == 0)
-  n_ci = int(numpy.floor((t2 - t1) / numpy.pi))
-  t2 = t2 + ((ci - n_ci)) * numpy.pi
-  return numpy.array((t1, t2))
+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):
-  return b * alpha + (1 - alpha) * a
+    """Blends a and b.
 
-# Pure vector normalization.
+    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):
-  norm = numpy.linalg.norm(v)
-  if norm == 0:
-    return v
-  return v / norm
+    """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_ci_and_derivs(x, y, dx, dy, c_i_select):
-  a = to_theta_with_ci(x, y, c_i_select)
-  b = to_theta_with_ci(x + dx * 0.0001, y + dy * 0.0001, c_i_select)
-  c = to_theta_with_ci(x - dx * 0.0001, y - dy * 0.0001, 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])
+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)]
+    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)]
 
-# subdivision thresholds.
-max_dist = 1.0
-max_dist_theta = numpy.pi / 64
 
 # 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], pt[1], True, 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[0], pt[1], False, cross_point))
-    last_pt = n_pt
-  return out
+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
+    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
+    return out
 
-  items = [to_xy(t1, t2) for t1, t2 in lines]
-  return [(item[0], item[1]) for item in items]
 
 # Segment in angle space.
 class AngleSegment:
-  def __init__(self, st, ed):
-    self.st = st
-    self.ed = ed
-  def __repr__(self):
-    return "AngleSegment(%s, %s)" % (repr(self.st), repr(self.ed))
+    def __init__(self, start, end, name=None):
+        """Creates an angle segment.
 
-  def DrawTo(self, cr, theta_version):
-    if (theta_version):
-      cr.move_to(self.st[0], self.st[1])
-      cr.line_to(self.ed[0], self.ed[1])
-    else:
-      draw_lines(cr, back_to_xy_loop([self.st, self.ed]))
+        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
 
-  def ToThetaPoints(self):
-    return [self.st, self.ed]
+    def __repr__(self):
+        return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
 
-# Segment in X-Y space.
+    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:
-  def __init__(self, st, ed):
-    self.st = st
-    self.ed = ed
-  def __repr__(self):
-    return "XYSegment(%s, %s)" % (repr(self.st), repr(self.ed))
-  def DrawTo(self, cr, theta_version):
-    if (theta_version):
-      t1, t2 = self.st
-      c_i_select = int(numpy.floor((self.st[1] - self.st[0]) / numpy.pi))
-      st = to_xy(*self.st)
-      ed = to_xy(*self.ed)
+    """Straight line in XY space."""
 
-      ln = [(st[0], st[1]), (ed[0], ed[1])]
-      draw_lines(cr, [to_theta_with_ci(x, y, c_i_select) for x, y in subdivide_xy(ln)])
-    else:
-      st = to_xy(*self.st)
-      ed = to_xy(*self.ed)
-      cr.move_to(st[0], st[1])
-      cr.line_to(ed[0], ed[1])
+    def __init__(self, start, end, name=None):
+        """Creates an XY segment.
 
-  # Converts to points in theta space via to_theta_with_ci_and_derivs
-  def ToThetaPoints(self):
-    t1, t2 = self.st
-    c_i_select = int(numpy.floor((self.st[1] - self.st[0]) / numpy.pi))
-    st = to_xy(*self.st)
-    ed = to_xy(*self.ed)
+        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
 
-    ln = [(st[0], st[1]), (ed[0], ed[1])]
+    def __repr__(self):
+        return "XYSegment(%s, %s)" % (repr(self.start), repr(self.end))
 
-    dx = ed[0] - st[0]
-    dy = ed[1] - st[1]
-    mag = numpy.sqrt((dx) ** 2 + (dy) ** 2)
-    dx /= mag
-    dy /= mag
+    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)
 
-    return [to_theta_with_ci_and_derivs(x, y, dx, dy, c_i_select) for x, y in subdivide_xy(ln, 1.0)]
+            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)
 
-segs = [XYSegment((1.3583511559969876, 0.99753029519739866), (0.97145546090878643, -1.4797428713062153))]
-segs = [XYSegment((1.3583511559969876, 0.9975302951973987), (1.5666193247337956, 0.042054827580659759))]
+    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):
+        self.start = start
+        self.control1 = control1
+        self.control2 = control2
+        self.end = end
+        self.name = name
+
+    def __repr__(self):
+        return "XYSegment(%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)
+            ])
+        else:
+            start = get_xy(self.start)
+            control1 = get_xy(self.control1)
+            control2 = get_xy(self.control2)
+            end = get_xy(self.end)
+            #cr.move_to(start[0], start[1])
+            draw_lines(cr, [
+                spline_eval(start, control1, control2, end, alpha)
+                for alpha in subdivide_spline(start, control1, control2, end)
+            ])
+            # cr.spline_to(control1[0], control1[1], control2[0], control2[1], end[0], end[1])
+
+    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)
+        ]
+
+
+tall_box_x = 0.401
+tall_box_y = 0.13
+
+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_middle2_box = to_theta_with_circular_index(
+    0.732, 2.268, circular_index=-1)
+front_middle1_box = to_theta_with_circular_index(
+    0.878, 1.885, circular_index=-1)
+front_low_box = to_theta_with_circular_index(0.926, 1.522, 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.732, 2.268, circular_index=0)
+back_middle1_box = to_theta_with_circular_index(
+    -0.878, 1.885, circular_index=0)
+back_low_box = to_theta_with_circular_index(-0.926, 1.522, 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)
+
+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)
+
+ready_above_box_c1 = to_theta((0.38, 0.33), circular_index=-1)
+ready_above_box_c2 = to_theta((0.42, 0.51), circular_index=-1)
+
+points = [(ready_above_box, "ReadyAboveBox"),
+          (tall_box_grab, "TallBoxGrab"),
+          (short_box_grab, "ShortBoxGrab"),
+          (front_high_box, "FrontHighBox"),
+          (front_middle2_box, "FrontMiddle2Box"),
+          (front_middle1_box, "FrontMiddle1Box"),
+          (front_low_box, "FrontLowBox"),
+          (back_high_box, "BackHighBox"),
+          (back_middle2_box, "BackMiddle2Box"),
+          (back_middle1_box, "BackMiddle1Box"),
+          (back_low_box, "BackLowBox"),
+          (front_switch, "FrontSwitch"),
+          (back_switch, "BackSwitch"),
+          (neutral, "Neutral"),
+          (up, "Up")]  # yapf: disable
+
+# We need to define critical points so we can create paths connecting them.
+# TODO(austin): Attach velocities to the slow ones.
+named_segments = [
+    XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox"),
+    XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox"),
+    XYSegment(tall_box_grab, short_box_grab, "TallToShortBox"),
+    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+                  ready_above_box, "ReadyToNeutral"),
+    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, "NeutralToFronMiddle2"),
+    SplineSegment(neutral, front_middle1_box_c1, front_middle1_box_c2,
+                  front_middle1_box, "NeutralToFronMiddle1"),
+]
+
+unnamed_segments = [
+    AngleSegment(neutral, back_switch),
+    XYSegment(neutral, front_switch),
+
+    XYSegment(up, front_high_box),
+    XYSegment(up, front_middle2_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),
+    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),
+]
+
+segments = named_segments + unnamed_segments