Splines gui

Change-Id: Ic625138c1a9f39833c15542149fdeb11301b8c95
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index acb472c..a0ee825 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -167,6 +167,8 @@
     name = "path_edit",
     srcs = [
         "path_edit.py",
+        "basic_window.py",
+        "color.py",
     ],
     visibility = ["//visibility:public"],
     restricted_to = ["//tools:k8"],
@@ -174,7 +176,6 @@
         ":python_init",
         ":libspline",
         "@python_gtk",
-        ":basic_window",
     ],
 )
 
@@ -182,11 +183,24 @@
     name = "basic_window",
     srcs = [
         "basic_window.py",
-        "color.py",
+        "color.py"
     ],
+    visibility = ["//visibility:public"],
     restricted_to = ["//tools:k8"],
     deps = [
         ":python_init",
         "@python_gtk",
     ],
 )
+
+py_library(
+    name = "color",
+    srcs = [
+        "color.py",
+    ],
+    visibility = ["//visibility:public"],
+    restricted_to = ["//tools:k8"],
+    deps = [
+        ":python_init",
+    ],
+)
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
index 7a34b3a..78324a3 100644
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -4,7 +4,6 @@
 from gi.repository import GLib
 from gi.repository import Gdk
 from gi.repository import GdkX11
-from color import Color, palette
 import cairo
 
 identity = cairo.Matrix()
@@ -46,31 +45,13 @@
 
 # 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)
 
     # 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)
+        #self.window.connect("destroy", quit_main_loop)
 
-        self.set_size_request(640, 400)
-        self.window.add(self)
-        self.window.show_all()
+        self.set_size_request(640, 600)
         self.center = (0, 0)
         self.shape = (640, 400)
         self.needs_redraw = False
@@ -100,29 +81,3 @@
     # Handle the expose-event by drawing
     def handle_draw(self, cr):
         pass
-
-    def do_key_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_button_press(self, event):
-        pass
-
-    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/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
index 7a77637..e9e6c5f 100644
--- a/frc971/control_loops/python/libspline.py
+++ b/frc971/control_loops/python/libspline.py
@@ -12,7 +12,7 @@
     try:
         libSpline = ct.cdll.LoadLibrary(
             os.path.join(path, 'frc971/control_loops/drivetrain/spline.so'))
-    except OSError, e:
+    except (OSError):
         pass
 
 # Define required output types.
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index a0aa621..3a0a323 100644
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -1,34 +1,38 @@
 #!/usr/bin/python3
-
 from __future__ import print_function
 import os
+import copy
 import basic_window
 from color import Color, palette
 import random
 import gi
 import numpy as np
+from libspline import Spline
 import scipy.spatial.distance
 gi.require_version('Gtk', '3.0')
-from gi.repository import Gdk
+from gi.repository import Gdk, Gtk, GLib
 import cairo
-
 import enum
-import csv # For writing to csv files
+import csv  # For writing to csv files
 
 from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
 
 LENGTH_OF_FIELD = 323.65
 PIXELS_ON_SCREEN = 300
 
+
 def pxToIn(p):
-    return p*LENGTH_OF_FIELD/PIXELS_ON_SCREEN
+    return p * LENGTH_OF_FIELD / PIXELS_ON_SCREEN
+
 
 def inToPx(i):
-    return i*PIXELS_ON_SCREEN/LENGTH_OF_FIELD
+    return (i * PIXELS_ON_SCREEN / LENGTH_OF_FIELD)
+
 
 def px(cr):
     return OverrideMatrix(cr, identity)
 
+
 def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
     """Draws a cross with fixed dimensions in pixel space."""
     set_color(cr, color)
@@ -41,6 +45,7 @@
     cr.stroke()
     set_color(cr, palette["LIGHT_GREY"])
 
+
 def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
     """Draws a x with fixed dimensions in pixel space."""
     length_px = length_px1 / np.sqrt(2)
@@ -54,9 +59,12 @@
     cr.stroke()
     set_color(cr, palette["LIGHT_GREY"])
 
+
 def draw_points(cr, p, size):
     for i in range(0, len(p)):
