Resolves #22, #23, #24, #25 and #26.

16 files changed, 978 insertions, 146 deletions

diff --git a/ArcWelder/arc_welder.cpp b/ArcWelder/arc_welder.cpp
index d7b6120..710c3cd 100644
--- a/ArcWelder/arc_welder.cpp
+++ b/ArcWelder/arc_welder.cpp
@@ -203,8 +203,8 @@ arc_welder_results results;
 		<< ", g90_91_influences_extruder: " << (p_source_position_->get_g90_91_influences_extruder() ? "True" : "False")
 		<< ", allow_3d_arcs: " << (allow_3d_arcs_ ? "True" : "False")
 		<< ", allow_dynamic_precision: " << (allow_dynamic_precision_ ? "True" : "False")
-		<< ", default_xyz_precision: " << std::setprecision(0) << (current_arc_.get_xyz_precision())
-		<< ", default_e_precision: " << std::setprecision(0) << (current_arc_.get_e_precision());
+		<< ", default_xyz_precision: " << std::setprecision(0) << static_cast<double>(current_arc_.get_xyz_precision())
+		<< ", default_e_precision: " << std::setprecision(0) << static_cast<double>(current_arc_.get_e_precision());
 	p_logger_->log(logger_type_, INFO, stream.str());
 
 
diff --git a/ArcWelder/arc_welder.h b/ArcWelder/arc_welder.h
index 9e6d74a..4e86426 100644
--- a/ArcWelder/arc_welder.h
+++ b/ArcWelder/arc_welder.h
@@ -37,6 +37,8 @@
 #include "unwritten_command.h"
 #include "logger.h"
 #include <cmath>
+#include <iomanip>
+#include <sstream>
 
 #ifdef _MSC_VER
 #define _CRT_SECURE_NO_WARNINGS
diff --git a/ArcWelder/segmented_arc.cpp b/ArcWelder/segmented_arc.cpp
index 0618917..fe212ff 100644
--- a/ArcWelder/segmented_arc.cpp
+++ b/ArcWelder/segmented_arc.cpp
@@ -27,7 +27,8 @@
 #include "utilities.h"
 #include "segmented_shape.h"
 #include <iostream>
-#include <iomanip>
+//#include <iomanip>
+//#include <sstream>
 #include <stdio.h>
 #include <cmath>
 
@@ -213,7 +214,7 @@ bool segmented_arc::try_add_point_internal_(point p, double pd)
   double previous_shape_length = original_shape_length_;
   original_shape_length_ += pd;
   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_precision(), allow_3d_arcs_))
+  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;
@@ -235,7 +236,10 @@ bool segmented_arc::try_add_point_internal_(point p, double pd)
     {
       abort_arc = true;
     }
