Implement #29

5 files changed, 358 insertions, 158 deletions

diff --git a/ArcWelder/arc_welder.cpp b/ArcWelder/arc_welder.cpp
index 710c3cd..ecfcf38 100644
--- a/ArcWelder/arc_welder.cpp
+++ b/ArcWelder/arc_welder.cpp
@@ -386,7 +386,6 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end, bool is_reprocess
 	position* p_pre_pos = p_source_position_->get_previous_position_ptr();
 	extruder extruder_current = p_cur_pos->get_current_extruder();
 	extruder previous_extruder = p_pre_pos->get_current_extruder();
-	point p(p_cur_pos->get_gcode_x(), p_cur_pos->get_gcode_y(), p_cur_pos->get_gcode_z(), extruder_current.e_relative);
 	//std::cout << lines_processed_ << " - " << cmd.gcode << ", CurrentEAbsolute: " << cur_extruder.e <<", ExtrusionLength: " << cur_extruder.extrusion_length << ", Retraction Length: " << cur_extruder.retraction_length << ", IsExtruding: " << cur_extruder.is_extruding << ", IsRetracting: " << cur_extruder.is_retracting << ".\n";
 
 	int lines_written = 0;
@@ -394,12 +393,11 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end, bool is_reprocess
 	
 	bool arc_added = false;
 	bool clear_shapes = false;
-	
+	double movement_length_mm = 0;
+	bool has_e_changed = extruder_current.is_extruding || extruder_current.is_retracting;
 	// Update the source file statistics
-	if (p_cur_pos->has_xy_position_changed && (extruder_current.is_extruding || extruder_current.is_retracting) && !is_reprocess)
+	if (p_cur_pos->has_xy_position_changed && (has_e_changed))
 	{
-		double movement_length_mm;
-		
 		if (allow_3d_arcs_) {
 			movement_length_mm = utilities::get_cartesian_distance(p_pre_pos->x, p_pre_pos->y, p_pre_pos->z, p_cur_pos->x, p_cur_pos->y, p_cur_pos->z);
 		}
@@ -409,7 +407,8 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end, bool is_reprocess
 		
 		if (movement_length_mm > 0)
 		{
-			segment_statistics_.update(movement_length_mm, true);
+			if (!is_reprocess)
+				segment_statistics_.update(movement_length_mm, true);
 		}
 	}
 
@@ -449,7 +448,9 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end, bool is_reprocess
 			!p_cur_pos->is_relative &&
 			(
 				!waiting_for_arc_ ||
-				(previous_extruder.is_extruding && extruder_current.is_extruding) ||
+				extruder_current.is_extruding ||
+				//(previous_extruder.is_extruding && extruder_current.is_extruding) || // Test to see if 
+				// we can get more arcs.
 				(previous_extruder.is_retracting && extruder_current.is_retracting)
 			) &&
 			p_cur_pos->is_extruder_relative == p_pre_pos->is_extruder_relative &&
@@ -457,7 +458,8 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end, bool is_reprocess
 			(!waiting_for_arc_ || p_pre_pos->feature_type_tag == p_cur_pos->feature_type_tag)
 			)
 	) {
-		
+
+		printer_point p(p_cur_pos->get_gcode_x(), p_cur_pos->get_gcode_y(), p_cur_pos->get_gcode_z(), extruder_current.e_relative, movement_length_mm);
 		if (!waiting_for_arc_)
 		{
 			previous_is_extruder_relative_ = p_pre_pos->is_extruder_relative;
@@ -467,15 +469,16 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end, bool is_reprocess
 			}
 			write_unwritten_gcodes_to_file();
 			// add the previous point as the starting point for the current arc
-			point previous_p(p_pre_pos->get_gcode_x(), p_pre_pos->get_gcode_y(), p_pre_pos->get_gcode_z(), previous_extruder.e_relative);
+			printer_point previous_p(p_pre_pos->get_gcode_x(), p_pre_pos->get_gcode_y(), p_pre_pos->get_gcode_z(), previous_extruder.e_relative, 0);
 			// Don't add any extrusion, or you will over extrude!
 			//std::cout << "Trying to add first point (" << p.x << "," << p.y << "," << p.z << ")...";
-			current_arc_.try_add_point(previous_p, 0);
+			
+			current_arc_.try_add_point(previous_p);
 		}
 		
 		double e_relative = extruder_current.e_relative;
 		int num_points = current_arc_.get_num_segments();
-		arc_added = current_arc_.try_add_point(p, e_relative);
+		arc_added = current_arc_.try_add_point(p);
 		if (arc_added)
 		{
 			if (!waiting_for_arc_)
@@ -694,29 +697,11 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end, bool is_reprocess
 		}
 
 	}