-        draw_px_cross(cr, p[i][0], p[i][1], size, Color(0, np.sqrt(0.2 * i), 0))
+        draw_px_cross(cr, p[i][0], p[i][1], size, Color(
+            0, np.sqrt(0.2 * i), 0))
+
 
 class Mode(enum.Enum):
     kViewing = 0
@@ -64,14 +72,48 @@
     kEditing = 2
     kExporting = 3
     kImporting = 4
+    kConstraint = 5
+
+
+class ConstraintType(enum.Enum):
+    kMaxSpeed = 0
+    kMaxAcceleration = 1
+
 
 def display_text(cr, text, widtha, heighta, widthb, heightb):
     cr.scale(widtha, -heighta)
     cr.show_text(text)
     cr.scale(widthb, -heightb)
 
-# Create a GTK+ widget on which we will draw using Cairo
+
+def redraw(needs_redraw, window):
+    print("Redrew")
+    if not needs_redraw:
+        window.queue_draw()
+
+
+class Constraint():
+    def __init__(self, start, end, constraint, value):
+        self.start = start  #Array with index and distance from start of spline
+        self.end = end  #Array with index and distance from start of spline
+        self.constraint = constraint  #INT
+        self.value = value  #INT
+        if self.constraint == 0:
+            self.conName = "kMaxSpeed"
+        else:
+            self.conName = "kMaxAcceleration"
+
+    def toString(self):
+
+        return "START: " + str(self.start[0]) + ",   " + str(
+            self.start[1]) + " |  END: " + str(self.end[0]) + ",   " + str(
+                self.end[1]) + " |  " + str(self.conName) + ":  " + str(
+                    self.value)
+
+
 class GTK_Widget(basic_window.BaseWindow):
+    """Create a GTK+ widget on which we will draw using Cairo"""
+
     def __init__(self):
         super(GTK_Widget, self).__init__()
 
@@ -82,8 +124,6 @@
         self.x = 0
         self.y = 0
 
-        self.switch = True
-
         # update list of control points
         self.point_selected = False
         # self.adding_spline = False
@@ -91,20 +131,39 @@
         self.new_point = []
 
         # For the editing mode
-        self.index_of_edit = -1 # Can't be zero beause array starts at 0
+        self.index_of_edit = -1  # Can't be zero beause array starts at 0
         self.held_x = 0
+        self.spline_edit = -1
 
-        # Theo take them from here?
+        self.curves = []
+
+        self.colors = []
+
+        for c in palette:
+            self.colors.append(palette[c])
+
         self.selected_points = []
-
+        self.splines = []
+        self.spline = []
         self.reinit_extents()
 
+        self.inStart = None
+        self.inEnd = None
+        self.inConstraint = None
+        self.inValue = None
+        self.startSet = False
+
     #John also wrote this
     def add_point(self, x, y):
-        if(len(self.selected_points)<4):
-            self.selected_points.append([x,y])
+        if (len(self.selected_points) < 6):
+            self.selected_points.append([x, y])
+            if (len(self.selected_points) == 6):
+                self.mode = Mode.kEditing
+                self.splines.append(np.array(self.selected_points))
+                self.selected_points = []
 
-    """set extents on images, this needs to be redone with proper distances"""
+    """set extents on images"""
+
     def reinit_extents(self):
         self.extents_x_min = -800
         self.extents_x_max = 800
@@ -123,10 +182,55 @@
     def get_nearest_point(self):
         return self.all_controls[self.get_index_of_nearest_point()]
 
