diff --git a/Makefile b/Makefile
index b099521..b2eab6a 100644
--- a/Makefile
+++ b/Makefile
@@ -30,7 +30,7 @@
 DEVICE     = atmega168
 CLOCK      = 16000000
 PROGRAMMER = -c avrisp2 -P usb
-OBJECTS    = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o geometry.o
+OBJECTS    = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o
 FUSES      = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
 
 # Tune the lines below only if you know what you are doing:
diff --git a/gcode.c b/gcode.c
index 90a1792..f68c61b 100644
--- a/gcode.c
+++ b/gcode.c
@@ -50,7 +50,6 @@
 #include "config.h"
 #include "motion_control.h"
 #include "spindle_control.h"
-#include "geometry.h"
 #include "errno.h"
 #include "serial_protocol.h"
 
@@ -113,13 +112,29 @@ void gc_init() {
   memset(&gc, 0, sizeof(gc));
   gc.feed_rate = DEFAULT_FEEDRATE/60;
   select_plane(X_AXIS, Y_AXIS, Z_AXIS);
-  gc.absolute_mode = true;
+  gc.absolute_mode = TRUE;
 }
 
 inline float to_millimeters(double value) {
   return(gc.inches_mode ? (value * INCHES_PER_MM) : value);
 }
 
+// Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis, 
+// positive to the right.
+double theta(double x, double y)
+{
+  double theta = atan(x/fabs(y));
+  if (y>0) {
+    return(theta);
+  } else {
+    if (theta>0) 
+    {
+      return(M_PI-theta);
+    } else {
+      return(-M_PI-theta);
+    }
+  }
+}
 
 // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
 // characters and signed floats (no whitespace).
@@ -129,9 +144,9 @@ uint8_t gc_execute_line(char *line) {
   double value;
   double unit_converted_value;
   double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
-  int radius_mode = false;
+  int radius_mode = FALSE;
   
-  uint8_t absolute_override = false;       /* 1 = absolute motion for this block only {G53} */
+  uint8_t absolute_override = FALSE;       /* 1 = absolute motion for this block only {G53} */
   uint8_t next_action = NEXT_ACTION_DEFAULT;         /* One of the NEXT_ACTION_-constants */
   
   double target[3], offset[3];  
@@ -163,15 +178,15 @@ uint8_t gc_execute_line(char *line) {
         case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
         case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
         case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
-        case 20: gc.inches_mode = true; break;
-        case 21: gc.inches_mode = false; break;
+        case 20: gc.inches_mode = TRUE; break;
+        case 21: gc.inches_mode = FALSE; break;
         case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
-        case 53: absolute_override = true; break;
+        case 53: absolute_override = TRUE; break;
         case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
-        case 90: gc.absolute_mode = true; break;
-        case 91: gc.absolute_mode = false; break;
-        case 93: gc.inverse_feed_rate_mode = true; break;
-        case 94: gc.inverse_feed_rate_mode = false; break;
+        case 90: gc.absolute_mode = TRUE; break;
+        case 91: gc.absolute_mode = FALSE; break;
+        case 93: gc.inverse_feed_rate_mode = TRUE; break;
+        case 94: gc.inverse_feed_rate_mode = FALSE; break;
         default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT);
       }
       break;