-
-
+	
 	if (waiting_for_arc_ || !arc_added)
 	{
 		position* cur_pos = p_source_position_->get_current_position_ptr();
-		extruder& cur_extruder = cur_pos->get_current_extruder();
-
-		double length = 0;
-		if (p_cur_pos->has_xy_position_changed && (cur_extruder.is_extruding || cur_extruder.is_retracting))
-		{
-			position* prev_pos = p_source_position_->get_previous_position_ptr();
-			length = 0;
-			if (allow_3d_arcs_)
-			{
-				length = utilities::get_cartesian_distance(cur_pos->x, cur_pos->y, cur_pos->z, prev_pos->x, prev_pos->y, prev_pos->z);
-			}
-			else {
-				length = utilities::get_cartesian_distance(cur_pos->x, cur_pos->y, prev_pos->x, prev_pos->y);
-			}
-			
-		}
-		
-		unwritten_commands_.push_back(unwritten_command(cur_pos, length));
+		unwritten_commands_.push_back(unwritten_command(cur_pos, movement_length_mm));
 		
 	}
 	if (!waiting_for_arc_)
diff --git a/ArcWelder/segmented_arc.cpp b/ArcWelder/segmented_arc.cpp
index fe212ff..4da8f8e 100644
--- a/ArcWelder/segmented_arc.cpp
+++ b/ArcWelder/segmented_arc.cpp
@@ -76,7 +76,7 @@ segmented_arc::~segmented_arc()
 {
 }
 
-point segmented_arc::pop_front(double e_relative)
+printer_point segmented_arc::pop_front(double e_relative)
 {
   e_relative_ -= e_relative;
   if (points_.count() == get_min_segments())
@@ -85,7 +85,7 @@ point segmented_arc::pop_front(double e_relative)
   }
   return points_.pop_front();
 }