-      # Handle the expose-event by updating the Window and drawing
+    def set_index_to_nearest_spline_point(self):
+        nearest = 50
+        index_of_closest = 0
+        self.spline_edit = 0
+        cur_p = [self.x, self.y]
+
+        for index_splines, points in enumerate(self.spline):
+            for index_points, i in enumerate(points.curve):
+                # pythagorean
+                distance = np.sqrt((cur_p[0] - i[0])**2 + (cur_p[1] - i[1])**2)
+                if distance < nearest:
+                    nearest = distance
+                    print("DISTANCE: ", distance, " | INDEX: ", index_points)
+                    index_of_closest = index_points
+                    self.index_of_edit = index_of_closest
+                    self.spline_edit = index_splines
+                    self.held_x = self.x
+        if self.startSet == False:
+            self.inStart = [self.index_of_edit, self.findDistance()]
+            self.startSet = True
+        else:
+            self.inEnd = [self.index_of_edit, self.findDistance()]
+            self.spline[self.spline_edit].addConstraint(
+                self.inStart, self.inEnd, self.inConstraint, self.inValue)
+            self.startSet = False
+            self.mode = Mode.kEditing
+            self.spline_edit = -1
+            self.index_of_edit = -1
+
+        print("Nearest: " + str(nearest))
+        print("Spline: " + str(self.spline_edit))
+        print("Index: " + str(index_of_closest))
+
+    def findDistance(self):
+        """ findDistance goes through each point on the spline finding distance to that point from the point before.
+        It does this to find the the length of the spline to the point that is currently selected.
+        """
+        distance = 0
+        points = self.curves[self.spline_edit]
+        for index, point in enumerate(points):
+            if index > 0 and index <= self.index_of_edit:
+                distance += np.sqrt((points[index - 1][0] - point[0])**2 +
+                                    (points[index - 1][1] - point[1])**2)
+        return pxToIn(distance)
+
+    # Handle the expose-event by updating the Window and drawing
     def handle_draw(self, cr):
-        print(self.new_point)
-        print("SELF.POINT_SELECTED: " + str(self.point_selected))
+        # print(self.new_point)
+        # print("SELF.POINT_SELECTED: " + str(self.point_selected))
 
         # begin drawing
         # Fill the background color of the window with grey
@@ -146,25 +250,40 @@
         cr.show_text('Press "i" to import')
 
         set_color(cr, Color(0.3, 0.3, 0.3))
-        cr.rectangle(-150,-150,300,300)
+        cr.rectangle(-450, -150, 300, 300)
         cr.fill()
         set_color(cr, palette["BLACK"])
-        cr.rectangle(-150,-150,300,300)
+        cr.rectangle(-450, -150, 300, 300)
         cr.set_line_join(cairo.LINE_JOIN_ROUND)
         cr.stroke()
-        cr.rectangle(inToPx(140-161.825),inToPx(76.575),inToPx(56),inToPx(-153.15))
+        cr.rectangle((inToPx(140 - 161.825) - 300), inToPx(76.575), inToPx(56),
+                     inToPx(-153.15))
         cr.set_line_join(cairo.LINE_JOIN_ROUND)
         cr.stroke()
-        cr.rectangle(inToPx(161.825-24),inToPx(90),inToPx(24),inToPx(-180))
+        cr.rectangle((inToPx(161.825 - 24) - 300), inToPx(90), inToPx(24),
+                     inToPx(-180))
         cr.set_line_join(cairo.LINE_JOIN_ROUND)
         cr.stroke()
 
         set_color(cr, Color(0.2, 0.2, 0.2))
-        cr.rectangle(inToPx(140-161.825),inToPx(76.575),inToPx(56),inToPx(-153.15))
+        cr.rectangle(
+            inToPx(140 - 161.825) - 300, inToPx(76.575), inToPx(56),
+            inToPx(-153.15))
         cr.fill()
-        cr.rectangle(inToPx(161.825-24),inToPx(90),inToPx(24),inToPx(-180))
+        cr.rectangle(
+            inToPx(161.825 - 24) - 300, inToPx(90), inToPx(24), inToPx(-180))
         cr.fill()
 
+        y = 0
+        for x, i in enumerate(self.spline):
+            for j in i.constraints:
+                cr.move_to(-650, -y * 10 + 320)
+                set_color(cr, palette["BLACK"])
+                display_text(
+                    cr, str("Spline " + str(x) + ":   " + str(j.toString())),
+                    0.5, 0.5, 2, 2)
+                y += 1
+
         # update all the things
 
         if self.mode == Mode.kViewing:
@@ -172,32 +291,24 @@
             cr.move_to(-300, 170)
             cr.show_text("VIEWING")
             set_color(cr, palette["GREY"])
-            # its gonna check for points_added from button_press_action
-            # The behavior of the click is such that it runs twice
-            # This is consistant with graph_edit.py which someone smart wrote
-            # So I'm just going to delete the last element in order to not get
-            # repeating points
+
             if len(self.selected_points) > 0:
                 print("SELECTED_POINTS: " + str(len(self.selected_points)))
                 print("ITEMS:")