@@ -212,7 +227,7 @@ uint8_t gc_execute_line(char *line) {
       break;
       case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break;
       case 'P': p = value; break;
-      case 'R': r = unit_converted_value; radius_mode = true; break;
+      case 'R': r = unit_converted_value; radius_mode = TRUE; break;
       case 'S': gc.spindle_speed = value; break;
       case 'X': case 'Y': case 'Z':
       if (gc.absolute_mode || absolute_override) {
diff --git a/gcode.h b/gcode.h
index aada2f9..f3a1d32 100644
--- a/gcode.h
+++ b/gcode.h
@@ -36,7 +36,4 @@ void gc_init();
 // Execute one block of rs275/ngc/g-code
 uint8_t gc_execute_line(char *line);
 
-// get the current logical position (in current units), the current status code and the unit mode
-void gc_get_status(double *position_, uint8_t *status_code_, int *inches_mode_, uint32_t *line_number_);
-
 #endif
diff --git a/geometry.c b/geometry.c
deleted file mode 100644
index 86aff47..0000000
--- a/geometry.c
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
-  geometry.h - a place for geometry helpers
-  Part of Grbl
-
-  Copyright (c) 2009 Simen Svale Skogsrud
-
-  Grbl is free software: you can redistribute it and/or modify
-  it under the terms of the GNU General Public License as published by
-  the Free Software Foundation, either version 3 of the License, or
-  (at your option) any later version.
-
-  Grbl is distributed in the hope that it will be useful,
-  but WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-  GNU General Public License for more details.
-
-  You should have received a copy of the GNU General Public License
-  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#include "geometry.h"
-#include <avr/io.h>
-#include <math.h>
-#include <stdlib.h>
-
-// Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis, 
-// positive to the right.
-double theta(double x, double y)
-{
-  double theta = atan(x/fabs(y));
-  if (y>0) {
-    return(theta);
-  } else {
-    if (theta>0) 
-    {
-      return(M_PI-theta);
-    } else {
-      return(-M_PI-theta);
-    }
-  }
-}
-
diff --git a/geometry.h b/geometry.h
deleted file mode 100644
index 1f2ecb5..0000000
--- a/geometry.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
-  geometry.h - a place for geometry helpers
-  Part of Grbl
-
-  Copyright (c) 2009 Simen Svale Skogsrud
-
-  Grbl is free software: you can redistribute it and/or modify
-  it under the terms of the GNU General Public License as published by
-  the Free Software Foundation, either version 3 of the License, or
-  (at your option) any later version.
-
-  Grbl is distributed in the hope that it will be useful,
-  but WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-  GNU General Public License for more details.
-
-  You should have received a copy of the GNU General Public License
-  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#ifndef geometry_h
-#define geometry_h 
-#include <avr/io.h>
-
-// Find the angle from the positive y axis to the given point with respect to origo.
-double theta(double x, double y);
-
-#endif
diff --git a/motion_control.c b/motion_control.c
index e878411..5ac41f0 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -34,18 +34,11 @@
 #include <stdlib.h>
 #include "nuts_bolts.h"
 #include "stepper.h"
-#include "geometry.h"
 
 #include "wiring_serial.h"
 
-#define ONE_MINUTE_OF_MICROSECONDS 60000000.0
-
 int32_t position[3];    // The current position of the tool in absolute steps
 
-inline void step_steppers(uint8_t bits);
-inline void step_axis(uint8_t axis);
-void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate);
-
 void mc_init()
 {
   clear_vector(position);
@@ -53,7 +46,8 @@ void mc_init()
 
 void mc_dwell(uint32_t milliseconds) 
 {
-  st_buffer_line(0, 0, 0, milliseconds*1000);
+  st_synchronize();
+  _delay_ms(milliseconds);
 }
 
 // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
@@ -61,13 +55,10 @@ void mc_dwell(uint32_t milliseconds)
 // 1/feed_rate minutes.
 void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate)
 {
-	// Flags to keep track of which axes to step
   uint8_t axis; // loop variable
   int32_t target[3]; // The target position in absolute steps
   int32_t steps[3]; // The target line in relative steps
   
-  // Setup ---------------------------------------------------------------------------------------------------
-
   target[X_AXIS] = lround(x*X_STEPS_PER_MM);
   target[Y_AXIS] = lround(y*Y_STEPS_PER_MM);
   target[Z_AXIS] = lround(z*Z_STEPS_PER_MM); 
@@ -95,6 +86,10 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
 // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the
 // circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining
 // axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
+
+// The arc is approximated by generating a huge number of tiny, linear segments. The length of each 
+// segment is configured in config.h by setting MM_PER_ARC_SEGMENT.  
+
 // ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis.
 void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, 
   int axis_linear, double feed_rate, int invert_feed_rate)
@@ -102,14 +97,22 @@ void mc_arc(double theta, double angular_travel, double radius, double linear_tr
   double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel));
   if (millimeters_of_travel == 0.0) { return; }
   uint16_t segments = ceil(millimeters_of_travel/MM_PER_ARC_SEGMENT);