-point segmented_arc::pop_back(double e_relative)
+printer_point segmented_arc::pop_back(double e_relative)
 {
   e_relative_ -= e_relative;
   return points_.pop_back();
@@ -118,8 +118,11 @@ bool segmented_arc::is_shape() const
 {
   return is_shape_;
 }
-
-bool segmented_arc::try_add_point(point p, double e_relative)
+double segmented_arc::get_shape_length()
+{
+  return current_arc_.length;
+}
+bool segmented_arc::try_add_point(printer_point p)
 {
 
   bool point_added = false;
@@ -129,42 +132,33 @@ bool segmented_arc::try_add_point(point p, double e_relative)
     // Too many points, we can't add more
     return false;
   }
-  double distance = 0;
   if (points_.count() > 0)
   {
-    point p1 = points_[points_.count() - 1];
-    if (allow_3d_arcs_) {
-      // If we can draw arcs in 3 space, add in the distance of the z axis changes
-      distance = utilities::get_cartesian_distance(p1.x, p1.y, p1.z, p.x, p.y, p.z);
-    }
-    else {
-      distance = utilities::get_cartesian_distance(p1.x, p1.y, p.x, p.y);
-      if (!utilities::is_equal(p1.z, p.z))
-      {
-        // Z axis changes aren't allowed
-        return false;
-      }
+    printer_point p1 = points_[points_.count() - 1];
+    if (!allow_3d_arcs_ && !utilities::is_equal(p1.z, p.z))
+    {
+      // Z axis changes aren't allowed
+      return false;
     }
 
-    if (utilities::is_zero(distance))
+    if (utilities::is_zero(p.distance))
     {
       // there must be some distance between the points
       // to make an arc.
       return false;
     }
-
   }
 
   if (points_.count() < get_min_segments() - 1)
   {
     point_added = true;
     points_.push_back(p);
-    original_shape_length_ += distance;
+    original_shape_length_ += p.distance;
   }
   else
   {
     // if we're here, we need to see if the new point can be included in the shape
-    point_added = try_add_point_internal_(p, distance);
+    point_added = try_add_point_internal_(p);
   }
   if (point_added)
   {
@@ -172,7 +166,7 @@ bool segmented_arc::try_add_point(point p, double e_relative)
     if (points_.count() > 1)
     {
       // Only add the relative distance to the second point on up.
-      e_relative_ += e_relative;
+      e_relative_ += p.e_relative;
     }
     //std::cout << " success - " << points_.count() << " points.\n";
   }
@@ -180,27 +174,20 @@ bool segmented_arc::try_add_point(point p, double e_relative)
   {
     // If we haven't added a point, and we have exactly min_segments_,
     // pull off the initial arc point and try again
-
-    point old_initial_point = points_.pop_front();
-    // We have to remove the distance and e relative value
-    // accumulated between the old arc start point and the new
-    point new_initial_point = points_[0];
-    if (allow_3d_arcs_) {
-      original_shape_length_ -= utilities::get_cartesian_distance(old_initial_point.x, old_initial_point.y, old_initial_point.z, new_initial_point.x, new_initial_point.y, new_initial_point.z);
-    }
-    else {
-      original_shape_length_ -= utilities::get_cartesian_distance(old_initial_point.x, old_initial_point.y, new_initial_point.x, new_initial_point.y);
-    }
-
+    points_.pop_front();
+    // Get the new initial point
+    printer_point new_initial_point = points_[0];
+    // The length and e_relative distance of the arc has been reduced 
+    // by removing the front point.  Calculate this.
+    original_shape_length_ -= new_initial_point.distance;
     e_relative_ -= new_initial_point.e_relative;
-    //std::cout << " failed - removing start point and retrying current point.\n";
-    return try_add_point(p, e_relative);
+    return try_add_point(p);
   }
 
   return point_added;
 }
 
-bool segmented_arc::try_add_point_internal_(point p, double pd)
+bool segmented_arc::try_add_point_internal_(printer_point p)
 {
   // If we don't have enough points (at least min_segments) return false
   if (points_.count() < get_min_segments() - 1)
@@ -212,14 +199,15 @@ bool segmented_arc::try_add_point_internal_(point p, double pd)
   // the circle is new..  we have to test it now, which is expensive :(
   points_.push_back(p);
   double previous_shape_length = original_shape_length_;
-  original_shape_length_ += pd;
+  original_shape_length_ += p.distance;
   arc original_arc = current_arc_;
   if (arc::try_create_arc(points_, current_arc_, original_shape_length_, max_radius_mm_, resolution_mm_, path_tolerance_percent_, min_arc_segments_, mm_per_arc_segment_, get_xyz_tolerance(), allow_3d_arcs_))
   {
-    // See how many arcs will be interpolated
     bool abort_arc = false;
     if (min_arc_segments_ > 0 && mm_per_arc_segment_ > 0)
     {
+      // Apply firmware compensation
+      // See how many arcs will be interpolated
       double circumference = 2.0 * PI_DOUBLE * current_arc_.radius;
       int num_segments = (int)std::floor(circumference / min_arc_segments_);
       if (num_segments < min_arc_segments_) {
@@ -231,15 +219,20 @@ bool segmented_arc::try_add_point_internal_(point p, double pd)
         }
       }
     }
-    // check for I=0 and J=0
-    if (!abort_arc && utilities::is_zero(current_arc_.get_i(), get_xyz_tolerance()) && utilities::is_zero(current_arc_.get_j(), get_xyz_tolerance()))
-    {
-      abort_arc = true;
-    }
-    if (!abort_arc && current_arc_.length < get_xyz_tolerance())
+    if (!abort_arc)
     {
-      abort_arc = true;
+      if (utilities::is_zero(current_arc_.get_i(), get_xyz_tolerance()) && utilities::is_zero(current_arc_.get_j(), get_xyz_tolerance()))
+      {
+        // I and J are both 0, which is invalid!  Abort!
+        abort_arc = true;
+      }
+      else if (current_arc_.length < get_xyz_tolerance())
+      {
+        // the arc length is below our tolerance, abort!
+        abort_arc = true;
+      }
     }
+    
     if (abort_arc)
     {
       // This arc has been cancelled either due to firmware correction,
diff --git a/ArcWelder/segmented_arc.h b/ArcWelder/segmented_arc.h
index 52833b3..c67c071 100644
--- a/ArcWelder/segmented_arc.h
+++ b/ArcWelder/segmented_arc.h
@@ -45,20 +45,21 @@ public:
 		unsigned char default_e_precision = DEFAULT_E_PRECISION
 	);
 	virtual ~segmented_arc();
-	virtual bool try_add_point(point p, double e_relative);
+	virtual bool try_add_point(printer_point p);
+	virtual double get_shape_length();
 	std::string get_shape_gcode_absolute(double e, double f);
 	std::string get_shape_gcode_relative(double f);
 	
 	virtual bool is_shape() const;
-	point pop_front(double e_relative);
-	point pop_back(double e_relative);
+	printer_point pop_front(double e_relative);
+	printer_point pop_back(double e_relative);
 	double get_max_radius() const;
 	int get_min_arc_segments() const;
 	double get_mm_per_arc_segment() const;
 	int get_num_firmware_compensations() const;
 
 private:
-	bool try_add_point_internal_(point p, double pd);
+	bool try_add_point_internal_(printer_point p);
 	std::string get_shape_gcode_(bool has_e, double e, double f) const;
 	arc current_arc_;
 	double max_radius_mm_;
diff --git a/ArcWelder/segmented_shape.cpp b/ArcWelder/segmented_shape.cpp
index 217154d..83142e2 100644
--- a/ArcWelder/segmented_shape.cpp
+++ b/ArcWelder/segmented_shape.cpp
@@ -29,12 +29,12 @@
 #include <cmath>
 #include <iostream>
 #pragma region Operators for Vector and Point
+
 point operator +(point lhs, const vector rhs) {
   point p(
     lhs.x + rhs.x,
     lhs.y + rhs.y,
-    lhs.z + rhs.z,
-    lhs.e_relative + rhs.e_relative
+    lhs.z + rhs.z
   );
   return p;
 }
@@ -43,11 +43,9 @@ point operator -(point lhs, const vector rhs) {
   return point(
     lhs.x - rhs.x,
     lhs.y - rhs.y,
-    lhs.z - rhs.z,
-    lhs.e_relative - rhs.e_relative
+    lhs.z - rhs.z
   );
-}
-
+} 
 vector operator -(point& lhs, point& rhs) {
   return vector(
     lhs.x - rhs.x,
@@ -80,7 +78,7 @@ point point::get_midpoint(point p1, point p2)
   double y = (p1.y + p2.y) / 2.0;
   double z = (p1.z + p2.z) / 2.0;
 
-  return point(x, y, z, 0);
+  return point(x, y, z);
 }
 #pragma endregion Point Functions
 
@@ -176,8 +174,6 @@ bool circle::try_create_circle(const point& p1, const point& p2, const point& p3
   {
     return false;
   }
-
-
   double b = (x1 * x1 + y1 * y1) * (y3 - y2)
     + (x2 * x2 + y2 * y2) * (y1 - y3)
     + (x3 * x3 + y3 * y3) * (y2 - y1);
@@ -192,6 +188,7 @@ bool circle::try_create_circle(const point& p1, const point& p2, const point& p3
   double radius = utilities::get_cartesian_distance(x, y, x1, y1);
   if (radius > max_radius)
     return false;
+
   new_circle.center.x = x;
   new_circle.center.y = y;
   new_circle.center.z = p1.z;
@@ -199,8 +196,8 @@ bool circle::try_create_circle(const point& p1, const point& p2, const point& p3
 
   return true;
 }
-
-bool circle::try_create_circle(const array_list<point>& points, const double max_radius, const double resolution_mm, const double xyz_tolerance, bool allow_3d_arcs, bool check_middle_only, circle& new_circle)
+/*
+bool circle::try_create_circle(const array_list<printer_point>& points, const double max_radius, const double resolution_mm, const double xyz_tolerance, bool allow_3d_arcs, bool check_middle_only, circle& new_circle)
 {
   int middle_index = points.count() / 2;
   int check_index;
@@ -245,12 +242,53 @@ bool circle::try_create_circle(const array_list<point>& points, const double max
   }
   return false;
 }
-
-double circle::get_radians(const point& p1, const point& p2) const
+*/
+bool circle::try_create_circle(const array_list<printer_point>& points, const double max_radius, const double resolution_mm, const double xyz_tolerance, bool allow_3d_arcs, bool check_middle_only, circle& new_circle)
 {
-  double distance_sq = std::pow(utilities::get_cartesian_distance(p1.x, p1.y, p2.x, p2.y), 2.0);
-  double two_r_sq = 2.0 * radius * radius;
-  return acos((two_r_sq - distance_sq) / two_r_sq);
+  int count = points.count();
+  
+  int middle_index = count / 2;
+  // The middle point will almost always produce the best arcs.
+  if (circle::try_create_circle(points[0], points[middle_index], points[count - 1], max_radius, new_circle) && !new_circle.is_over_deviation(points, resolution_mm, xyz_tolerance, allow_3d_arcs))
+  {
+    return true;
+  }
+  
+  if (check_middle_only || count == 3)
+  {
+    // If we are only checking the middle, or if we only have 3 points return
+    return false;
+  }
+  
+  // Find the circle with the least deviation, if one exists.
+  // Note, this could possibly take a LONG time in the worst case, but it's a pretty unlikely.
+  // However, if the midpoint check doesn't pass, it's worth it to spend a bit more time 
+  // finding the best fit for the circle (least squares deviation) 
+  circle test_circle;
+  double least_deviation;
+  bool found_circle=false;
+  for (int index = 1; index < count - 1; index++)
+  {
+    
+    if (index == middle_index)
+    {
+      // We already checked this one, and it failed, continue.
+      continue;
+    }
+    
+    double current_deviation;
+    if (circle::try_create_circle(points[0], points[index], points[count - 1], max_radius, test_circle) && test_circle.get_deviation_sum_squared(points, resolution_mm, xyz_tolerance, allow_3d_arcs, current_deviation))
+    {
+      
+      if (!found_circle || current_deviation < least_deviation)
+      {
+        found_circle = true;
+        least_deviation = current_deviation;
+        new_circle = test_circle;
+      }
+    }
+  }
+  return found_circle;
 }
 
 double circle::get_polar_radians(const point& p1) const
@@ -261,50 +299,86 @@ double circle::get_polar_radians(const point& p1) const
   return polar_radians;
 }
 
-point circle::get_closest_point(const point& p) const
+bool circle::get_deviation_sum_squared(const array_list<printer_point>& points, const double resolution_mm, const double xyz_tolerance, const bool allow_3d_arcs, double &total_deviation)
 {
-  vector v = p - center;
-  double mag = v.get_magnitude();
-  double px = center.x + v.x / mag * radius;
-  double py = center.y + v.y / mag * radius;
-  double pz = center.z + v.z / mag * radius;
-  return point(px, py, pz, 0);
+  // We need to ensure that the Z steps are constand per linear travel unit
+  double z_step_per_distance = 0;
+  total_deviation = 0;
+  // Skip the first and last points since they will fit perfectly.
+  for (int index = 1; index < points.count() - 1; index++)
+  {
+    // Make sure the length from the center of our circle to the test point is 
+    // at or below our max distance.
+    double distance_from_center = utilities::get_cartesian_distance(points[index].x, points[index].y, center.x, center.y);
+    if (allow_3d_arcs) {
+      double z1 = points[index - 1].z;
+      double z2 = points[index].z;
+
+      double current_z_stepper_distance = (z2 - z1) / distance_from_center;
+      if (index == 1) {
+        z_step_per_distance = current_z_stepper_distance;
+      }
+      else if (!utilities::is_equal(z_step_per_distance, current_z_stepper_distance, xyz_tolerance))
+      {
+        // The z step is uneven, can't create arc				
+        return false;
+      }
+    }
+    double deviation = std::fabs(distance_from_center - radius);
+    total_deviation += deviation * deviation;
+    if (deviation > resolution_mm)
+    {
+      // Too much deviation
+      return false;
+    }
+  }
+  // Check the point perpendicular from the segment to the circle's center, if any such point exists
+  for (int index = 0; index < points.count() - 1; index++)
+  {
+    point point_to_test;
+    if (segment::get_closest_perpendicular_point(points[index], points[index + 1], center, point_to_test))
+    {
+      double distance = utilities::get_cartesian_distance(point_to_test.x, point_to_test.y, center.x, center.y);
+      double deviation = std::fabs(distance - radius);
+      total_deviation += deviation * deviation;
+      if (deviation > resolution_mm)
+      {
+        return false;
+      }
+    }
+  }
+  return true;
 }
 
-bool circle::is_over_deviation(const array_list<point>& points, const double resolution_mm, const double xyz_tolerance, const bool allow_3d_arcs)
+bool circle::is_over_deviation(const array_list<printer_point>& points, const double resolution_mm, const double xyz_tolerance, const bool allow_3d_arcs)
 {
   // We need to ensure that the Z steps are constand per linear travel unit
   double z_step_per_distance = 0;
   // Skip the first and last points since they will fit perfectly.
-  // UNLESS allow z changes is set to true, then we need to do some different stuff
-  int final_index = points.count() - 1 + (allow_3d_arcs ? 1 : 0);
   for (int index = 1; index < points.count() - 1; index++)
   {
     // Make sure the length from the center of our circle to the test point is 
     // at or below our max distance.
-    double distance = distance = utilities::get_cartesian_distance(points[index].x, points[index].y, center.x, center.y);
+    double distance_from_center = utilities::get_cartesian_distance(points[index].x, points[index].y, center.x, center.y);
     if (allow_3d_arcs) {
       double z1 = points[index - 1].z;
       double z2 = points[index].z;
 
-      double current_z_stepper_distance = (z2 - z1) / distance;
-      if (z_step_per_distance == 0) {
+      double current_z_stepper_distance = (z2 - z1) / distance_from_center;
+      if (index == 1) {
         z_step_per_distance = current_z_stepper_distance;
       }
-      if (!utilities::is_equal(z_step_per_distance, current_z_stepper_distance, xyz_tolerance))
+      else if (!utilities::is_equal(z_step_per_distance, current_z_stepper_distance, xyz_tolerance))
       {
         // The z step is uneven, can't create arc				
         return true;
       }
-
     }
-
-    if (std::abs(distance - radius) > resolution_mm)
+    if (std::fabs(distance_from_center - radius) > resolution_mm)
     {
       return true;
     }
   }
-
   // Check the point perpendicular from the segment to the circle's center, if any such point exists
   for (int index = 0; index < points.count() - 1; index++)
   {
@@ -312,19 +386,17 @@ bool circle::is_over_deviation(const array_list<point>& points, const double res
     if (segment::get_closest_perpendicular_point(points[index], points[index + 1], center, point_to_test))
     {
       double distance = utilities::get_cartesian_distance(point_to_test.x, point_to_test.y, center.x, center.y);
-      if (std::abs(distance - radius) > resolution_mm)
+      if (std::fabs(distance - radius) > resolution_mm)
       {
         return true;
       }
     }
-
   }
   return false;
 }
 #pragma endregion Circle Functions
 
 #pragma region Arc Functions
-
 double arc::get_i() const
 {
   return center.x - start_point.x;
@@ -352,12 +424,12 @@ bool arc::try_create_arc(
 
   // variable to hold radians
   double angle_radians = 0;
-  int direction = 0;  // 1 = counter clockwise, 2 = clockwise, 3 = unknown.
+  DirectionEnum direction = DirectionEnum::UNKNOWN;  // 1 = counter clockwise, 2 = clockwise, 3 = unknown.
   // Determine the direction of the arc
   if (polar_end_theta > polar_start_theta)
   {
     if (polar_start_theta < polar_mid_theta && polar_mid_theta < polar_end_theta) {
-      direction = 1;
+      direction = DirectionEnum::COUNTERCLOCKWISE;
       angle_radians = polar_end_theta - polar_start_theta;
     }
     else if (
@@ -365,7 +437,7 @@ bool arc::try_create_arc(
       (polar_end_theta < polar_mid_theta && polar_mid_theta < (2.0 * PI_DOUBLE))
       )
     {
-      direction = 2;
+      direction = DirectionEnum::CLOCKWISE;
       angle_radians = polar_start_theta + ((2.0 * PI_DOUBLE) - polar_end_theta);
     }
   }
@@ -376,12 +448,12 @@ bool arc::try_create_arc(
       (0.0 < polar_mid_theta && polar_mid_theta < polar_end_theta)
       )
     {
-      direction = 1;
+      direction = DirectionEnum::COUNTERCLOCKWISE;
       angle_radians = polar_end_theta + ((2.0 * PI_DOUBLE) - polar_start_theta);
     }
     else if (polar_end_theta < polar_mid_theta && polar_mid_theta < polar_start_theta)
     {
-      direction = 2;
+      direction = DirectionEnum::CLOCKWISE;
       angle_radians = polar_start_theta - polar_end_theta;
     }
   }
@@ -414,7 +486,7 @@ bool arc::try_create_arc(
     // see if an arc moving in the opposite direction had the correct length.
 
     // Find the rest of the angle across the circle
-    double test_radians = std::abs(angle_radians - 2 * PI_DOUBLE);
+    double test_radians = std::fabs(angle_radians - 2 * PI_DOUBLE);
     // Calculate the length of that arc
     double test_arc_length = c.radius * test_radians;
     if (allow_3d_arcs)
@@ -432,7 +504,7 @@ bool arc::try_create_arc(
     }
     // So, let's set the new length and flip the direction (but not the angle)!
     arc_length = test_arc_length;
-    direction = direction == 1 ? 2 : 1;
+    direction = direction == DirectionEnum::COUNTERCLOCKWISE ? DirectionEnum::CLOCKWISE : DirectionEnum::COUNTERCLOCKWISE;
   }
 
   if (allow_3d_arcs)
@@ -448,7 +520,7 @@ bool arc::try_create_arc(
   if (direction == 2) {
     angle_radians *= -1.0;
   }
-
+  target_arc.direction = direction;
   target_arc.center.x = c.center.x;
   target_arc.center.y = c.center.y;
   target_arc.center.z = c.center.z;
@@ -463,9 +535,9 @@ bool arc::try_create_arc(
   return true;
 
 }
-
+/*
 bool arc::try_create_arc(
-  const array_list<point>& points,
+  const array_list<printer_point>& points,
   arc& target_arc,
   double approximate_length,
   double max_radius_mm,
@@ -486,6 +558,144 @@ bool arc::try_create_arc(
   }
   return false;
 }
+*/
+bool arc::try_create_arc(
+  const array_list<printer_point>& points,
+  arc& target_arc,
+  double approximate_length,
+  double max_radius_mm,
+  double resolution_mm,
+  double path_tolerance_percent,
+  int min_arc_segments,
+  double mm_per_arc_segment,
+  double xyz_tolerance,
+  bool allow_3d_arcs)
+{
+  circle test_circle;
+  if (!circle::try_create_circle(points, max_radius_mm, resolution_mm, xyz_tolerance, allow_3d_arcs, false, test_circle))
+  {
+    return false;
+  }
+
+  // We could save a bit of processing power and do our firmware compensation here, but we won't be able to track statistics for this easily.
+  // moved check to segmented_arc.cpp
+  int mid_point_index = ((points.count() - 2) / 2) + 1;
+  arc test_arc;
+  if (!arc::try_create_arc(test_circle, points[0], points[mid_point_index], points[points.count() - 1], test_arc, approximate_length, resolution_mm, path_tolerance_percent, allow_3d_arcs))
+  {
+    return false;
+  }
+
+  if (arc::are_points_within_slice(test_arc, points))
+  {
+    target_arc = test_arc;
+    return true;
+  }
+  return false;
+}
+
+bool arc::are_points_within_slice(const arc& test_arc, const array_list<printer_point>& points)
+{
+  
+  // Loop through the points and see if they fit inside of the angles
+  double previous_polar = test_arc.polar_start_theta;
+  bool crossed_zero = false;
+
+  point start_norm((test_arc.start_point.x - test_arc.center.x) / test_arc.radius, (test_arc.start_point.y - test_arc.center.y) / test_arc.radius, 0.0);
+  point end_norm((test_arc.end_point.x - test_arc.center.x) / test_arc.radius, (test_arc.end_point.y - test_arc.center.y) / test_arc.radius, 0.0);
+  
+  for (int index = 1; index < points.count(); index++)
+  {
+    double polar_test;
+    if (index < points.count() - 1)
+    {
+      polar_test = test_arc.get_polar_radians(points[index]);
+    }
+    else
+    {
+      polar_test = test_arc.polar_end_theta;
+    }
+
+    // First ensure the test point is within the arc
+    if (test_arc.direction == DirectionEnum::COUNTERCLOCKWISE)
+    {
+      // Only check to see if we are within the arc if this isn't the endpoint
+      if (index < points.count() - 1)
+      {
+        // First test to see if this point lies within the arc
+        if (test_arc.polar_start_theta < test_arc.polar_end_theta && !(test_arc.polar_start_theta < polar_test && polar_test < test_arc.polar_end_theta))
+        {
+          return false;
+        }
+        else if (test_arc.polar_start_theta > test_arc.polar_end_theta && !(polar_test > test_arc.polar_start_theta || polar_test < test_arc.polar_end_theta))
+        {
+          return false;
+        }
+      }
+      // Now make sure the angles are increasing
+      if (previous_polar > polar_test)
+      {
+        // Allow the angle to cross zero once
+        if (crossed_zero)
+        {
+          return false;
+        }
+        crossed_zero = true;
+      }
+    }
+    else 
+    {
+      if (index < points.count() - 1)
+      {
+        if (test_arc.polar_start_theta > test_arc.polar_end_theta && !(test_arc.polar_start_theta > polar_test && polar_test > test_arc.polar_end_theta))
+        {
+          return false;
+        }
+        else if (test_arc.polar_start_theta < test_arc.polar_end_theta && !(polar_test < test_arc.polar_start_theta || polar_test > test_arc.polar_end_theta))
+        {
+          return false;
+        }
+      }
+
+      // Now make sure the angles are decreasing
+      if (previous_polar < polar_test)
+      {
+        // Allow the angle to cross zero once
+        if (crossed_zero)
+        {
+          return false;
+        }
+        crossed_zero = true;
+      }
+    }
+
+    // Now see if the segment intersects either of the vector from the center of the circle to the endpoints of the arc
+    if ((index != 1 && ray_intersects_segment(test_arc.center, start_norm, points[index-1], points[index])) || (index != points.count()-1 && ray_intersects_segment(test_arc.center, end_norm, points[index-1], points[index])))
+      return false;
+    previous_polar = polar_test;
+  }
+  return true;
+}
+
+// return the distance of ray origin to intersection point
+bool arc::ray_intersects_segment(const point rayOrigin, const point rayDirection, const printer_point point1, const printer_point point2)
+{
+  vector v1 = rayOrigin - point1;
+  vector v2 = point2 - point1;
+  vector v3 = vector(-rayDirection.y, rayDirection.x, 0);
+
+  float dot = dot(v2, v3);
+  if (std::fabs(dot) < 0.000001)
+    return false;
+
+  float t1 = vector::cross_product_magnitude(v2, v1) / dot;
+  float t2 = dot(v1,v3) / dot;
+
+  if (t1 >= 0.0 && (t2 >= 0.0 && t2 <= 1.0))
+    return true;
+
+  return false;
+}
 
 #pragma endregion
 
@@ -641,16 +851,16 @@ void segmented_shape::set_resolution_mm(double resolution_mm)
   resolution_mm_ = resolution_mm;
 
 }
-point segmented_shape::pop_front()
+printer_point segmented_shape::pop_front()
 {
   return points_.pop_front();
 }
-point segmented_shape::pop_back()
+printer_point segmented_shape::pop_back()
 {
   return points_.pop_back();
 }
 
-bool segmented_shape::try_add_point(point p, double e_relative)
+bool segmented_shape::try_add_point(printer_point p, double e_relative)
 {
   throw std::exception();
 }
diff --git a/ArcWelder/segmented_shape.h b/ArcWelder/segmented_shape.h
index 1070625..6489707 100644
--- a/ArcWelder/segmented_shape.h
+++ b/ArcWelder/segmented_shape.h
@@ -36,19 +36,27 @@
 #define DEFAULT_XYZ_TOLERANCE 0.001
 #define DEFAULT_E_PRECISION 5
 #define ARC_LENGTH_PERCENT_TOLERANCE_DEFAULT 0.05  // one percent
+
 struct point
 {
 public:
-	point() :x(0), y(0), z(0), e_relative(0){}
-	point(double x, double y, double z, double e_relative) 
-	  : x(x), y(y), z(z), e_relative(e_relative){}
+	point() : x(0), y(0), z(0) {};
+	point(double x, double y, double z) : x(x), y(y), z(z) {};
 	double x;
 	double y;
 	double z;
-	double e_relative;
 	static point get_midpoint(point p1, point p2);
 };
 
+struct printer_point : point
+{
+public:
+	printer_point() :point(0, 0, 0), e_relative(0), distance(0) {}
+	printer_point(double x, double y, double z, double e_relative, double distance) :point(x,y,z), e_relative(e_relative), distance(distance) {}
+	double e_relative;
+	double distance;
+};
+
 struct segment
 {
 	segment()
@@ -110,21 +118,22 @@ struct circle {
 
 	static bool try_create_circle(const point &p1, const point &p2, const point &p3, const double max_radius, circle& new_circle);
 	
-	static bool try_create_circle(const array_list<point>& points, const double max_radius, const double resolutino_mm, const double xyz_tolerance, bool allow_3d_arcs, bool check_middle_only, circle& new_circle);
-
-	double get_radians(const point& p1, const point& p2) const;
+	static bool try_create_circle(const array_list<printer_point>& points, const double max_radius, const double resolutino_mm, const double xyz_tolerance, bool allow_3d_arcs, bool check_middle_only, circle& new_circle);
 
 	double get_polar_radians(const point& p1) const;
 
 	point get_closest_point(const point& p) const;
 
-	bool is_over_deviation(const array_list<point>& points, const double resolution_mm, const double xyz_tolerance, const bool allow_3d_arcs);
+	bool is_over_deviation(const array_list<printer_point>& points, const double resolution_mm, const double xyz_tolerance, const bool allow_3d_arcs);
+	
+	bool get_deviation_sum_squared(const array_list<printer_point>& points, const double resolution_mm, const double xyz_tolerance, const bool allow_3d_arcs, double& sum_deviation);
 };
 
 #define DEFAULT_RESOLUTION_MM 0.05
 #define DEFAULT_ALLOW_3D_ARCS false
 #define DEFAULT_MIN_ARC_SEGMENTS 0
 #define DEFAULT_MM_PER_ARC_SEGMENT 0
+enum DirectionEnum { UNKNOWN = 0, COUNTERCLOCKWISE = 1, CLOCKWISE = 2};
 struct arc : circle
 {
 	arc() {
@@ -144,6 +153,7 @@ struct arc : circle
 		polar_start_theta = 0;
 		polar_end_theta = 0;
 		max_deviation = 0;
+		direction = DirectionEnum::UNKNOWN;
 	}
 	
 	bool is_arc;
@@ -154,23 +164,11 @@ struct arc : circle
 	double max_deviation;
 	point start_point;
 	point end_point;
-
+	DirectionEnum direction;
 	double get_i() const;
 	double get_j() const;
-	// Statis functions
-	static bool try_create_arc(
-		const circle& c, 
-		const point& start_point, 
-		const point& mid_point, 
-		const point& end_point, 
-		arc& target_arc, 
-		double approximate_length,
-		double resolution = DEFAULT_RESOLUTION_MM, 
-		double path_tolerance_percent = ARC_LENGTH_PERCENT_TOLERANCE_DEFAULT,
-		bool allow_3d_arcs = DEFAULT_ALLOW_3D_ARCS);
-
 	static bool try_create_arc(
-		const array_list<point>& points, 
+		const array_list<printer_point>& points, 
 		arc& target_arc, 
 		double approximate_length, 
 		double max_radius = DEFAULT_MAX_RADIUS_MM,
@@ -180,6 +178,19 @@ struct arc : circle
 		double mm_per_arc_segment = DEFAULT_MM_PER_ARC_SEGMENT,
 		double xyz_tolerance = DEFAULT_XYZ_TOLERANCE,
 		bool allow_3d_arcs = DEFAULT_ALLOW_3D_ARCS);
+	static bool are_points_within_slice(const arc& test_arc, const array_list<printer_point>& points);
+	static bool ray_intersects_segment(const point rayOrigin, const point rayDirection, const printer_point point1, const printer_point point2);
+	private:
+		static bool try_create_arc(
+			const circle& c,
+			const point& start_point,
+			const point& mid_point,
+			const point& end_point,
+			arc& target_arc,
+			double approximate_length,
+			double resolution = DEFAULT_RESOLUTION_MM,
+			double path_tolerance_percent = ARC_LENGTH_PERCENT_TOLERANCE_DEFAULT,
+			bool allow_3d_arcs = DEFAULT_ALLOW_3D_ARCS);
 };
 double distance_from_segment(segment s, point p);
 
@@ -204,7 +215,7 @@ public:
 	int get_max_segments();
 	double get_resolution_mm();
 	double get_path_tolerance_percent();
-	double get_shape_length();
+	virtual double get_shape_length();
 	double get_shape_e_relative();
 	void set_resolution_mm(double resolution_mm);
 	void reset_precision();
@@ -213,9 +224,9 @@ public:
 	virtual bool is_shape() const;
 	// public virtual functions
 	virtual void clear();
-	virtual point pop_front();
-	virtual point pop_back();
-	virtual bool try_add_point(point p, double e_relative);
+	virtual printer_point pop_front();
+	virtual printer_point pop_back();
+	virtual bool try_add_point(printer_point p, double e_relative);
 	virtual std::string get_shape_gcode_absolute(double e_abs_start);
 	virtual std::string get_shape_gcode_relative();
 	bool is_extruding();
@@ -223,7 +234,7 @@ public:
 	unsigned char get_e_precision() const;
 	double get_xyz_tolerance() const;
 protected:
-	array_list<point> points_;
+	array_list<printer_point> points_;
 	void set_is_shape(bool value);
 	double original_shape_length_;	
 	double e_relative_;