-                for item in self.selected_points:
-                    print(str(item))
+                # for item in self.selected_points:
+                # print(str(item))
                 for i, point in enumerate(self.selected_points):
-                    print("I: " + str(i))
+                    # print("I: " + str(i))
                     draw_px_x(cr, point[0], point[1], 10)
-                    cr.move_to(point[0], point[1]-15)
+                    cr.move_to(point[0], point[1] - 15)
                     display_text(cr, str(i), 0.5, 0.5, 2, 2)
 
-        if self.mode == Mode.kPlacing:
+        elif self.mode == Mode.kPlacing:
             set_color(cr, palette["BLACK"])
             cr.move_to(-300, 170)
             display_text(cr, "ADD", 1, 1, 1, 1)
             set_color(cr, palette["GREY"])
-            # its gonna check for points_added from button_press_action
-            # The behavior of the click is such that it runs twice
-            # This is consistant with graph_edit.py which someone smart wrote
-            # So I'm just going to delete the last element in order to not get
-            # repeating points
+
             if len(self.selected_points) > 0:
                 print("SELECTED_POINTS: " + str(len(self.selected_points)))
                 print("ITEMS:")
@@ -206,59 +317,76 @@
                 for i, point in enumerate(self.selected_points):
                     print("I: " + str(i))
                     draw_px_x(cr, point[0], point[1], 10)
-                    cr.move_to(point[0], point[1]-15)
+                    cr.move_to(point[0], point[1] - 15)
                     display_text(cr, str(i), 0.5, 0.5, 2, 2)
-                    if(i==3):
-                        self.mode = Mode.kEditing
 
         elif self.mode == Mode.kEditing:
             set_color(cr, palette["BLACK"])
             cr.move_to(-300, 170)
             display_text(cr, "EDITING", 1, 1, 1, 1)
-            set_color(cr, palette["GREY"])
-            if len(self.selected_points) > 0:
-                print("SELECTED_POINTS: " + str(len(self.selected_points)))
-                print("ITEMS:")
-                for item in self.selected_points:
-                    print(str(item))
-                for i, point in enumerate(self.selected_points):
-                    print("I: " + str(i))
-                    draw_px_x(cr, point[0], point[1], 10)
-                    cr.move_to(point[0], point[1]-15)
-                    display_text(cr, str(i), 0.5, 0.5, 2, 2)
+            if len(self.splines) > 0:
+                # print("Splines: " + str(len(self.splines)))
+                # print("ITEMS:")
+                holder_spline = []
+                for i, points in enumerate(self.splines):
+                    array = np.zeros(shape=(6, 2), dtype=float)
+                    for j, point in enumerate(points):
+                        array[j, 0] = point[0]
+                        array[j, 1] = point[1]
+                    spline = Spline(np.ascontiguousarray(np.transpose(array)))
+                    for k in np.linspace(0.01, 1, 100):
+
+                        cr.move_to(
+                            spline.Point(k - 0.01)[0],
+                            spline.Point(k - 0.01)[1])
+                        cr.line_to(spline.Point(k)[0], spline.Point(k)[1])
+                        cr.stroke()
+                        holding = [
+                            spline.Point(k - 0.01)[0],
+                            spline.Point(k - 0.01)[1]
+                        ]
+
+                        holder_spline.append(holding)
+                self.curves.append(holder_spline)
+
+                for spline, points in enumerate(self.splines):
+                    # for item in points:
+                    #     print(str(item))
+                    for i, point in enumerate(points):
+                        # print("I: " + str(i))
+                        if spline == self.spline_edit and i == self.index_of_edit:
+                            draw_px_x(cr, point[0], point[1], 15,
+                                      self.colors[spline])
+                        elif (spline == 0 and not i == 5) or (not i == 0
+                                                              and not i == 5):
+                            draw_px_x(cr, point[0], point[1], 10,
+                                      self.colors[spline])
+                        cr.move_to(point[0], point[1] - 15)
+                        display_text(cr, str(i), 0.5, 0.5, 2, 2)
 
         elif self.mode == Mode.kExporting:
             set_color(cr, palette["BLACK"])
             cr.move_to(-300, 170)
             display_text(cr, "VIEWING", 1, 1, 1, 1)
             set_color(cr, palette["GREY"])
-            #its gonna check for points_added from button_press_action
 
-            # The behavior of the click is such that it runs twice
-            # This is consistant with graph_edit.py which someone smart wrote
-            # So I'm just going to delete the last element in order to not get
-            # repeating points
             if len(self.selected_points) > 0:
                 print("SELECTED_POINTS: " + str(len(self.selected_points)))
                 print("ITEMS:")
-                for item in self.selected_points:
-                    print(str(item))
+                # for item in self.selected_points:
+                #     print(str(item))
                 for i, point in enumerate(self.selected_points):
-                    print("I: " + str(i))
+                    # print("I: " + str(i))
                     draw_px_x(cr, point[0], point[1], 10)
-                    cr.move_to(point[0], point[1]-15)
+                    cr.move_to(point[0], point[1] - 15)
                     display_text(cr, str(i), 0.5, 0.5, 2, 2)
+
         elif self.mode == Mode.kImporting:
             set_color(cr, palette["BLACK"])
             cr.move_to(-300, 170)
             display_text(cr, "VIEWING", 1, 1, 1, 1)
             set_color(cr, palette["GREY"])
-            # its gonna check for points_added from button_press_action
 
-            # The behavior of the click is such that it runs twice
-            # This is consistant with graph_edit.py which someone smart wrote
-            # So I'm just going to delete the last element in order to not get
-            # repeating points
             if len(self.selected_points) > 0:
                 print("SELECTED_POINTS: " + str(len(self.selected_points)))
                 print("ITEMS:")
@@ -267,9 +395,25 @@
                 for i, point in enumerate(self.selected_points):
                     print("I: " + str(i))
                     draw_px_x(cr, point[0], point[1], 10)
-                    cr.move_to(point[0], point[1]-15)
+                    cr.move_to(point[0], point[1] - 15)
                     display_text(cr, str(i), 0.5, 0.5, 2, 2)
 
+        elif self.mode == Mode.kConstraint:
+            print("Drawn")
+            set_color(cr, palette["BLACK"])
+            cr.move_to(-300, 170)
+            display_text(cr, "Adding Constraint", 1, 1, 1, 1)
+            if len(self.splines) > 0:
+                # print("Splines: " + str(len(self.splines)))
+                # print("ITEMS:")
+                for s, points in enumerate(self.splines):
+                    # for item in points:
+                    #     print(str(item))
+                    for i, point in enumerate(points):
+                        # print("I: " + str(i))
+                        draw_px_x(cr, point[0], point[1], 10, self.colors[s])
+                        cr.move_to(point[0], point[1] - 15)
+                        display_text(cr, str(i), 0.5, 0.5, 2, 2)
 
         cr.paint_with_alpha(.65)
 
@@ -277,7 +421,7 @@
 
     def do_key_press(self, event):
         keyval = Gdk.keyval_to_lower(event.keyval)
-        print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+        # print("Gdk.KEY_" + Gdk.keyval_name(keyval))
         if keyval == Gdk.KEY_q:
             print("Found q key and exiting.")
             quit_main_loop()
@@ -285,11 +429,14 @@
             self.mode = Mode.kExporting
             # Will export to csv file
             with open('points_for_pathedit.csv', mode='w') as points_file:
-                writer = csv.writer(points_file, delimiter=',', quotechar='"',
-                        quoting=csv.QUOTE_MINIMAL)
+                writer = csv.writer(
+                    points_file,
+                    delimiter=',',
+                    quotechar='"',
+                    quoting=csv.QUOTE_MINIMAL)
                 for item in self.selected_points:
                     writer.writerow([str(item[0]), str(item[1])])
-                    print("Wrote: " + str(item[0]) + " " +  str(item[1]))
+                    print("Wrote: " + str(item[0]) + " " + str(item[1]))
         if keyval == Gdk.KEY_i:
             self.mode = Mode.kImporting
             # import from csv file
@@ -298,53 +445,95 @@
                 reader = csv.reader(points_file, delimiter=',')
                 for row in reader:
                     self.add_point(float(row[0]), float(row[1]))
-                    print("Added: " + row[0] + " " +  row[1])
+                    print("Added: " + row[0] + " " + row[1])
+        if keyval == Gdk.KEY_p:
+            self.mode = Mode.kPlacing
+            # F0 = A1
+            # B1 = 2F0 - E0
+            # C1= d0 + 4F0 - 4E0
+            spline_index = len(self.splines) - 1
+            self.selected_points = []
+            f = self.splines[spline_index][5]
+            e = self.splines[spline_index][4]
+            d = self.splines[spline_index][3]
+            self.selected_points.append(f)
+            self.selected_points.append(f * 2 + e * -1)
+            self.selected_points.append(d + f * 4 + e * -4)
 
-        self.redraw()
+        if keyval == Gdk.KEY_c:
+            self.mode = Mode.kConstraint
 
     def button_press_action(self):
-        if self.switch:
-            self.switch = False
-            if self.mode == Mode.kPlacing:
-                #Check that the point clicked is on the field
-                if(self.x<150 and self.x>-150 and self.y <150 and self.y >-150):
-                    self.add_point(self.x, self.y)
-            if self.mode == Mode.kEditing:
-                # Now after index_of_edit is not -1, the point is selected, so
-                # user can click for new point
-                    print("INDEX OF EDIT: " + str(self.index_of_edit))
+        if self.mode == Mode.kPlacing:
+            #Check that the point clicked is on the field
+            if (self.x < -150 and self.x > -450 and self.y < 150
+                    and self.y > -150):
+                self.add_point(self.x, self.y)
+        elif self.mode == Mode.kEditing:
+            # Now after index_of_edit is not -1, the point is selected, so
+            # user can click for new point
+            if self.index_of_edit > -1 and self.held_x != self.x:
+                print("INDEX OF EDIT: " + str(self.index_of_edit))
+                self.splines[self.spline_edit][self.index_of_edit] = [
+                    self.x, self.y
+                ]
 
-                    if self.index_of_edit > -1 and self.held_x != self.x:
-                        print("INDEX OF EDIT: " + str(self.index_of_edit))
-                        self.selected_points[self.index_of_edit] = [self.x, self.y]
-                        self.index_of_edit = -1
-                    else:
-                        print("mode == 2")
-                        # Get clicked point
-                        # Find nearest
-                        # Move nearest to clicked
-                        cur_p = [self.x, self.y]
-                        print("CUR_P: " + str(self.x) + " " + str(self.y))
-                        # What I wanna do is get each point
-                        # Get the distance between each for x and y
-                        # Save the index of the point closest
-                        nearest = 1000
-                        index = 0
-                        for ind, i in enumerate(self.selected_points):
-                            # pythagorean
-                            distance = np.sqrt((cur_p[0] - i[0])**2 + (cur_p[1] - i[1])**2)
-                            if distance < nearest:
-                                nearest = distance
-                                index = ind
-                                print("Nearest: " + str(nearest))
-                                print("Index: " + str(index))
-                                self.index_of_edit = index
-                                self.held_x = self.x
-        else:
-            self.switch = True
+                if not self.spline_edit == len(self.splines) - 1:
+                    spline_edit = self.spline_edit + 1
+                    f = self.splines[self.spline_edit][5]
+                    e = self.splines[self.spline_edit][4]
+                    d = self.splines[self.spline_edit][3]
+                    self.splines[spline_edit][0] = f
+                    self.splines[spline_edit][1] = f * 2 + e * -1
+                    self.splines[spline_edit][2] = d + f * 4 + e * -4
 
-        self.redraw()
+                    self.spline[spline_edit].point = self.splines[spline_edit]
+                    self.spline[spline_edit].math()
 