+  // Multiply inverse feed_rate to compensate for the fact that this movement is approximated
+  // by a number of discrete segments. The inverse feed_rate should be correct for the sum of 
+  // all segments.
   if (invert_feed_rate) { feed_rate *= segments; }
+  // The angular motion for each segment
   double theta_per_segment = angular_travel/segments;
+  // The linear motion for each segment
   double linear_per_segment = linear_travel/segments;
+  // Compute the center of this circle
   double center_x = (position[axis_1]/X_STEPS_PER_MM)-sin(theta)*radius;
   double center_y = (position[axis_2]/Y_STEPS_PER_MM)-cos(theta)*radius;
+  // a vector to track the end point of each segment
   double target[3];
   int i;
-  target[axis_linear] = position[axis_linear];
+  // Initialize the linear axis
+  target[axis_linear] = position[axis_linear]/Z_STEPS_PER_MM;
   for (i=0; i<=segments; i++) {
     target[axis_linear] += linear_per_segment;
     theta += theta_per_segment;
diff --git a/motion_control.h b/motion_control.h
index 5927d9c..dcee5eb 100644
--- a/motion_control.h
+++ b/motion_control.h
@@ -23,26 +23,18 @@
 
 #include <avr/io.h>
 
-#define MC_MODE_AT_REST 0
-#define MC_MODE_LINEAR 1
-#define MC_MODE_ARC 2
-#define MC_MODE_DWELL 3
-#define MC_MODE_HOME 4
-
 // Initializes the motion_control subsystem resources
 void mc_init();
 
 // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
 // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
-// 1/feed_rate minutes.
+// (1 minute)/feed_rate time.
 void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate);
 
-// Prepare an arc. theta == start angle, angular_travel == number of radians to go along the arc,
+// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
 // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the
-// circle in millimeters. axis_1 and axis_2 selects the plane in tool space. 
-// Known issue: This method pretends that all axes uses the same steps/mm as the X axis. Which might
-// not be the case ... (To be continued) 
-// Regarding feed rate see note on mc_line.
+// circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining
+// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
 void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, 
   int axis_linear, double feed_rate, int invert_feed_rate);
 
@@ -52,8 +44,4 @@ void mc_dwell(uint32_t milliseconds);
 // Send the tool home
 void mc_go_home();
 
-// Check motion control status. result == 0: the system is idle. result > 0: the system is busy,
-// result < 0: the system is in an error state.
-int mc_status();
-
 #endif
diff --git a/nuts_bolts.h b/nuts_bolts.h
index 6e90e27..f27f4c6 100644
--- a/nuts_bolts.h
+++ b/nuts_bolts.h
@@ -20,14 +20,17 @@
 
 #ifndef nuts_bolts_h
 #define nuts_bolts_h
-
 #include <string.h>
 
+#define ONE_MINUTE_OF_MICROSECONDS 60000000.0
+#define TICKS_PER_MICROSECOND (F_CPU/1000000)
+
+
 #define max(a,b) (((a) > (b)) ? (a) : (b))
 #define min(a,b) (((a) < (b)) ? (a) : (b))
 
-#define false 0
-#define true 1
+#define FALSE 0
+#define TRUE 1
 
 // Decide the sign of a value
 #define signof(a) (((a)>0) ? 1 : (((a)<0) ? -1 : 0))
diff --git a/stepper.c b/stepper.c
index 23eecba..0b829b3 100644
--- a/stepper.c
+++ b/stepper.c
@@ -32,7 +32,6 @@
 
 #include "wiring_serial.h"
 
-#define TICKS_PER_MICROSECOND (F_CPU/1000000)
 #define LINE_BUFFER_SIZE 10
 
 struct Line {
diff --git a/stepper.h b/stepper.h
index 283dca8..e371d7f 100644
--- a/stepper.h
+++ b/stepper.h
@@ -24,11 +24,6 @@
 #include <avr/io.h>
 #include <avr/sleep.h>
 
-#define STEPPER_MODE_STOPPED 0
-#define STEPPER_MODE_RUNNING 1
-#define STEPPER_MODE_LIMIT_OVERRUN 2
-#define STEPPER_MODE_HOMING 3
-
 // Initialize and start the stepper motor subsystem
 void st_init();