diff --git a/src/geo_controller.py b/src/geo_controller.py
index 1fe62aa..7007f42 100755
--- a/src/geo_controller.py
+++ b/src/geo_controller.py
@@ -18,8 +18,43 @@ def get_range(data, theta):
     print("range: " + str(data.ranges[index]))
     # Return the LiDAR scan value at that index
     # Do some error checking for NaN and ubsurd values
-    ## Your code goes here
-    return data.ranges[index]
+    ranges = data.ranges[index-2:index+3]
+    r = np.mean(ranges)
+    return r
+
+
+def get_steering_angle(lowpoint, highpoint):
+    """
+    This function returns the steering angle with the appropriate signage
+    X<-HighPoint
+    |\
+    | \
+    |  \
+    |   \
+    |    \
+    L-----X<-LowPoint
+    :param lowpoint: The point closest in the y dimension to the car, as a tuple (x,y)
+    :param highpoint: The point furthest from the car in the y dimension, as a tuple (x,y)
+    :return: angle in radians
+    """
+    delx = float(highpoint[0])-float(lowpoint[0])
+    dely = float(highpoint[1])-float(lowpoint[1])
+    hyp = math.sqrt(delx**2 + dely**2)
+    theta = -1*math.asin(delx/hyp)
+    return theta
+
+
+def get_coords(theta, range):
+    """
+    This maps polar to rectangular coordinates given the LiDAR's frame of reference.
+    We rotate 90deg to right to reorient to polar origin
+    :param theta: LiDAR theta
+    :param range: LiDAR range
+    :return:
+    """
+    y = math.cos(theta)*range
+    x = -1 * math.sin(theta)*range
+    return x, y
 
 
 def laser_subscriber(msg, thetas):
@@ -42,10 +77,7 @@ def main():
     print("Geometric Controller Initialized")
     rospy.init_node('geometric_controller_filter')
     pub = rospy.Publisher("drive_parameters", drive_params)
-    # laser_sub = rospy.Subscriber("scan", LaserScan, laser_subscriber, [-math.pi/2, -11*math.pi/24])
-    # rospy.spin()
-    counter = 0
-    while (not rospy.core.is_shutdown()):
+    while not rospy.core.is_shutdown():
         msg = rospy.client.wait_for_message('scan', LaserScan)
         print(msg.angle_min, msg.angle_max, msg.angle_increment)
         TOP = 1
@@ -53,24 +85,31 @@ def main():
         thetas = 3*math.pi/24
         thetar = [-math.pi / 2, (-math.pi / 2)+thetas]
         rangesr = [get_range(msg, t) for t in thetar]
+        pointsr = [(rangesr[BOT], 0), get_coords(thetar[TOP], rangesr[TOP])]
+        thetasr = get_steering_angle(pointsr[TOP], pointsr[BOT])
+        print('pointsr', pointsr)
+        print('thetasR', thetasr)
+
         thetal = [math.pi / 2, (math.pi / 2)-thetas]
         rangesl = [get_range(msg, t) for t in thetal]
-        #print(counter)
-        #counter += 1
-        #print(ranges)
-        d1r = rangesr[BOT]
-        d2r = rangesr[TOP]
-        d3r = math.sqrt(d1r**2 + d2r**2 - 2*d1r*d2r*math.cos(thetas))
-        deltaxr = d2r*math.cos(thetas) - d1r
-        thetadr = math.asin(deltaxr/d3r)
-        d1l = rangesl[BOT]
-        d2l = rangesl[TOP]
-        d3l = math.sqrt(d1l**2 + d2l**2 - 2*d1l*d2l*math.cos(thetas))
-        deltaxl = (d2l*math.cos(thetas) - d1l)
-        thetadl = math.asin(deltaxl/d3l)
-        thetad = thetadl + thetadr
-        steering_input = interp(thetad, [np.radians(-20), np.radians(20)], [-100, 100])
-        print("ThetaR:", thetad, "|Steering Angle:", steering_input)
+        pointsl = [(-rangesl[BOT], 0), get_coords(thetal[TOP], rangesl[TOP])]
+        thetasl = get_steering_angle(pointsl[TOP], pointsl[BOT])
+        print('pointsl', pointsl)
+        print('thetasL', thetasl)
+
+        """
+        Weight the thetas by how far the max distance is to the wall.
+        This should keep us away from walls.
+        """
+        maxr = max(rangesr)**2
+        maxl = max(rangesl)**2
+        rweight = maxr/(maxr+maxl)
+        lweight = maxl/(maxr+maxl)
+        print("Weights:", lweight, "|", rweight)
+
+        thetas = thetasl*lweight + thetasr*rweight
+        steering_input = interp(thetas, [np.radians(-20), np.radians(20)], [-100, 100])
+        print("ThetaR:", thetas, "|Steering Angle:", steering_input)
         message = drive_params()
         message.angle = steering_input
         message.velocity = 15