+                if not self.spline_edit == 0:
+                    spline_edit = self.spline_edit - 1
+                    a = self.splines[self.spline_edit][0]
+                    b = self.splines[self.spline_edit][1]
+                    c = self.splines[self.spline_edit][2]
+                    self.splines[spline_edit][5] = a
+                    self.splines[spline_edit][4] = a * 2 + b * -1
+                    self.splines[spline_edit][3] = c + a * 4 + b * -4
+
+                    self.spline[spline_edit].point = self.splines[spline_edit]
+                    self.spline[spline_edit].math()
+
+                self.spline[self.spline_edit].edit(self.index_of_edit,
+                                                   [self.x, self.y])
+                self.index_of_edit = -1
+                self.spline_edit = -1
+            else:
+                print("mode == 2")
+                # Get clicked point
+                # Find nearest
+                # Move nearest to clicked
+                cur_p = [self.x, self.y]
+                print("CUR_P: " + str(self.x) + " " + str(self.y))
+                # Get the distance between each for x and y
+                # Save the index of the point closest
+                nearest = 50
+                index_of_closest = 0
+                for index_splines, points in enumerate(self.splines):
+                    for index_points, val in enumerate(points):
+                        # pythagorean
+                        distance = np.sqrt((cur_p[0] - val[0])**2 +
+                                           (cur_p[1] - val[1])**2)
+                        if distance < nearest:
+                            nearest = distance
+                            index_of_closest = index_points
+                            print("Nearest: " + str(nearest))
+                            print("Index: " + str(index_of_closest))
+                            self.index_of_edit = index_of_closest
+                            self.spline_edit = index_splines
+                            self.held_x = self.x
+        elif self.mode == Mode.kConstraint:
+            print("RAN")
+            self.set_index_to_nearest_spline_point()
+            print("FINISHED")
 
     def do_button_press(self, event):
         print("button press activated")
@@ -353,6 +542,100 @@
         self.button_press_action()
 
 
+class GridWindow(Gtk.Window):
+    def method_connect(self, event, cb):
+        def handler(obj, *args):
+            cb(*args)
 
-silly = GTK_Widget()
+        print("Method_connect ran")
+        self.connect(event, handler)
+
+    def button_press(self, event):
+        print("button press activated")
+        o_x = event.x
+        o_y = event.y
+        x = event.x - self.drawing_area.window_shape[0] / 2
+        y = self.drawing_area.window_shape[1] / 2 - event.y
+        scale = self.drawing_area.get_current_scale()
+        event.x = x / scale + self.drawing_area.center[0]
+        event.y = y / scale + self.drawing_area.center[1]
+        self.drawing_area.do_button_press(event)
+        event.x = o_x
+        event.y = o_y
+
+    def key_press(self, event):
+        print("key press activated")
+        self.drawing_area.do_key_press(event)
+        self.queue_draw()
+
+    def configure(self, event):
+        print("configure activated")
+        self.drawing_area.window_shape = (event.width, event.height)
+
+    def on_submit_click(self, widget):
+        self.drawing_area.inConstraint = int(self.constraint_box.get_text())
+        self.drawing_area.inValue = int(self.value_box.get_text())
+
+    def __init__(self):
+        Gtk.Window.__init__(self)
+
+        self.set_default_size(1366, 738)
+
+        flowBox = Gtk.FlowBox()
+        flowBox.set_valign(Gtk.Align.START)
+        flowBox.set_selection_mode(Gtk.SelectionMode.NONE)
+
+        flowBox.set_valign(Gtk.Align.START)
+
+        self.add(flowBox)
+
+        container = Gtk.Fixed()
+        flowBox.add(container)
+
+        self.eventBox = Gtk.EventBox()
+        container.add(self.eventBox)
+
+        self.eventBox.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.drawing_area = GTK_Widget()
+        self.eventBox.add(self.drawing_area)
+
+        self.method_connect("key-release-event", self.key_press)
+        self.method_connect("button-release-event", self.button_press)
+        self.method_connect("configure-event", self.configure)
+
+        # Constraint Boxes
+
+        self.start_box = Gtk.Entry()
+        self.start_box.set_size_request(100, 20)
+
+        self.constraint_box = Gtk.Entry()
+        self.constraint_box.set_size_request(100, 20)
+
+        self.constraint_box.set_text("Constraint")
+        self.constraint_box.set_editable(True)
+
+        container.put(self.constraint_box, 700, 0)
+
+        self.value_box = Gtk.Entry()
+        self.value_box.set_size_request(100, 20)
+
+        self.value_box.set_text("Value")
+        self.value_box.set_editable(True)
+
+        container.put(self.value_box, 700, 40)
+
+        self.submit_button = Gtk.Button("Submit")
+        self.submit_button.connect('clicked', self.on_submit_click)
+
+        container.put(self.submit_button, 880, 0)
+
+        self.show_all()
+
+
+window = GridWindow()
 basic_window.RunApp()