-
+    if (!abort_arc && current_arc_.length < get_xyz_tolerance())
+    {
+      abort_arc = true;
+    }
     if (abort_arc)
     {
       // This arc has been cancelled either due to firmware correction,
@@ -271,31 +275,94 @@ std::string segmented_arc::get_shape_gcode_relative(double f)
 
 std::string segmented_arc::get_shape_gcode_(bool has_e, double e, double f) const
 {
-  char buf[100];
   std::string gcode;
+  // Calculate gcode size
+  bool has_f = utilities::greater_than_or_equal(f, 1);
+  bool has_z = allow_3d_arcs_ && !utilities::is_equal(
+    current_arc_.start_point.z, current_arc_.end_point.z, get_xyz_tolerance()
+  );
+  gcode.reserve(96);
 
+  if (current_arc_.angle_radians < 0)
+  {
+    gcode += "G2";
+  }
+  else
+  {
+    gcode += "G3";
+
+  }
+  // Add X, Y, I and J
+  gcode += " X";
+  gcode += utilities::dtos(current_arc_.end_point.x, get_xyz_precision());
+  
+  gcode += " Y";
+  gcode += utilities::dtos(current_arc_.end_point.y, get_xyz_precision());
+  
+  if (has_z)
+  {
+    gcode += " Z";
+    gcode += utilities::dtos(current_arc_.end_point.z, get_xyz_precision());
+  }
 
+  // Output I and J, but do NOT check for 0.  
+  // Simplify 3d has issues visualizing G2/G3 with 0 for I or J
+  // and until it is fixed, it is not worth the hassle.
   double i = current_arc_.get_i();
+  gcode += " I";
+  gcode += utilities::dtos(i, get_xyz_precision());
+
   double j = current_arc_.get_j();
-  // Here is where the performance part kicks in (these are expensive calls) that makes things a bit ugly.
-  // there are a few cases we need to take into consideration before choosing our sprintf string
-  // create the XYZ portion
+  gcode += " J";
+  gcode += utilities::dtos(j, get_xyz_precision());
+
+  // Add E if it appears
+  if (has_e)
+  {
+    gcode += " E";
+    gcode += utilities::dtos(e, get_e_precision());
+  }
+
+  // Add F if it appears
+  if (has_f)
+  {
+    gcode += " F";
+    gcode += utilities::dtos(f, 0);
+  }
+
+  return gcode;
+
+}
+
+/*
+* This is an older implementation using ostringstream.  It is substantially slower.
+* Keep this around in case there are problems with the custom dtos function
+std::string segmented_arc::get_shape_gcode_(bool has_e, double e, double f) const
+{
+  static std::ostringstream gcode;
+  gcode.str("");
+  gcode << std::fixed;
 
   if (current_arc_.angle_radians < 0)
   {
-    gcode = "G2";
+    gcode << "G2";
   }
   else
   {
-    gcode = "G3";
+    gcode << "G3";
 
   }
+  gcode << std::setprecision(get_xyz_precision());
   // Add X, Y, I and J
-  gcode += " X";
-  gcode += utilities::to_string(current_arc_.end_point.x, get_xyz_precision(), buf, false);
+  if (!utilities::is_zero(current_arc_.end_point.x, get_xyz_precision()))
+  {
+    gcode << " X" << current_arc_.end_point.x;
+  }
 
-  gcode += " Y";
-  gcode += utilities::to_string(current_arc_.end_point.y, get_xyz_precision(), buf, false);
+  if (!utilities::is_zero(current_arc_.end_point.y, get_xyz_precision()))
+  {
+    gcode << " Y" << current_arc_.end_point.y;
+  }
 
   if (allow_3d_arcs_)
   {
@@ -305,37 +372,34 @@ std::string segmented_arc::get_shape_gcode_(bool has_e, double e, double f) cons
     if (!utilities::is_equal(z_initial, z_final, get_xyz_tolerance()))
     {
       // The z axis has changed within the precision of the gcode coordinates
-      gcode += " Z";
-      gcode += utilities::to_string(current_arc_.end_point.z, get_xyz_precision(), buf, false);
+      gcode << " Z" << current_arc_.end_point.z;
     }
   }
 
-  if (!utilities::is_zero(i, get_xyz_tolerance()))
-  {
-    gcode += " I";
-    gcode += utilities::to_string(i, get_xyz_precision(), buf, false);
-  }
-  if (!utilities::is_zero(j, get_xyz_tolerance()))
-  {
-    gcode += " J";
-    gcode += utilities::to_string(j, get_xyz_precision(), buf, false);
-  }
+  // Output I and J, but do NOT check for 0.
+  // Simplify 3d has issues visualizing G2/G3 with 0 for I or J
+  // and until it is fixed, it is not worth the hassle.
+  double i = current_arc_.get_i();
+  gcode << " I" << i;
+
+  double j = current_arc_.get_j();
+  gcode << " J" << j;
 
   // Add E if it appears
   if (has_e)
   {
-    gcode += " E";
-    gcode += utilities::to_string(e, get_e_precision(), buf, false);
+    gcode << std::setprecision(get_e_precision());
+    gcode << " E" << e;
   }
 
   // Add F if it appears
   if (utilities::greater_than_or_equal(f, 1))
   {
-    gcode += " F";
-    gcode += utilities::to_string(f, 0, buf, true);
+    gcode << std::setprecision(0);
+    gcode << " F" << f;
   }
 
-  return gcode;
+  return gcode.str();
 
 }
-
+*/
\ No newline at end of file
diff --git a/ArcWelder/segmented_arc.h b/ArcWelder/segmented_arc.h
index 36d20da..52833b3 100644
--- a/ArcWelder/segmented_arc.h
+++ b/ArcWelder/segmented_arc.h
@@ -25,9 +25,6 @@
 
 #pragma once
 #include "segmented_shape.h"
-#include <iomanip>
-#include <sstream>
-
 #define GCODE_CHAR_BUFFER_SIZE 1000
 
 class segmented_arc :
@@ -63,9 +60,6 @@ public:
 private:
 	bool try_add_point_internal_(point p, double pd);
 	std::string get_shape_gcode_(bool has_e, double e, double f) const;
-	std::string get_g1(double x, double y, double z, double e, double f, bool has_z);
-	std::string interpolate_arc(double f, bool is_relative, double start_e = 0);
-	//circle arc_circle_;
 	arc current_arc_;
 	double max_radius_mm_;
 	int min_arc_segments_;
diff --git a/ArcWelder/segmented_shape.cpp b/ArcWelder/segmented_shape.cpp
index f505d59..217154d 100644
--- a/ArcWelder/segmented_shape.cpp
+++ b/ArcWelder/segmented_shape.cpp
@@ -94,7 +94,9 @@ bool segment::get_closest_perpendicular_point(const point& p1, const point& p2,
 {
   // [(Cx - Ax)(Bx - Ax) + (Cy - Ay)(By - Ay)] / [(Bx - Ax) ^ 2 + (By - Ay) ^ 2]
   double num = (c.x - p1.x) * (p2.x - p1.x) + (c.y - p1.y) * (p2.y - p1.y);
-  double denom = (std::pow((p2.x - p1.x), 2) + std::pow((p2.y - p1.y), 2));
+  double x_dif = p2.x - p1.x;
+  double y_dif = p2.y - p1.y;
+  double denom = (x_dif * x_dif) + (y_dif * y_dif);
   double t = num / denom;
 
   // We're considering this a failure if t == 0 or t==1 within our tolerance.  In that case we hit the endpoint, which is OK.
@@ -198,7 +200,7 @@ 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 int xyz_precision, bool allow_3d_arcs, bool check_middle_only, circle& new_circle)
+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)
 {
   int middle_index = points.count() / 2;
   int check_index;
@@ -210,7 +212,7 @@ bool circle::try_create_circle(const array_list<point>& points, const double max
     // Check the index
     if (circle::try_create_circle(points[0], points[check_index], points[points.count() - 1], max_radius, new_circle))
     {
-      if (!new_circle.is_over_deviation(points, resolution_mm, xyz_precision, allow_3d_arcs))
+      if (!new_circle.is_over_deviation(points, resolution_mm, xyz_tolerance, allow_3d_arcs))
       {
         return true;
       }
@@ -269,7 +271,7 @@ point circle::get_closest_point(const point& p) const
   return point(px, py, pz, 0);
 }
 
-bool circle::is_over_deviation(const array_list<point>& points, const double resolution_mm, const int xyz_precision, const bool allow_3d_arcs)
+bool circle::is_over_deviation(const array_list<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;
@@ -289,7 +291,7 @@ bool circle::is_over_deviation(const array_list<point>& points, const double res
       if (z_step_per_distance == 0) {
         z_step_per_distance = current_z_stepper_distance;
       }
-      if (!utilities::is_equal(z_step_per_distance, current_z_stepper_distance, std::pow(10.0, -1.0 * xyz_precision)))
+      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;
@@ -471,11 +473,11 @@ bool arc::try_create_arc(
   double path_tolerance_percent,
   int min_arc_segments,
   double mm_per_arc_segment,
-  int xyz_precision,
+  double xyz_tolerance,
   bool allow_3d_arcs)
 {
   circle test_circle;
-  if (circle::try_create_circle(points, max_radius_mm, resolution_mm, xyz_precision, allow_3d_arcs, false, test_circle))
+  if (circle::try_create_circle(points, max_radius_mm, resolution_mm, xyz_tolerance, allow_3d_arcs, false, test_circle))
   {
     // 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
diff --git a/ArcWelder/segmented_shape.h b/ArcWelder/segmented_shape.h
index 3b8e807..1070625 100644
--- a/ArcWelder/segmented_shape.h
+++ b/ArcWelder/segmented_shape.h
@@ -33,6 +33,7 @@
 // The minimum theta value allowed between any two arc in order for an arc to be
 // created.  This prevents sign calculation issues for very small values of theta
 #define DEFAULT_XYZ_PRECISION 3
+#define DEFAULT_XYZ_TOLERANCE 0.001
 #define DEFAULT_E_PRECISION 5
 #define ARC_LENGTH_PERCENT_TOLERANCE_DEFAULT 0.05  // one percent
 struct point
@@ -109,7 +110,7 @@ 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 int xyz_precision, bool allow_3d_arcs, bool check_middle_only, 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;
 
@@ -117,7 +118,7 @@ struct circle {
 
 	point get_closest_point(const point& p) const;
 
-	bool is_over_deviation(const array_list<point>& points, const double resolution_mm, const int xyz_precision, const bool allow_3d_arcs);
+	bool is_over_deviation(const array_list<point>& points, const double resolution_mm, const double xyz_tolerance, const bool allow_3d_arcs);
 };
 
 #define DEFAULT_RESOLUTION_MM 0.05
@@ -177,7 +178,7 @@ struct arc : circle
 		double path_tolerance_percent = ARC_LENGTH_PERCENT_TOLERANCE_DEFAULT,
 		int min_arc_segments = DEFAULT_MIN_ARC_SEGMENTS,
 		double mm_per_arc_segment = DEFAULT_MM_PER_ARC_SEGMENT,
-		int xyz_precision = DEFAULT_XYZ_PRECISION,
+		double xyz_tolerance = DEFAULT_XYZ_TOLERANCE,
 		bool allow_3d_arcs = DEFAULT_ALLOW_3D_ARCS);
 };
 double distance_from_segment(segment s, point p);
diff --git a/ArcWelderTest/ArcWelderTest.cpp b/ArcWelderTest/ArcWelderTest.cpp
index 0a11498..d631821 100644
--- a/ArcWelderTest/ArcWelderTest.cpp
+++ b/ArcWelderTest/ArcWelderTest.cpp
@@ -33,6 +33,7 @@ int main(int argc, char* argv[])
 
 int run_tests(int argc, char* argv[])
 {
+
 	_CrtMemState state;
 	// This line will take a snapshot
 	// of the memory allocated at this point.
@@ -51,6 +52,7 @@ int run_tests(int argc, char* argv[])
 	for (unsigned int index = 0; index < num_runs; index++)
 	{
 		std::cout << "Processing test run " << index + 1 << " of " << num_runs << ".\r\n";
+		
 		TestAntiStutter(ANTI_STUTTER_TEST);
 		//TestParsingCase();
 		//TestDoubleToString();
@@ -58,6 +60,37 @@ int run_tests(int argc, char* argv[])
 		//TestCircularBuffer();
 		//TestSegmentedLine();
 		//TestSegmentedArc();
+		/*
+		if (!TestProblemDoubles())
+		{
+			std::cout << "Test Failed!" << std::endl;
+		}
+
+		if (!TestIntToStringRandom(-1000000, 1000000, 1000000))
+		{
+			std::cout << "Test Failed!" << std::endl;
+		}
+
+		
+		if (!TestDoubleToStringRandom(-0.5, 0.5, 1000000))
+		{
+			std::cout << "Test Failed!" << std::endl;
+		}
+		if (!TestDoubleToStringRandom(-100, 100, 1000000))
+		{
+			std::cout << "Test Failed!" << std::endl;
+		}
+		if (!TestDoubleToStringRandom(-1, 1, 1000000))
+		{
+			std::cout << "Test Failed!" << std::endl;
+		}
+		if (!TestDoubleToStringRandom(-1000000, 1000000, 1000000))
+		{
+			std::cout << "Test Failed!" << std::endl;
+		}		
+		*/
+	
+		 
 
 	}
 	auto end = std::chrono::high_resolution_clock::now();
@@ -259,7 +292,7 @@ static void TestAntiStutter(std::string filePath)
 	//arc_welder arc_welder_obj(BENCHY_0_5_MM_NO_WIPE, "C:\\Users\\Brad\\Documents\\3DPrinter\\AntiStutter\\test_output.gcode", p_logger, max_resolution, false, 50, static_cast<progress_callback>(on_progress));
 	//arc_welder arc_welder_obj(SIX_SPEED_TEST, "C:\\Users\\Brad\\Documents\\3DPrinter\\AntiStutter\\test_output.gcode", p_logger, max_resolution, false, 50, on_progress);
 	arc_welder arc_welder_obj(
-		ISSUE_93,
+		BARBARIAN,
 		"C:\\Users\\Brad\\Documents\\3DPrinter\\AntiStutter\\test_output.gcode", 
 		p_logger, 
 		max_resolution, 
@@ -268,7 +301,7 @@ static void TestAntiStutter(std::string filePath)
 		min_arc_segments,
 		mm_per_arc_segment,
 		false,
-		true,
+		false,
 		DEFAULT_ALLOW_DYNAMIC_PRECISION,
 		DEFAULT_XYZ_PRECISION,
 		DEFAULT_E_PRECISION,
@@ -309,21 +342,6 @@ bool on_progress(arc_welder_progress progress, logger * p_logger, int logger_typ
 	return true;
 }
 
-void TestDoubleToString()
-{
-	char buffer[100];
-
-	
-	for (int index = 0; index < 1000; index++)
-	{
-		double r = (double)rand() / RAND_MAX;
-		r = -1000000.0 + r * (1000000.0 - -1000000.0);
-		std::cout << std::fixed << std::setprecision(10) << "Number: " << r << std::endl;
-		utilities::to_string(r, 5, buffer, true);
-		std::cout << buffer << std::endl;
-	}
-	
-}
 
 static void TestParsingCase()
 {
@@ -334,4 +352,69 @@ static void TestParsingCase()
 	parsed_command command3 = parser.parse_gcode("G0 X1 y2; test", true);
 	
 
+}
+
+bool TestIntToStringRandom(int low, int high, int num_runs)
+{
+	bool all_success = true;
+	for (int index = 0; index < num_runs; index++)
+	{
+		int value = utilities::rand_range(low, high);
+		unsigned char precision = utilities::rand_range(static_cast<unsigned char>(0), static_cast<unsigned char>(6));
+		if (!CompareDoubleToStringResult(static_cast<double>(value), precision))
+		{
+			all_success = false;
+		}
+	}
+	return all_success;
+}
+
+
+bool TestDoubleToStringRandom(double low, double high, int num_runs)
+{
+	bool all_success = true;
+	for (int index = 0; index < num_runs; index++)
+	{
+		double value = utilities::rand_range(low, high);
+		unsigned char precision = utilities::rand_range(static_cast<unsigned char>(0), static_cast<unsigned char>(6));
+		if (!CompareDoubleToStringResult(value, precision))
+		{
+			all_success = false;
+		}
+	}
+	return all_success;
+}
+
+bool CompareDoubleToStringResult(double value, unsigned char precision)
+{
+	
+	std::ostringstream stream;
+	stream << std::fixed;
+	stream << std::setprecision(precision) << value;
+	//std::cout << std::fixed << "Testing: " << std::setprecision(12) << value << " precision: " << std::setprecision(0) << static_cast <int> (precision);
+	std::string test_string = utilities::dtos(value, precision);
+	if (test_string != stream.str())
+	{
+		std::cout << std::fixed << "Failed to convert: " << std::setprecision(24) << value << " Precision:" << std::setprecision(0) << static_cast <int> (precision) << " String:" << test_string << " Stream:" << stream.str() << std::endl;
+		return false;
+	}
+	//std::cout << std::endl;
+	return true;
+	
+}
+
+bool TestProblemDoubles()
+{
+	bool result = true;
+	result = result && CompareDoubleToStringResult(-0.000030518509475996325, 4);
+	result = result && CompareDoubleToStringResult(0.500000000000000000000000, static_cast<unsigned int>(2));
+	result = result && CompareDoubleToStringResult(9.9999999999999, static_cast<unsigned int>(2));
+	result = result && CompareDoubleToStringResult(9.9950, static_cast<unsigned int>(2));
+	result = result && CompareDoubleToStringResult(39.6, static_cast<unsigned int>(3));
+
+	result = result && CompareDoubleToStringResult(39.600000000000001421085472, static_cast<unsigned int>(3));
+	result = result && CompareDoubleToStringResult(40.228999999999999204192136, static_cast<unsigned int>(3));
+	
+	
+	return result;
 }
\ No newline at end of file
diff --git a/ArcWelderTest/ArcWelderTest.h b/ArcWelderTest/ArcWelderTest.h
index 65a3604..3a80f3c 100644
--- a/ArcWelderTest/ArcWelderTest.h
+++ b/ArcWelderTest/ArcWelderTest.h
@@ -45,9 +45,11 @@ static gcode_position_args get_5_shared_extruder_position_args();
 static gcode_position_args get_5_extruder_position_args();
 static void TestAntiStutter(std::string filePath);
 bool on_progress(arc_welder_progress progress, logger* p_logger, int logger_type);
-static void TestDoubleToString();
 static void TestParsingCase();
-
+bool CompareDoubleToStringResult(double value, unsigned char precision);
+bool TestIntToStringRandom(int low, int high, int num_runs);
+bool TestDoubleToStringRandom(double low, double high, int num_runs);
+bool TestProblemDoubles();
 
 static std::string ANTI_STUTTER_TEST = "C:\\Users\\Brad\\Documents\\3DPrinter\\AntiStutter\\5x5_cylinder_2000Fn_0.2mm_PLA_MK2.5MMU2_4m.gcode";
 static std::string BENCHY_GCODE = "C:\\Users\\Brad\\Documents\\3DPrinter\\Calibration\\Benchy\\3DBenchy_0.2mm_PLA_MK2.5MMU2.gcode";
@@ -75,6 +77,7 @@ static std::string SIX_SPEED_TEST = "C:\\Users\\Brad\\Documents\\3DPrinter\\Anti
 // Issues
 static std::string ISSUE_MIMUPREFERIDA = "C:\\Users\\Brad\\Documents\\AntiStutter\\Issues\\MIMUPREFERIDA\\TESTSTUTTER.gcode";
 static std::string BARBARIAN = "C:\\Users\\Brad\\Documents\\AntiStutter\\Issues\\PricklyPear\\Barbarian.gcode";
+static std::string BAD_ARC = "C:\\Users\\Brad\\Documents\\AntiStutter\\Issues\\PricklyPear\\bad_arc.gcode";
 static std::string ISSUE_PRICKLYPEAR_LAYER_0_114 = "C:\\Users\\Brad\\Documents\\AntiStutter\\Issues\\PricklyPear\\Layers0_114.gcode";
 // Sanity tests
 static std::string COLINEAR_TEST_1 = "C:\\Users\\Brad\\Documents\\AntiStutter\\Sanity Checks\\G2_colinear_test.gcode";
diff --git a/GcodeProcessorLib/GcodeProcessorLib.vcxproj b/GcodeProcessorLib/GcodeProcessorLib.vcxproj
index 12aa788..513f74c 100644
--- a/GcodeProcessorLib/GcodeProcessorLib.vcxproj
+++ b/GcodeProcessorLib/GcodeProcessorLib.vcxproj
@@ -197,6 +197,7 @@
     <ClInclude Include="array_list.h" />
     <ClInclude Include="circular_buffer.h" />
     <ClInclude Include="extruder.h" />
+    <ClInclude Include="fpconv.h" />
     <ClInclude Include="gcode_comment_processor.h" />
     <ClInclude Include="gcode_parser.h" />
     <ClInclude Include="gcode_position.h" />
@@ -211,6 +212,7 @@
     <ClCompile Include="array_list.cpp" />
     <ClCompile Include="circular_buffer.cpp" />
     <ClCompile Include="extruder.cpp" />
+    <ClCompile Include="fpconv.cpp" />
     <ClCompile Include="gcode_comment_processor.cpp" />
     <ClCompile Include="gcode_parser.cpp" />
     <ClCompile Include="gcode_position.cpp" />
diff --git a/GcodeProcessorLib/GcodeProcessorLib.vcxproj.filters b/GcodeProcessorLib/GcodeProcessorLib.vcxproj.filters
index 3989cb0..60b8ca9 100644
--- a/GcodeProcessorLib/GcodeProcessorLib.vcxproj.filters
+++ b/GcodeProcessorLib/GcodeProcessorLib.vcxproj.filters
@@ -51,6 +51,9 @@
     <ClInclude Include="version.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="fpconv.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="array_list.cpp">
@@ -86,6 +89,9 @@
     <ClCompile Include="utilities.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="fpconv.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <Text Include="CMakeLists.txt" />
diff --git a/GcodeProcessorLib/array_list.h b/GcodeProcessorLib/array_list.h
index 6e48f96..4303fe1 100644
--- a/GcodeProcessorLib/array_list.h
+++ b/GcodeProcessorLib/array_list.h
@@ -95,7 +95,7 @@ public:
 		count_++;
 	}
 	
-	T pop_front()
+	T& pop_front()
 	{
 		if (count_ == 0)
 		{
@@ -108,7 +108,7 @@ public:
 		return items_[prev_start];
 	}
 
-	T pop_back()
+	T& pop_back()
 	{
 		if (count_ == 0)
 		{
@@ -123,7 +123,7 @@ public:
 		return items_[(front_index_ + index + max_size_) % max_size_];
 	}
 
-	T get(int index) const
+	T& get(int index) const
 	{
 		return items_[(front_index_ + index + max_size_) % max_size_];
 	}
diff --git a/GcodeProcessorLib/circular_buffer.h b/GcodeProcessorLib/circular_buffer.h
index 1658b78..0312a29 100644
--- a/GcodeProcessorLib/circular_buffer.h
+++ b/GcodeProcessorLib/circular_buffer.h
@@ -61,7 +61,7 @@ public:
 		count_++;
 		items_[front_index_] = object;
 	}
-	T pop_front()
+	T& pop_front()
 	{
 		if (count_ == 0)
 		{
@@ -74,7 +74,7 @@ public:
 		return items_[prev_start];
 	}
 
-	T get(int index)
+	T& get(int index)
 	{
 		return items_[(front_index_ + index + max_size_) % max_size_];
 	}
diff --git a/GcodeProcessorLib/fpconv.cpp b/GcodeProcessorLib/fpconv.cpp
new file mode 100644
index 0000000..5d34203
--- /dev/null
+++ b/GcodeProcessorLib/fpconv.cpp
@@ -0,0 +1,532 @@
+// This code was taken from https://github.com/miloyip/dtoa-benchmark
+// And was modified to output a rounded string in decimal format
+// with fixed precision.
+// This is the license as of the time of this writing
+/*
+Copyright (C) 2014 Milo Yip
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*/
+
+// The license for the fpconv algorithm was taken from Milo's github here
+// https://github.com/miloyip/dtoa-benchmark/blob/master/src/fpconv/license
+// The text of the license appears below, from the time of this writing.
+/*
+The MIT License
+Copyright(c) 2013 Andreas Samoljuk
+
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of this softwareand associated documentation files(the
+  "Software"), to deal in the Software without restriction, including
+  without limitation the rights to use, copy, modify, merge, publish,
+  distribute, sublicense, and /or sell copies of the Software, and to
+  permit persons to whom the Software is furnished to do so, subject to
+  the following conditions :
+
+The above copyright noticeand this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <stdbool.h>
+#include <string.h>
+
+#include "fpconv.h"
+
+#define fracmask  0x000FFFFFFFFFFFFFU
+#define expmask   0x7FF0000000000000U
+#define hiddenbit 0x0010000000000000U
+#define signmask  0x8000000000000000U
+#define expbias   (1023 + 52)
+
+#define absv(n) ((n) < 0 ? -(n) : (n))
+#define minv(a, b) ((a) < (b) ? (a) : (b))
+
+static uint64_t tens[] = {
+    10000000000000000000U, 1000000000000000000U, 100000000000000000U,
+    10000000000000000U, 1000000000000000U, 100000000000000U,
+    10000000000000U, 1000000000000U, 100000000000U,
+    10000000000U, 1000000000U, 100000000U,
+    10000000U, 1000000U, 100000U,
+    10000U, 1000U, 100U,
+    10U, 1U
+};
+
+static /*inline */uint64_t get_dbits(double d)
+{
+  union {
+    double   dbl;
+    uint64_t i;
+  } dbl_bits = { d };
+
+  return dbl_bits.i;
+}
+
+static Fp build_fp(double d)
+{
+  uint64_t bits = get_dbits(d);
+
+  Fp fp;
+  fp.frac = bits & fracmask;
+  fp.exp = (bits & expmask) >> 52;
+
+  if (fp.exp) {
+    fp.frac += hiddenbit;
+    fp.exp -= expbias;
+
+  }
+  else {
+    fp.exp = -expbias + 1;
+  }
+
+  return fp;
+}
+
+static void normalize(Fp* fp)
+{
+  while ((fp->frac & hiddenbit) == 0) {
+    fp->frac <<= 1;
+    fp->exp--;
+  }
+
+  int shift = 64 - 52 - 1;
+  fp->frac <<= shift;
+  fp->exp -= shift;
+}
+
+static void get_normalized_boundaries(Fp* fp, Fp* lower, Fp* upper)
+{
+  upper->frac = (fp->frac << 1) + 1;
+  upper->exp = fp->exp - 1;
+
+  while ((upper->frac & (hiddenbit << 1)) == 0) {
+    upper->frac <<= 1;
+    upper->exp--;
+  }
+
+  int u_shift = 64 - 52 - 2;
+
+  upper->frac <<= u_shift;
+  upper->exp = upper->exp - u_shift;
+
+
+  int l_shift = fp->frac == hiddenbit ? 2 : 1;
+
+  lower->frac = (fp->frac << l_shift) - 1;
+  lower->exp = fp->exp - l_shift;
+
+
+  lower->frac <<= lower->exp - upper->exp;
+  lower->exp = upper->exp;
+}
+
+static Fp multiply(Fp* a, Fp* b)
+{
+  const uint64_t lomask = 0x00000000FFFFFFFF;
+
+  uint64_t ah_bl = (a->frac >> 32) * (b->frac & lomask);
+  uint64_t al_bh = (a->frac & lomask) * (b->frac >> 32);
+  uint64_t al_bl = (a->frac & lomask) * (b->frac & lomask);
+  uint64_t ah_bh = (a->frac >> 32) * (b->frac >> 32);
+
+  uint64_t tmp = (ah_bl & lomask) + (al_bh & lomask) + (al_bl >> 32);
+  /* round up */
+  tmp += 1U << 31;
+
+  Fp fp = {
+      ah_bh + (ah_bl >> 32) + (al_bh >> 32) + (tmp >> 32),
+      a->exp + b->exp + 64
+  };
+
+  return fp;
+}
+
+static void round_digit(char* digits, int ndigits, uint64_t delta, uint64_t rem, uint64_t kappa, uint64_t frac)
+{
+  while (rem < frac && delta - rem >= kappa &&
+    (rem + kappa < frac || frac - rem > rem + kappa - frac)) {
+
+    digits[ndigits - 1]--;
+    rem += kappa;
+  }
+}
+
+static int generate_digits(Fp* fp, Fp* upper, Fp* lower, char* digits, int* K)
+{
+  uint64_t wfrac = upper->frac - fp->frac;
+  uint64_t delta = upper->frac - lower->frac;
+
+  Fp one;
+  one.frac = 1ULL << -upper->exp;
+  one.exp = upper->exp;
+
+  uint64_t part1 = upper->frac >> -one.exp;
+  uint64_t part2 = upper->frac & (one.frac - 1);
+
+  int idx = 0, kappa = 10;
+  uint64_t* divp;
+  /* 1000000000 */
+  for (divp = tens + 10; kappa > 0; divp++) {
+
+    uint64_t div = *divp;
+    unsigned digit = part1 / div;
+
+    if (digit || idx) {
+      digits[idx++] = digit + '0';
+    }
+
+    part1 -= digit * div;
+    kappa--;
+
+    uint64_t tmp = (part1 << -one.exp) + part2;
+    if (tmp <= delta) {
+      *K += kappa;
+      round_digit(digits, idx, delta, tmp, div << -one.exp, wfrac);
+
+      return idx;
+    }
+  }
+
+  /* 10 */
+  uint64_t* unit = tens + 18;
+
+  while (true) {
+    part2 *= 10;
+    delta *= 10;
+    kappa--;
+
+    unsigned digit = part2 >> -one.exp;
+    if (digit || idx) {
+      digits[idx++] = digit + '0';
+    }
+
+    part2 &= one.frac - 1;
+    if (part2 < delta) {
+      *K += kappa;
+      round_digit(digits, idx, delta, part2, one.frac, wfrac * *unit);
+
+      return idx;
+    }
+
+    unit--;
+  }
+}
+
+static int grisu2(double d, char* digits, int* K)
+{
+  Fp w = build_fp(d);
+
+  Fp lower, upper;
+  get_normalized_boundaries(&w, &lower, &upper);
+
+  normalize(&w);
+
+  int k;
+  Fp cp = find_cachedpow10(upper.exp, &k);
+
+  w = multiply(&w, &cp);
+  upper = multiply(&upper, &cp);
+  lower = multiply(&lower, &cp);
+
+  lower.frac++;
+  upper.frac--;
+
+  *K = -k;
+
+  return generate_digits(&w, &upper, &lower, digits, K);
+}
+
+static int emit_digits(char* digits, int ndigits, char* dest, int K, bool neg)
+{
+  int exp = absv(K + ndigits - 1);
+
+  /* write plain integer */
+  if (K >= 0 && (exp < (ndigits + 7))) {
+    memcpy(dest, digits, ndigits);
+    memset(dest + ndigits, '0', K);
+
+    return ndigits + K;
+  }
+
+  /* write decimal w/o scientific notation */
+  if (K < 0 && (K > -7 || exp < 4)) {
+    int offset = ndigits - absv(K);
+    /* fp < 1.0 -> write leading zero */
+    if (offset <= 0) {
+      offset = -offset;
+      dest[0] = '0';
+      dest[1] = '.';
+      memset(dest + 2, '0', offset);
+      memcpy(dest + offset + 2, digits, ndigits);
+
+      return ndigits + 2 + offset;
+
+      /* fp > 1.0 */
+    }
+    else {
+      memcpy(dest, digits, offset);
+      dest[offset] = '.';
+      memcpy(dest + offset + 1, digits + offset, ndigits - offset);
+
+      return ndigits + 1;
+    }
+  }
+
+  /* write decimal w/ scientific notation */
+  ndigits = minv(ndigits, 18 - neg);
+
+  int idx = 0;
+  dest[idx++] = digits[0];
+
+  if (ndigits > 1) {
+    dest[idx++] = '.';
+    memcpy(dest + idx, digits + 1, ndigits - 1);
+    idx += ndigits - 1;
+  }
+
+  dest[idx++] = 'e';
+
+  char sign = K + ndigits - 1 < 0 ? '-' : '+';
+  dest[idx++] = sign;
+
+  int cent = 0;
+
+  if (exp > 99) {
+    cent = exp / 100;
+    dest[idx++] = cent + '0';
+    exp -= cent * 100;
+  }
+  if (exp > 9) {
+    int dec = exp / 10;
+    dest[idx++] = dec + '0';
+    exp -= dec * 10;
+
+  }
+  else if (cent) {
+    dest[idx++] = '0';
+  }
+
+  dest[idx++] = exp % 10 + '0';
+
+  return idx;
+}
+
+static int emit_digits_decimal(char* digits, int ndigits, char* dest, int K, bool neg, unsigned char precision)
+{
+  int exp = absv(K + ndigits - 1);
+  unsigned char length = 0;
+  /* write plain integer */
+  if (K >= 0 && (exp < (ndigits + 7))) {
+    memcpy(dest, digits, ndigits);
+    memset(dest + ndigits, '0', K);
+    // pad with 0s if we need to 
+    
+    length = ndigits + K;
+    if (precision > 0)
+    {
+      dest[length++] = '.';
+      for (; precision > 0; precision--)
+      {
+        dest[length] = '0';
+        length++;
+      }
+    }
+    return length;
+  }
+
+  /* write decimal w/o scientific notation */
+  
+  int offset = ndigits - absv(K);
+  
+  // round the number in dest
+  unsigned char terminator_location;
+  /* fp < 1.0 -> write leading zero */
+  if (offset <= 0) {
+    offset = -offset;
+    dest[0] = '0';
+    dest[1] = '.';
+    memset(dest + 2, '0', offset);
+    memcpy(dest + offset + 2, digits, ndigits);
+    length = ndigits + 2 + offset;
+    terminator_location = 2 + precision;
+    
+    /* fp > 1.0 */
+  }
+  else {
+    memcpy(dest, digits, offset);
+    dest[offset] = '.';
+    memcpy(dest + offset + 1, digits + offset, ndigits - offset);
+    length = ndigits + 1;
+    terminator_location = offset + precision + 1;
+  }
+
+  if (ndigits < terminator_location)
+  {
+    // add 0s as necessary
+    for (int zero_index = terminator_location; zero_index >= length; zero_index--)
+    {
+      dest[zero_index] = '0';
+    }
+  }
+  
+  
+  if (terminator_location < 24)
+  {
+    // Get the character at the terminator location
+    // we will use this to determine if we need to round
+    char term_char = dest[terminator_location];
+    // Create a pointer to the terminator location for fast access
+    char* p = &dest[terminator_location];
+    bool decimal_found = false;
+    int index = terminator_location;
+    if (term_char != '.' && term_char > '4')
+    {
+      // The character we found a 5-9, we need to round
+      bool roundup = true;
+      // Loop towards the left and round until we can't anymore
+      while (--index > -1)
+      {
+        p--;
+        if (!decimal_found && *p == '.')
+        {
+          continue;
+        }
+        if (*p == '9')
+        {
+          *p = '0';
+        }
+        else
+        {
+          *p += 1;
+          roundup = false;
+          break;
+        }
+      }
+      if (roundup)
+      {
+        // drat, we need to shift everything over one
+        for (int ri = length - 1; ri > -1; --ri) {
+          dest[ri+1] = dest[ri];
+        }
+        dest[0] = '1';
+        terminator_location++;
+        length++;
+      }
+    }
+    if (precision == 0)
+    {
+      terminator_location--;
+      if (terminator_location < 0)
+      {
+        terminator_location = 0;
+      }
+    }
+    //dest[terminator_location] = '\0';
+    length = terminator_location;
+  }
+  return length;
+}
+
+
+static int filter_special(double fp, char* dest)
+{
+  if (fp == 0.0) {
+    dest[0] = '0';
+    return 1;
+  }
+
+  uint64_t bits = get_dbits(fp);
+
+  bool nan = (bits & expmask) == expmask;
+
+  if (!nan) {
+    return 0;
+  }
+
+  if (bits & fracmask) {
+    dest[0] = 'n'; dest[1] = 'a'; dest[2] = 'n';
+
+  }
+  else {
+    dest[0] = 'i'; dest[1] = 'n'; dest[2] = 'f';
+  }
+
+  return 3;
+}
+
+int fpconv_dtoa(double d, char dest[24])
+{
+  char digits[18];
+
+  int str_len = 0;
+  bool neg = false;
+
+  if (get_dbits(d) & signmask) {
+    dest[0] = '-';
+    str_len++;
+    neg = true;
+  }
+
+  int spec = filter_special(d, dest + str_len);
+
+  if (spec) {
+    return str_len + spec;
+  }
+
+  int K = 0;
+  int ndigits = grisu2(d, digits, &K);
+
+  str_len += emit_digits(digits, ndigits, dest + str_len, K, neg);
+
+  return str_len;
+}
+
+int fpconv_dtos(double d, char dest[24], unsigned char precision)
+{
+  char digits[18];
+
+  int str_len = 0;
+  bool neg = false;
+
+  if (get_dbits(d) & signmask) {
+    dest[0] = '-';
+    str_len++;
+    neg = true;
+  }
+
+  int spec = filter_special(d, dest + str_len);
+
+  if (spec) {
+    return str_len + spec;
+  }
+
+  int K = 0;
+  int ndigits = grisu2(d, digits, &K);
+
+  str_len += emit_digits_decimal(digits, ndigits, dest + str_len, K, neg, precision);
+
+  return str_len;
+}
diff --git a/GcodeProcessorLib/fpconv.h b/GcodeProcessorLib/fpconv.h
new file mode 100644
index 0000000..2156eb5
--- /dev/null
+++ b/GcodeProcessorLib/fpconv.h
@@ -0,0 +1,174 @@
+// This code was taken from https://github.com/miloyip/dtoa-benchmark
+// And was modified to output a rounded string in decimal format
+// with fixed precision.
+// This is the license as of the time of this writing
+/*
+Copyright (C) 2014 Milo Yip
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*/
+
+// The license for the fpconv algorithm was taken from Milo's github here
+// https://github.com/miloyip/dtoa-benchmark/blob/master/src/fpconv/license
+// The text of the license appears below, from the time of this writing.
+/*
+The MIT License
+Copyright(c) 2013 Andreas Samoljuk
+
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of this softwareand associated documentation files(the
+  "Software"), to deal in the Software without restriction, including
+  without limitation the rights to use, copy, modify, merge, publish,
+  distribute, sublicense, and /or sell copies of the Software, and to
+  permit persons to whom the Software is furnished to do so, subject to
+  the following conditions :
+
+The above copyright noticeand this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+#ifndef FPCONV_H
+#define FPCONV_H
+
+#include <stdint.h>
+/* Fast and accurate double to string conversion based on Florian Loitsch's
+ * Grisu-algorithm[1].
+ *
+ * Input:
+ * fp -> the double to convert, dest -> destination buffer.
+ * The generated string will never be longer than 24 characters.
+ * Make sure to pass a pointer to at least 24 bytes of memory.
+ * The emitted string will not be null terminated.
+ *
+ * Output:
+ * The number of written characters.
+ *
+ * Exemplary usage:
+ *
+ * void print(double d)
+ * {
+ *      char buf[24 + 1] // plus null terminator
+ *      int str_len = fpconv_dtoa(d, buf);
+ *
+ *      buf[str_len] = '\0';
+ *      printf("%s", buf);
+ * }
+ *
+ */
+
+int fpconv_dtoa(double fp, char dest[24]);
+int fpconv_dtos(double fp, char dest[24], unsigned char precision);
+
+#endif
+
+/* [1] http://florian.loitsch.com/publications/dtoa-pldi2010.pdf */
+
+
+#define npowers     87
+#define steppowers  8
+#define firstpower -348 /* 10 ^ -348 */
+
+#define expmax     -32
+#define expmin     -60
+
+
+typedef struct Fp {
+  uint64_t frac;
+  int exp;
+} Fp;
+
+static Fp powers_ten[] = {
+    { 18054884314459144840U, -1220 }, { 13451937075301367670U, -1193 },
+    { 10022474136428063862U, -1166 }, { 14934650266808366570U, -1140 },
+    { 11127181549972568877U, -1113 }, { 16580792590934885855U, -1087 },
+    { 12353653155963782858U, -1060 }, { 18408377700990114895U, -1034 },
+    { 13715310171984221708U, -1007 }, { 10218702384817765436U, -980 },
+    { 15227053142812498563U, -954 },  { 11345038669416679861U, -927 },
+    { 16905424996341287883U, -901 },  { 12595523146049147757U, -874 },
+    { 9384396036005875287U,  -847 },  { 13983839803942852151U, -821 },
+    { 10418772551374772303U, -794 },  { 15525180923007089351U, -768 },
+    { 11567161174868858868U, -741 },  { 17236413322193710309U, -715 },
+    { 12842128665889583758U, -688 },  { 9568131466127621947U,  -661 },
+    { 14257626930069360058U, -635 },  { 10622759856335341974U, -608 },
+    { 15829145694278690180U, -582 },  { 11793632577567316726U, -555 },
+    { 17573882009934360870U, -529 },  { 13093562431584567480U, -502 },
+    { 9755464219737475723U,  -475 },  { 14536774485912137811U, -449 },
+    { 10830740992659433045U, -422 },  { 16139061738043178685U, -396 },
+    { 12024538023802026127U, -369 },  { 17917957937422433684U, -343 },
+    { 13349918974505688015U, -316 },  { 9946464728195732843U,  -289 },
+    { 14821387422376473014U, -263 },  { 11042794154864902060U, -236 },
+    { 16455045573212060422U, -210 },  { 12259964326927110867U, -183 },
+    { 18268770466636286478U, -157 },  { 13611294676837538539U, -130 },
+    { 10141204801825835212U, -103 },  { 15111572745182864684U, -77 },
+    { 11258999068426240000U, -50 },   { 16777216000000000000U, -24 },
+    { 12500000000000000000U,   3 },   { 9313225746154785156U,   30 },
+    { 13877787807814456755U,  56 },   { 10339757656912845936U,  83 },
+    { 15407439555097886824U, 109 },   { 11479437019748901445U, 136 },
+    { 17105694144590052135U, 162 },   { 12744735289059618216U, 189 },
+    { 9495567745759798747U,  216 },   { 14149498560666738074U, 242 },
+    { 10542197943230523224U, 269 },   { 15709099088952724970U, 295 },
+    { 11704190886730495818U, 322 },   { 17440603504673385349U, 348 },
+    { 12994262207056124023U, 375 },   { 9681479787123295682U,  402 },
+    { 14426529090290212157U, 428 },   { 10748601772107342003U, 455 },
+    { 16016664761464807395U, 481 },   { 11933345169920330789U, 508 },
+    { 17782069995880619868U, 534 },   { 13248674568444952270U, 561 },
+    { 9871031767461413346U,  588 },   { 14708983551653345445U, 614 },
+    { 10959046745042015199U, 641 },   { 16330252207878254650U, 667 },
+    { 12166986024289022870U, 694 },   { 18130221999122236476U, 720 },
+    { 13508068024458167312U, 747 },   { 10064294952495520794U, 774 },
+    { 14996968138956309548U, 800 },   { 11173611982879273257U, 827 },
+    { 16649979327439178909U, 853 },   { 12405201291620119593U, 880 },
+    { 9242595204427927429U,  907 },   { 13772540099066387757U, 933 },
+    { 10261342003245940623U, 960 },   { 15290591125556738113U, 986 },
+    { 11392378155556871081U, 1013 },  { 16975966327722178521U, 1039 },
+    { 12648080533535911531U, 1066 }
+};
+
+static Fp find_cachedpow10(int exp, int* k)
+{
+  const double one_log_ten = 0.30102999566398114;
+
+  int approx = -(exp + npowers) * one_log_ten;
+  int idx = (approx - firstpower) / steppowers;
+
+  while (1) {
+    int current = exp + powers_ten[idx].exp + 64;
+
+    if (current < expmin) {
+      idx++;
+      continue;
+    }
+
+    if (current > expmax) {
+      idx--;
+      continue;
+    }
+
+    *k = (firstpower + idx * steppowers);
+
+    return powers_ten[idx];
+  }
+}
\ No newline at end of file
diff --git a/GcodeProcessorLib/utilities.cpp b/GcodeProcessorLib/utilities.cpp
index b0b0edc..d98e639 100644
--- a/GcodeProcessorLib/utilities.cpp
+++ b/GcodeProcessorLib/utilities.cpp
@@ -24,6 +24,7 @@
 #include <sstream>
 #include <iostream>
 #include <iomanip>
+#include "fpconv.h"
 
 const std::string utilities::WHITESPACE_ = " \n\r\t\f\v";
 const char utilities::GUID_RANGE[] = "0123456789abcdef";
@@ -72,7 +73,7 @@ double utilities::get_cartesian_distance(double x1, double y1, double x2, double
 	double xdif = x1 - x2;
 	double ydif = y1 - y2;
 	double dist_squared = xdif * xdif + ydif * ydif;
-	return sqrt(dist_squared);
+	return std::sqrt(dist_squared);
 }
 
 double utilities::get_cartesian_distance(double x1, double y1, double z1, double x2, double y2, double z2)
@@ -82,7 +83,7 @@ double utilities::get_cartesian_distance(double x1, double y1, double z1, double
 	double ydif = y1 - y2;
 	double zdif = z1 - z2;
 	double dist_squared = xdif * xdif + ydif * ydif + zdif * zdif;
-	return sqrt(dist_squared);
+	return std::sqrt(dist_squared);
 }
 
 std::string utilities::to_string(double value)
@@ -99,74 +100,6 @@ std::string utilities::to_string(int value)
 	return os.str();
 }
 
-char * utilities::to_string(double value, unsigned short precision, char * str, bool exact_precision)
-{
-	if (utilities::is_zero(value))
-	{
-		value = 0;
-	}
-	char reversed_int[REVERSED_INT_BUFFER];
-	
-	int char_count = 0, int_count = 0;
-	bool is_negative = false;
-	double integer_part, fractional_part;
-	fractional_part = std::abs(std::modf(value, &integer_part)); //Separate integer/fractional parts
-	if (value < 0)
-	{
-		str[char_count++] = '-';
-		integer_part *= -1;
-		is_negative = true;
-	}
-
-	if (integer_part == 0)
-	{
-		str[char_count++] = '0';
-	}
-	else
-	{
-		while (integer_part > 0) //Convert integer part, if any
-		{
-			reversed_int[int_count++] = '0' + (int)std::fmod(integer_part, 10);
-			integer_part = std::floor(integer_part / 10);
-		}
-	}
-	int start = is_negative ? 1 : 0;
-	int end = char_count - start;
-	for (int i = 0; i < int_count && i < REVERSED_INT_BUFFER; i++)
-	{
-		int reversed_int_index = int_count - i - 1;
-		if (reversed_int_index < 0 || reversed_int_index >= REVERSED_INT_BUFFER)
-		{
-			std::cerr << "Buffer overflow turning " << value << " into a string!";
-			break;
-		}
-		str[char_count++] = reversed_int[reversed_int_index];
-	}
-	if ( precision > 0)
-	{
-		str[char_count++] = '.'; //Decimal point
-
-		// We will look 1 past the precision to see if it is a 9.  if it is, we will round up.
-		//if (precision > 0) precision++;
-		while (fractional_part > 0 && precision-- > 0) //Convert fractional part, if any
-		{
-			fractional_part *= 10;
-			fractional_part = std::modf(fractional_part, &integer_part);
-			str[char_count++] = '0' + (int)integer_part;
-		}
-		// remove any unnecessary zeros
-		if (!exact_precision)
-		{
-			while (str[char_count-1] == '0') { char_count--; }
-			// Remove the period
-			if (str[char_count-1] == '.') char_count--;
-		}
-		
-	}
-	str[char_count] = 0; //String terminator
-	return str;
-}
-
 std::string utilities::ltrim(const std::string& s)
 {
 	size_t start = s.find_first_not_of(WHITESPACE_);
@@ -356,5 +289,26 @@ double utilities::hypot(double x, double y)
 	}
 	if (y == 0.0) return x;
 	y /= x;
-	return x * sqrt(1.0 + y * y);
+	return x * std::sqrt(1.0 + y * y);
 }
+
+std::string utilities::dtos(double x, unsigned char precision)
+{
+	static char buffer[FPCONV_BUFFER_LENGTH];
+	char* p = buffer;
+	buffer[fpconv_dtos(x, buffer, precision)] = '\0';
+	/* This is code that can be used to compare the output of the 
+		 modified fpconv_dtos function to the ofstream output
+		 Note:  It currently only fails for some checks where the original double does not store
+					  perfectly.  In this case I actually think the dtos output is better than ostringstream!
+	std::ostringstream stream;
+	stream << std::fixed;
+	stream << std::setprecision(precision) << x;
+	
+	if (std::string(buffer) != stream.str())
+	{
+		std::cout << std::fixed << "Failed to convert: " << std::setprecision(24) << x << " Precision:" << std::setprecision(0) << static_cast <int> (precision) << " String:" << std::string(buffer) << " Stream:" << stream.str() << std::endl;
+	}
+	*/
+	return buffer;
+}
\ No newline at end of file
diff --git a/GcodeProcessorLib/utilities.h b/GcodeProcessorLib/utilities.h
index c7fdcfb..401bed4 100644
--- a/GcodeProcessorLib/utilities.h
+++ b/GcodeProcessorLib/utilities.h
@@ -24,7 +24,7 @@
 #include <string>
 #include <vector>
 #include <set>
-#define REVERSED_INT_BUFFER 20
+#define FPCONV_BUFFER_LENGTH 25
 // Had to increase the zero tolerance because prusa slicer doesn't always 
 // retract enough while wiping.
 #define ZERO_TOLERANCE 0.000005
@@ -43,7 +43,6 @@ public:
 	static double get_cartesian_distance(double x1, double y1, double z1, double x2, double y2, double z2);
 	static std::string to_string(double value);
 	static std::string to_string(int value);
-	static char* to_string(double value, unsigned short precision, char* str, bool exact_precision);
 	static std::string ltrim(const std::string& s);
 	static std::string rtrim(const std::string& s);
 	static std::string trim(const std::string& s);
@@ -62,6 +61,22 @@ public:
 	// Man I can't wait till I can drop python 2.7 support so I can stop doing everything myself.  s
 	// td::hypot doesn't work for msvc for python 2.7....
 	static double hypot(double x, double y);
+	static std::string dtos(double x, unsigned char precision);
+
+	static double rand_range(double min, double max) {
+		double f = (double)std::rand() / RAND_MAX;
+		return min + f * (max - min);
+	}
+
+	static unsigned char rand_range(unsigned char min, unsigned char max) {
+		double f = (double)std::rand() / RAND_MAX;
+		return static_cast<unsigned char>(static_cast<double>(min) + f * (static_cast<double>(max) - static_cast<double>(min)));
+	}
+
+	static int rand_range(int min, int max) {
+		double f = (double)std::rand() / RAND_MAX;
+		return static_cast<int>(static_cast<double>(min) + f * (static_cast<double>(max) - static_cast<double>(min)));
+	}
 
 	
 protected: