Remove absolute extrusion corrections in arc_welder. Changes to inverse processor.

7 files changed, 214 insertions, 245 deletions

diff --git a/ArcWelder/arc_welder.cpp b/ArcWelder/arc_welder.cpp
index de89646..89adbe3 100644
--- a/ArcWelder/arc_welder.cpp
+++ b/ArcWelder/arc_welder.cpp
@@ -54,8 +54,9 @@ arc_welder::arc_welder(std::string source_path, std::string target_path, logger
 	last_gcode_line_written_ = 0;
 	points_compressed_ = 0;
 	arcs_created_ = 0;
-	waiting_for_line_ = false;
 	waiting_for_arc_ = false;
+	previous_feedrate_ = -1;
+	previous_is_extruder_relative_ = false;
 	gcode_position_args_.set_num_extruders(8);
 	for (int index = 0; index < 8; index++)
 	{
@@ -132,7 +133,6 @@ void arc_welder::reset()
 	file_size_ = 0;
 	points_compressed_ = 0;
 	arcs_created_ = 0;
-	waiting_for_line_ = false;
 	waiting_for_arc_ = false;
 }
 
@@ -336,7 +336,7 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end)
 		
 		if (!waiting_for_arc_)
 		{
-			previous_feedrate_ = p_pre_pos->f;
+			previous_is_extruder_relative_ = p_pre_pos->is_extruder_relative;
 			if (debug_logging_enabled_)
 			{
 				p_logger_->log(logger_type_, DEBUG, "Starting new arc from Gcode:" + cmd.gcode);
@@ -357,6 +357,7 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end)
 			if (!waiting_for_arc_)
 			{
 				waiting_for_arc_ = true;
+				previous_feedrate_ = p_pre_pos->f;
 			}
 			else
 			{
@@ -491,16 +492,24 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end)
 				p_source_position_->undo_update();
 				// IMPORTANT NOTE: p_cur_pos and p_pre_pos will NOT be usable beyond this point.
 				p_pre_pos = NULL;
-				p_cur_pos = NULL;
+				p_cur_pos = p_source_position_->get_current_position_ptr();
+				extruder_current = p_cur_pos->get_current_extruder();
 
 				// Set the current feedrate if it is different, else set to 0 to indicate that no feedrate should be included
-				if(previous_feedrate_ > 0 && previous_feedrate_ == current_f)
-				{
+				if(previous_feedrate_ > 0 && previous_feedrate_ == current_f){
 					current_f = 0;
 				}
 
 				// Craete the arc gcode
-				std::string gcode = get_arc_gcode(current_f, comment);
+				std::string gcode;
+				if (previous_is_extruder_relative_){
+					gcode = get_arc_gcode_relative(current_f, comment);
+				}
+					
+				else { 
+					gcode = get_arc_gcode_absolute(extruder_current.get_offset_e(), current_f, comment);
+				}
+				
 
 				if (debug_logging_enabled_)
 				{
@@ -510,7 +519,7 @@ int arc_welder::process_gcode(parsed_command cmd, bool is_end)
 				// Get and alter the current position so we can add it to the unwritten commands list
 				parsed_command arc_command = parser_.parse_gcode(gcode.c_str());
 				unwritten_commands_.push_back(
-					unwritten_command(arc_command, p_source_position_->get_current_position_ptr()->is_extruder_relative)
+					unwritten_command(arc_command, p_cur_pos->is_extruder_relative)
 				);
 				
 				// write all unwritten commands (if we don't do this we'll mess up absolute e by adding an offset to the arc)
@@ -625,25 +634,34 @@ int arc_welder::write_unwritten_gcodes_to_file()
 	return size;
 }
 
-std::string arc_welder::get_arc_gcode(double f, const std::string comment)
+std::string arc_welder::get_arc_gcode_relative(double f, const std::string comment)
 {
 	// Write gcode to file
 	std::string gcode;
-	position* p_new_current_pos = p_source_position_->get_current_position_ptr();
+
+	gcode = current_arc_.get_shape_gcode_relative(f);
 	
-	if (p_new_current_pos->is_extruder_relative)
-	{
-		gcode = current_arc_.get_shape_gcode_relative(f);
-	}
-	else
+	if (comment.length() > 0)
 	{
-		// Make sure to add the absoulte e offset
-		gcode = current_arc_.get_shape_gcode_absolute(f, p_new_current_pos->get_current_extruder().get_offset_e());
+		gcode += ";" + comment;
 	}
+	return gcode;
+	
+}
+
+std::string arc_welder::get_arc_gcode_absolute(double e, double f, const std::string comment)
+{
+	// Write gcode to file
+	std::string gcode;
+
+	gcode = current_arc_.get_shape_gcode_absolute(e, f);
+
 	if (comment.length() > 0)
 	{
 		gcode += ";" + comment;
 	}
 	return gcode;
-	
+
 }
+
+
diff --git a/ArcWelder/arc_welder.h b/ArcWelder/arc_welder.h
index 7e75038..d1ab85c 100644
--- a/ArcWelder/arc_welder.h
+++ b/ArcWelder/arc_welder.h
@@ -109,7 +109,8 @@ private:
 	progress_callback progress_callback_;
 	int process_gcode(parsed_command cmd, bool is_end);
 	int write_gcode_to_file(std::string gcode);
-	std::string get_arc_gcode(double f, const std::string comment);
+	std::string get_arc_gcode_relative(double f, const std::string comment);
+	std::string get_arc_gcode_absolute(double e, double f, const std::string comment);
 	std::string get_comment_for_arc();
 	int write_unwritten_gcodes_to_file();
 	std::string create_g92_e(double absolute_e);
@@ -127,7 +128,6 @@ private:
 	long get_file_size(const std::string& file_path);
 	double get_time_elapsed(double start_clock, double end_clock);
 	double get_next_update_time() const;
-	bool waiting_for_line_;
 	bool waiting_for_arc_;
 	array_list<unwritten_command> unwritten_commands_;
 	segmented_arc current_arc_;
@@ -135,7 +135,8 @@ private:
 	
 	// We don't care about the printer settings, except for g91 influences extruder.
 	gcode_position * p_source_position_;
-	double previous_feedrate_ = -1;
+	double previous_feedrate_;
+	bool previous_is_extruder_relative_;
 	gcode_parser parser_;
 	bool verbose_output_;
 	int logger_type_;
diff --git a/ArcWelder/segmented_arc.cpp b/ArcWelder/segmented_arc.cpp
index f51bf6a..4f46019 100644
--- a/ArcWelder/segmented_arc.cpp
+++ b/ArcWelder/segmented_arc.cpp
@@ -122,7 +122,7 @@ bool segmented_arc::try_add_point(point p, double e_relative)
 	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, distance);
 	}
 	if (point_added)
 	{
@@ -152,7 +152,7 @@ bool segmented_arc::try_add_point(point p, double e_relative)
 	return point_added;
 }
 
-bool segmented_arc::try_add_point_internal(point p, double pd)
+bool segmented_arc::try_add_point_internal_(point p, double pd)
 {
 	// If we don't have enough points (at least min_segments) return false
 	if (points_.count() < min_segments_ - 1)
@@ -172,7 +172,7 @@ bool segmented_arc::try_add_point_internal(point p, double pd)
 		bool circle_fits_points;
 
 		// the circle is new..  we have to test it now, which is expensive :(
-		circle_fits_points = does_circle_fit_points(test_circle, p, pd);
+		circle_fits_points = does_circle_fit_points_(test_circle, p, pd);
 		if (circle_fits_points)
 		{
 			arc_circle_ = test_circle;
@@ -190,7 +190,7 @@ bool segmented_arc::try_add_point_internal(point p, double pd)
 	
 }
 
-bool segmented_arc::does_circle_fit_points(circle c, point p, double pd)
+bool segmented_arc::does_circle_fit_points_(circle c, point p, double pd)
 {
 	// We know point 1 must fit (we used it to create the circle).  Check the other points
 	// Note:  We have not added the current point, but that's fine since it is guaranteed to fit too.
@@ -245,7 +245,7 @@ bool segmented_arc::does_circle_fit_points(circle c, point p, double pd)
 	
 	// get the current arc and compare the total length to the original length
 	arc a;
-	return try_get_arc(c, p, pd, a );
+	return try_get_arc_(c, p, pd, a );
 	
 }
 
@@ -255,13 +255,23 @@ bool segmented_arc::try_get_arc(arc & target_arc)
 	return arc::try_create_arc(arc_circle_, points_[0], points_[mid_point_index], points_[points_.count() - 1], original_shape_length_, resolution_mm_, target_arc);
 }
 
-bool segmented_arc::try_get_arc(circle& c, point endpoint, double additional_distance, arc &target_arc)
+bool segmented_arc::try_get_arc_(circle& c, point endpoint, double additional_distance, arc &target_arc)
 {
 	int mid_point_index = ((points_.count() - 1) / 2) + 1;
 	return arc::try_create_arc(c, points_[0], points_[mid_point_index], endpoint, original_shape_length_ + additional_distance, resolution_mm_, target_arc);
 }
 
-std::string segmented_arc::get_shape_gcode_absolute(double f, double e_abs_start)
+std::string segmented_arc::get_shape_gcode_absolute(double e, double f)
+{
+	return get_shape_gcode_(true, e, f);
+}
+std::string segmented_arc::get_shape_gcode_relative(double f)
+{
+	bool has_e = e_relative_ > 0;
+	return get_shape_gcode_(has_e, e_relative_, f);
+}
+
+std::string segmented_arc::get_shape_gcode_(bool has_e, double e, double f)
 {
 	arc c;
 	try_get_arc(c);
@@ -273,9 +283,8 @@ std::string segmented_arc::get_shape_gcode_absolute(double f, double e_abs_start
 	if (utilities::less_than(c.angle_radians, 0))
 	{
 		// G2
-		if (e_relative_ != 0)
+		if (has_e != 0)
 		{
-			double e = e_abs_start + e_relative_;
 			// Add E param
 			if (utilities::greater_than_or_equal(f, 1))
 			{
@@ -307,9 +316,8 @@ std::string segmented_arc::get_shape_gcode_absolute(double f, double e_abs_start
 	else
 	{
 		// G3
-		if (e_relative_ != 0)
+		if (has_e != 0)
 		{
-			double e = e_abs_start + e_relative_;
 			// Add E param
 			if (utilities::greater_than_or_equal(f, 1))
 			{
@@ -342,7 +350,3 @@ std::string segmented_arc::get_shape_gcode_absolute(double f, double e_abs_start
 
 }
 
-std::string segmented_arc::get_shape_gcode_relative(double f)
-{
-	return get_shape_gcode_absolute(f, 0.0);
-}
diff --git a/ArcWelder/segmented_arc.h b/ArcWelder/segmented_arc.h
index 02a8482..416fd3e 100644
--- a/ArcWelder/segmented_arc.h
+++ b/ArcWelder/segmented_arc.h
@@ -37,19 +37,21 @@ public:
 	segmented_arc(int max_segments, double resolution_mm);
 	virtual ~segmented_arc();
 	virtual bool try_add_point(point p, double e_relative);
-	virtual std::string get_shape_gcode_absolute(double f, double e_abs_start);
-	virtual std::string get_shape_gcode_relative(double f);
+	std::string get_shape_gcode_absolute(double e, double f);
+	std::string get_shape_gcode_relative(double f);
+	
 	virtual bool is_shape();
 	point pop_front(double e_relative);
 	point pop_back(double e_relative);
 	bool try_get_arc(arc & target_arc);
 	// static gcode buffer
-	
+
 private:
 	char gcode_buffer_[GCODE_CHAR_BUFFER_SIZE];
-	bool try_add_point_internal(point p, double pd);
-	bool does_circle_fit_points(circle c, point p, double additional_distance);
-	bool try_get_arc(circle& c, point endpoint, double additional_distance, arc & target_arc);
+	bool try_add_point_internal_(point p, double pd);
+	bool does_circle_fit_points_(circle c, point p, double additional_distance);
+	bool try_get_arc_(circle& c, point endpoint, double additional_distance, arc & target_arc);
+	std::string get_shape_gcode_(bool has_e, double e, double f);
 	int min_segments_;
 	circle arc_circle_;
 	int test_count_ = 0;
diff --git a/ArcWelderInverseProcessor/inverse_processor.cpp b/ArcWelderInverseProcessor/inverse_processor.cpp
index 02991f4..449d6c1 100644
--- a/ArcWelderInverseProcessor/inverse_processor.cpp
+++ b/ArcWelderInverseProcessor/inverse_processor.cpp
@@ -48,12 +48,7 @@
 */
 
 #include "inverse_processor.h"
-#include "math.h"
-#include <iostream>
-#include <string>
-#include <sstream>
-#include <iomanip>
-#include <fstream>
+
 
 //#include "Marlin.h"
 //#include "stepper.h"
@@ -135,19 +130,17 @@ void inverse_processor::process()
 
     // Create the source file read stream and target write stream
     std::ifstream gcode_file;
-    std::ofstream output_file;
     gcode_file.open(source_path_.c_str());
-    output_file.open(target_path_.c_str());
+    output_file_.open(target_path_.c_str());
     std::string line;
     int lines_with_no_commands = 0;
     gcode_file.sync_with_stdio(false);
-    output_file.sync_with_stdio(false);
+    output_file_.sync_with_stdio(false);
     gcode_parser parser;
-    int lines_processed = 0;
     int gcodes_processed = 0;
     if (gcode_file.is_open())
     {
-        if (output_file.is_open())
+        if (output_file_.is_open())
         {
             //stream.clear();
             //stream.str("");
@@ -157,7 +150,7 @@ void inverse_processor::process()
             // Communicate every second
             while (std::getline(gcode_file, line))
             {
-                lines_processed++;
+                lines_processed_++;
 
                 cmd.clear();
                 parser.try_parse_gcode(line.c_str(), cmd);
@@ -172,7 +165,7 @@ void inverse_processor::process()
                     lines_with_no_commands++;
                 }
 
-                p_source_position_->update(cmd, lines_processed, gcodes_processed, -1);
+                p_source_position_->update(cmd, lines_processed_, gcodes_processed, -1);
 
                 if (cmd.command == "G2" || cmd.command == "G3")
                 {
@@ -205,15 +198,16 @@ void inverse_processor::process()
                     }
                     float radius = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
                     uint8_t isclockwise = cmd.command == "G2" ? 1 : 0;
-                    output_file << mc_arc(position, target, offset, X_AXIS, Y_AXIS, Z_AXIS, static_cast<float>(p_cur_pos->f), radius, isclockwise, 0, p_cur_pos->is_extruder_relative) << "\n";
+                    output_relative_ = p_cur_pos->is_extruder_relative;
+                    mc_arc(position, target, offset, static_cast<float>(p_cur_pos->f), radius, isclockwise, 0);
                 }
                 else
                 {
-                    output_file << line << "\n";
+                    output_file_ << line << "\n";
                 }
 
             }
-            output_file.close();
+            output_file_.close();
         }
         else
         {
@@ -233,7 +227,7 @@ void inverse_processor::process()
     stream.clear();
     stream.str("");
     stream << "Completed file processing\r\n";
-    stream << "\tLines Processed      : " << lines_processed << "\r\n";
+    stream << "\tLines Processed      : " << lines_processed_ << "\r\n";
     stream << "\tTotal Seconds        : " << total_seconds << "\r\n";
     stream << "\tExtra Trig Count     : " << trig_calc_count << "\r\n";
     stream << "\tTotal E Adjustment   : " << total_e_adjustment << "\r\n";
@@ -242,52 +236,63 @@ void inverse_processor::process()
 
 // The arc is approximated by generating a huge number of tiny, linear segments. The length of each 
 // segment is configured in settings.mm_per_arc_segment.  
-std::string inverse_processor::mc_arc(float* position, float* target, float* offset, uint8_t axis_0, uint8_t axis_1,
-    uint8_t axis_linear, float feed_rate, float radius, uint8_t isclockwise, uint8_t extruder, bool output_relative)
+void inverse_processor::mc_arc(float* position, float* target, float* offset, float feed_rate, float radius, uint8_t isclockwise, uint8_t extruder)
 {
-    std::stringstream stream;
-    stream << std::fixed;
-    std::string gcodes;
-    
-    // Start Modifications
-    float center_axis0 = position[axis_0] + offset[axis_0];
-    float center_axis1 = position[axis_1] + offset[axis_1];
-    float linear_travel = target[axis_linear] - position[axis_linear];
-    float extruder_travel_total = target[E_AXIS] - position[E_AXIS];
-    float r_axis0 = -offset[axis_0];  // Radius vector from center to current location
-    float r_axis1 = -offset[axis_1];
-    float rt_axis0 = target[axis_0] - center_axis0;
-    float rt_axis1 = target[axis_1] - center_axis1;
+    // Extract the position to reduce indexing at the cost of a few bytes of mem
+    float p_x = position[X_AXIS];
+    float p_y = position[Y_AXIS];
+    float p_z = position[Z_AXIS];
+    float p_e = position[E_AXIS];
+
+    float t_x = target[X_AXIS];
+    float t_y = target[Y_AXIS];
+    float t_z = target[Z_AXIS];
+    float t_e = target[E_AXIS];
+
+    float r_axis_x = -offset[X_AXIS];  // Radius vector from center to current location
+    float r_axis_y = -offset[Y_AXIS];
+    float center_axis_x = p_x - r_axis_x;
+    float center_axis_y = p_y - r_axis_y;
+    float travel_z = t_z - p_z;
+    float extruder_travel_total = t_e - p_e;
+
+    float rt_x = t_x - center_axis_x;
+    float rt_y = t_y - center_axis_y;
     // 20200419 - Add a variable that will be used to hold the arc segment length
-    float mm_per_arc_segment = MM_PER_ARC_SEGMENT;
+    float mm_per_arc_segment = cs.mm_per_arc_segment;
 
     // CCW angle between position and target from circle center. Only one atan2() trig computation required.
-    float angular_travel_total = atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
+    float angular_travel_total = atan2(r_axis_x * rt_y - r_axis_y * rt_x, r_axis_x * rt_x + r_axis_y * rt_y);
     if (angular_travel_total < 0) { angular_travel_total += 2 * M_PI; }
 
-#ifdef MIN_ARC_SEGMENTS
-    // 20200417 - FormerLurker - Implement MIN_ARC_SEGMENTS if it is defined - from Marlin 2.0 implementation
-    // Do this before converting the angular travel for clockwise rotation
-    if (radius < arc_max_radius_threshold) mm_per_arc_segment = radius * ((2.0f * M_PI) / MIN_ARC_SEGMENTS);
-#endif
+    bool check_mm_per_arc_segment_max = false;
+    if (cs.min_arc_segments > 0)
+    {
+        // 20200417 - FormerLurker - Implement MIN_ARC_SEGMENTS if it is defined - from Marlin 2.0 implementation
+        // Do this before converting the angular travel for clockwise rotation
+        mm_per_arc_segment = radius * ((2.0f * M_PI) / cs.min_arc_segments);
+        check_mm_per_arc_segment_max = true;
+    }
 
-#ifdef ARC_SEGMENTS_PER_SEC
-    // 20200417 - FormerLurker - Implement MIN_ARC_SEGMENTS if it is defined - from Marlin 2.0 implementation
-    float mm_per_arc_segment_sec = (feed_rate / 60.0f) * (1.0f / ARC_SEGMENTS_PER_SEC);
-    if (mm_per_arc_segment_sec < mm_per_arc_segment) 
-        mm_per_arc_segment = mm_per_arc_segment_sec;
-#endif
+    if (cs.arc_segments_per_sec > 0)
+    {
+        // 20200417 - FormerLurker - Implement MIN_ARC_SEGMENTS if it is defined - from Marlin 2.0 implementation
+        float mm_per_arc_segment_sec = (feed_rate / 60.0f) * (1.0f / cs.arc_segments_per_sec);
+        if (mm_per_arc_segment_sec < mm_per_arc_segment)
+            mm_per_arc_segment = mm_per_arc_segment_sec;
+        check_mm_per_arc_segment_max = true;
+    }
 
-#ifdef MIN_MM_PER_ARC_SEGMENT
-    // 20200417 - FormerLurker - Implement MIN_MM_PER_ARC_SEGMENT if it is defined
-    // This prevents a very high number of segments from being generated for curves of a short radius
-    if (mm_per_arc_segment < MIN_MM_PER_ARC_SEGMENT)  mm_per_arc_segment = MIN_MM_PER_ARC_SEGMENT;
-#endif
+    if (cs.min_mm_per_arc_segment > 0)
+    {
+        check_mm_per_arc_segment_max = true;
+        // 20200417 - FormerLurker - Implement MIN_MM_PER_ARC_SEGMENT if it is defined
+        // This prevents a very high number of segments from being generated for curves of a short radius
+        if (mm_per_arc_segment < cs.min_mm_per_arc_segment)  mm_per_arc_segment = cs.min_mm_per_arc_segment;
+    }
+
+    if (check_mm_per_arc_segment_max && mm_per_arc_segment > cs.mm_per_arc_segment) mm_per_arc_segment = cs.mm_per_arc_segment;
 
-#if defined(MIN_MM_PER_ARC_SEGMENT) || defined(ARC_SEGMENTS_PER_SEC) || defined(MIN_ARC_SEGMENTS)
-    if (mm_per_arc_segment > MM_PER_ARC_SEGMENT)
-        mm_per_arc_segment = MM_PER_ARC_SEGMENT;
-#endif
 
 
     // Adjust the angular travel if the direction is clockwise
@@ -295,7 +300,7 @@ std::string inverse_processor::mc_arc(float* position, float* target, float* off
 
     //20141002:full circle for G03 did not work, e.g. G03 X80 Y80 I20 J0 F2000 is giving an Angle of zero so head is not moving
     //to compensate when start pos = target pos && angle is zero -> angle = 2Pi
-    if (position[axis_0] == target[axis_0] && position[axis_1] == target[axis_1] && angular_travel_total == 0)
+    if (p_x == t_x && p_y == t_y && angular_travel_total == 0)
     {
         angular_travel_total += 2 * M_PI;
     }
@@ -303,36 +308,18 @@ std::string inverse_processor::mc_arc(float* position, float* target, float* off
 
     // 20200417 - FormerLurker - rename millimeters_of_travel to millimeters_of_travel_arc to better describe what we are
     // calculating here
-    float millimeters_of_travel_arc = hypot(angular_travel_total * radius, fabs(linear_travel));
-    if (millimeters_of_travel_arc < 0.001) { return ""; }
+    float millimeters_of_travel_arc = hypot(angular_travel_total * radius, fabs(travel_z));
+    if (millimeters_of_travel_arc < 0.001) { return; }
     // Calculate the total travel per segment
     // Calculate the number of arc segments
     uint16_t segments = static_cast<uint16_t>(ceil(millimeters_of_travel_arc / mm_per_arc_segment));
-    // Ensure at least one segment
-    //if (segments < 1) segments = 1;
+
 
     // Calculate theta per segments and linear (z) travel per segment
     float theta_per_segment = angular_travel_total / segments;
-    float linear_per_segment = linear_travel / (segments);
-
-#ifdef ARC_EXTRUSION_CORRECTION
-    // 20200417 - FormerLurker - The feedrate needs to be adjusted becaue the perimeter of a regular polygon is always
-    // less than that of a circumscribed circle.  However, after testing it has been determined that this
-    // value is very small and may not be worth the clock cycles unless the settings are vastlyl different than the 
-    // defaults
-
-    // Calculate the individual segment arc and chord length
-    float segment_length_arc = millimeters_of_travel_arc / segments;
-    float segment_length_chord = 2.0f * radius * sin(fabs(theta_per_segment) * 0.5f); // This is a costly calculation..
-    // Determine the correction factor
-    float extrusion_correction_factor = fabs(segment_length_chord / segment_length_arc);
-    // Calculate the corrected extrusion amount per segment
-    float segment_extruder_travel = (extruder_travel_total / segments) * extrusion_correction_factor;
-#else
+    float linear_per_segment = travel_z / (segments);
     // Calculate the extrusion amount per segment
     float segment_extruder_travel = extruder_travel_total / (segments);
-#endif
-
     /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
        and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
            r_T = [cos(phi) -sin(phi);
@@ -346,136 +333,70 @@ std::string inverse_processor::mc_arc(float* position, float* target, float* off
        round off issues for CNC applications.) Single precision error can accumulate to be greater than
        tool precision in some cases. Therefore, arc path correction is implemented.
 
-       Small angle approximation may be used to reduce computation overhead further. This approximation
-       holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
-       theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
-       to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
-       numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
-       issue for CNC machines with the single precision Arduino calculations.
-
-       This approximation also allows mc_arc to immediately insert a line segment into the planner
-       without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
-       a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
-       This is important when there are successive arc motions.
-    */
+       The small angle approximation was removed because of excessive errors for small circles (perhaps unique to
+       3d printing applications, causing significant path deviation and extrusion issues).
+       Now there will be no corrections applied, but an accurate initial sin and cos will be calculated.
+       This seems to work with a very high degree of accuracy and results in much simpler code.
 
-    // The initial approximation causes irregular movements in some cases, causing back travel to the final segment.
-    // Initialize the linear axis
-    float arc_target[4];
-    arc_target[axis_linear] = position[axis_linear];
-
-    // Initialize the extruder axis
-    arc_target[E_AXIS] = position[E_AXIS];
+       Finding a faster way to approximate sin, knowing that there can be substantial deviations from the true
+       arc when using the previous approximation, would be beneficial.
+    */
 
-    // Don't bother calculating cot_T or sin_T if there is only 1 segment.  This will speed up arcs that only have
-    // 1 segment.
+    // Don't bother calculating cot_T or sin_T if there is only 1 segment.
     if (segments > 1)
     {
-        float cos_T;
-        float sin_T;
-        // 20200417 - FormerLurker - Using the small angle approximation causes drift if theta is large.
-        // Use true cos/sin if the angle is large (do we need a definition for this?)
-        if (theta_per_segment < (2.0f * M_PI / 16.0f) && theta_per_segment >(-2.0f * M_PI / 16.0f))
-        {
-            // Avoids cos and sin calculations.  However, in my testing this doesn't save much time
-            // since the majority of the cost is adding the segments to the planner.  If possible,
-            // I believe it's better to reduce the number of segments as much as possible, even if it
-            // means a bit more overhead in this function.  However, for small angles this works fine
-            // and is very fast.
-            cos_T = 1.0f - 0.5f * theta_per_segment * theta_per_segment; // Small angle approximation
-            sin_T = theta_per_segment;
-        }
-        else
-        {
-            // This seems to work even without N_ARC_CORRECTION enabled for all values I tested. It produces 
-            // extremely accurate segment endpoints even when an extremely high number of segments are 
-            // generated. With 3 decimals of precision on XYZ, this should work for any reasonable settings 
-            // without correction.
-            cos_T = cos(theta_per_segment);
-            sin_T = sin(theta_per_segment);
-        }
-        float sin_Ti;
-        float cos_Ti;
+        // Initialize the extruder axis
+
+        float cos_T = cos(theta_per_segment);
+        float sin_T = sin(theta_per_segment);
         float r_axisi;
         uint16_t i;
-        int8_t count = 0;
 
         for (i = 1; i < segments; i++) { // Increment (segments-1)
+            r_axisi = r_axis_x * sin_T + r_axis_y * cos_T;
+            r_axis_x = r_axis_x * cos_T - r_axis_y * sin_T;
+            r_axis_y = r_axisi;
 
-#ifdef N_ARC_CORRECTION
-// 20200417 - FormerLurker - Make N_ARC_CORRECTION optional.
-            if (count < N_ARC_CORRECTION) {
-#endif
-                // Apply vector rotation matrix 
-                float x_0 = r_axis0;
-                float y_0 = r_axis1;
-                r_axisi = r_axis0 * sin_T + r_axis1 * cos_T;
-                r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T;
-                r_axis1 = r_axisi;
-#ifdef N_ARC_CORRECTION
-                // 20200417 - FormerLurker - Make N_ARC_CORRECTION optional.
-                count++;
-            }
-            else {
-                // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
-                // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
-                cos_Ti = cos(i * theta_per_segment);
-                sin_Ti = sin(i * theta_per_segment);
-                r_axis0 = -offset[axis_0] * cos_Ti + offset[axis_1] * sin_Ti;
-                r_axis1 = -offset[axis_0] * sin_Ti - offset[axis_1] * cos_Ti;
-                count = 0;
-            }
-#endif
             // Update arc_target location
-            arc_target[axis_0] = center_axis0 + r_axis0;
-            arc_target[axis_1] = center_axis1 + r_axis1;
-            arc_target[axis_linear] += linear_per_segment;
-            arc_target[E_AXIS] += segment_extruder_travel;
-
-            //********** TODO:  MOVE THIS TO ANOTHER FUNCITON AND USE THE ORIGINAL C AS MUCH AS POSSIBLE
-            if (stream.tellp() > 1U)
-                stream << "\n";
-
-            stream << "G1 X" << std::setprecision(3) << arc_target[X_AXIS] << " Y" << arc_target[Y_AXIS];
-            if (output_relative)
-            {
-                stream << std::setprecision(5) << " E" << segment_extruder_travel;
-            }
-            else
-            {
-                stream << std::setprecision(5) << " E" << arc_target[E_AXIS];
-            }
-
-            stream << std::setprecision(0) << " F" << feed_rate;
-            //********** END TODO
+            p_x = center_axis_x + r_axis_x;
+            p_y = center_axis_y + r_axis_y;
+            p_z += linear_per_segment;
+            p_e += segment_extruder_travel;
+            // We can't clamp to the target because we are interpolating!  We would need to update a position, clamp to it
+            // after updating from calculated values.
+            //clamp_to_software_endstops(position);
+            plan_buffer_line(p_x, p_y, p_z, p_e, feed_rate, extruder);
         }
     }
-
-#ifdef ARC_EXTRUSION_CORRECTION
-    // 20200417 - FormerLurker - adjust the final absolute e coordinate based on our extruder correction
-    target[E_AXIS] = arc_target[E_AXIS] + segment_extruder_travel;
-#endif
     // Ensure last segment arrives at target location.
-    if (stream.tellp() > 1U)
-        stream << "\n";
+    // Here we could clamp, but why bother.  We would need to update our current position, clamp to it
+    //clamp_to_software_endstops(target);
+    plan_buffer_line(t_x, t_y, t_z, t_e, feed_rate, extruder);
+}
 
-    stream << "G1 X" << std::setprecision(3) << target[X_AXIS] << " Y" << target[Y_AXIS];
-    if (output_relative)
+void inverse_processor::clamp_to_software_endstops(float* target)
+{
+    // Do nothing, just added to keep mc_arc identical to the firmware version
+    return;
+}
+
+void inverse_processor::plan_buffer_line(float x, float y, float z, const float& e, float feed_rate, uint8_t extruder, const float* gcode_target)
+{
+    std::stringstream stream;
+    stream << std::fixed;
+    
+    //output_file_ <<
+
+    stream << "G1 X" << std::setprecision(3) << x << " Y" << y;
+    if (output_relative_)
     {
-        stream << std::setprecision(5) << " E" << segment_extruder_travel;
+        stream << std::setprecision(5) << " E" << output_relative_;
     }
     else
     {
-        stream << std::setprecision(5) << " E" << target[E_AXIS];
+        stream << std::setprecision(5) << " E" << e;
     }
-    stream << std::setprecision(0) << " F" << feed_rate;
-#ifdef ARC_EXTRUSION_CORRECTION
-    // 20200417 - FormerLurker - Hide the e axis corrections from the planner
-    // Is this necessary, and is this the prefered way to accomplish this?
-    //plan_set_e_position(target[E_AXIS]);
-#endif
-    return stream.str();
 
-    //   plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
+    stream << std::setprecision(0) << " F" << feed_rate << "\n";
+    output_file_ << stream.str();
 }
-
diff --git a/ArcWelderInverseProcessor/inverse_processor.h b/ArcWelderInverseProcessor/inverse_processor.h
index b84b92a..7d5a313 100644
--- a/ArcWelderInverseProcessor/inverse_processor.h
+++ b/ArcWelderInverseProcessor/inverse_processor.h
@@ -26,7 +26,12 @@
 
 #include <string>
 #include "gcode_position.h"
-
+#include "math.h"
+#include <iostream>
+#include <string>
+#include <sstream>
+#include <iomanip>
+#include <fstream>
 
 typedef unsigned char uint8_t;
 typedef unsigned short uint16_t;
@@ -34,34 +39,52 @@ typedef signed char int8_t;
 #define M_PI       3.14159265358979323846f   // pi
 enum AxisEnum { X_AXIS = 0, Y_AXIS= 1, Z_AXIS = 2, E_AXIS = 3, X_HEAD = 4, Y_HEAD = 5 };
 // Arc interpretation settings:
-#define MM_PER_ARC_SEGMENT 2.0f // REQUIRED - The enforced maximum length of an arc segment
-#define MIN_MM_PER_ARC_SEGMENT 0.2f /* OPTIONAL - the enforced minimum length of an interpolated segment.  Must be smaller than
-	MM_PER_ARC_SEGMENT if defined.  Only has an effect if MIN_ARC_SEGMENTS or ARC_SEGMENTS_PER_SEC is defined */
+#define DEFAULT_MM_PER_ARC_SEGMENT 100.0f // REQUIRED - The enforced maximum length of an arc segment
+#define DEFAULT_MIN_MM_PER_ARC_SEGMENT 0.2f /* OPTIONAL - the enforced minimum length of an interpolated segment.  Must be smaller than
+	MM_PER_ARC_SEGMENT.  Only has an effect if MIN_ARC_SEGMENTS > 0 or ARC_SEGMENTS_PER_SEC > 0 */
 	// If both MIN_ARC_SEGMENTS and ARC_SEGMENTS_PER_SEC is defined, the minimum calculated segment length is used.
-#define MIN_ARC_SEGMENTS 20 // OPTIONAL - The enforced minimum segments in a full circle of the same radius.
-#define ARC_SEGMENTS_PER_SEC 40 // OPTIONAL - Use feedrate to choose segment length.
-#define N_ARC_CORRECTION 25 // OPTIONAL - The number of interpolated segments that will be generated without a floating point correction
-//#define ARC_EXTRUSION_CORRECTION // If defined, we should apply correction to the extrusion length based on the
-								   // difference in true arc length.  The correctly is extremely small, and may not be worth the cpu cycles
+#define DEFAULT_MIN_ARC_SEGMENTS 10 // OPTIONAL - The enforced minimum segments in a full circle of the same radius.
+#define DEFAULT_ARC_SEGMENTS_PER_SEC 30 // OPTIONAL - Use feedrate to choose segment length.
+// approximation will not be used for the first segment.  Subsequent segments will be corrected following DEFAULT_N_ARC_CORRECTION.
 
+struct ConfigurationStore {
+	ConfigurationStore() {
+		mm_per_arc_segment = DEFAULT_MM_PER_ARC_SEGMENT;
+		min_mm_per_arc_segment = DEFAULT_MIN_MM_PER_ARC_SEGMENT;
+		min_arc_segments = DEFAULT_MIN_ARC_SEGMENTS;
+		arc_segments_per_sec = DEFAULT_ARC_SEGMENTS_PER_SEC;
+	}
+	float mm_per_arc_segment;
+	float min_mm_per_arc_segment;
+	int min_arc_segments; // If less than or equal to zero, this is disabled
+	int arc_segments_per_sec; // If less than or equal to zero, this is disabled
+};
 class inverse_processor {
 public:
 	inverse_processor(std::string source_path, std::string target_path, bool g90_g91_influences_extruder, int buffer_size);
 	virtual ~inverse_processor();
 	void process();
-	std::string mc_arc(float* position, float* target, float* offset, uint8_t axis_0, uint8_t axis_1,
-		uint8_t axis_linear, float feed_rate, float radius, uint8_t isclockwise, uint8_t extruder, bool output_relative);
+	void mc_arc(float* position, float* target, float* offset, float feed_rate, float radius, uint8_t isclockwise, uint8_t extruder);
+	ConfigurationStore cs;
 private:
 	gcode_position_args get_args_(bool g90_g91_influences_extruder, int buffer_size);
 	std::string source_path_;
 	std::string target_path_;
 	gcode_position* p_source_position_;
-	
+	std::ofstream output_file_;
+	bool output_relative_;
 	float arc_max_radius_threshold;
 	//float arc_min_radius_threshold;
 	float total_e_adjustment;
 	int trig_calc_count = 0;
+	int lines_processed_ = 0;
+	void clamp_to_software_endstops(float* target);
+
+	void plan_buffer_line(float x, float y, float z, const float& e, float feed_rate, uint8_t extruder, const float* gcode_target=NULL);
+	
 };
 
 
 
+
+
diff --git a/ArcWelderTest/ArcWelderTest.cpp b/ArcWelderTest/ArcWelderTest.cpp
index bc6d380..58585d0 100644
--- a/ArcWelderTest/ArcWelderTest.cpp
+++ b/ArcWelderTest/ArcWelderTest.cpp
@@ -240,14 +240,14 @@ static void TestAntiStutter(std::string filePath)
 	p_logger->set_log_level(INFO);
 	//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(SIX_SPEED_TEST, "C:\\Users\\Brad\\Documents\\3DPrinter\\AntiStutter\\SixSpeedTest_AS.gcode", p_logger, max_resolution, false, 50, on_progress);
+	arc_welder arc_welder_obj(BENCHY_GYROID_ABSOLUTE_E_NOWIPE, "C:\\Users\\Brad\\Documents\\3DPrinter\\AntiStutter\\test_output.gcode", p_logger, max_resolution, false, 50, on_progress);
 	//BENCHY_LAYER_1GCODE
 	//SMALL_TEST
 	//FACE_SHIELD
 	//BENCHY_LAYER_1_NO_WIPE
 	//BENCHY_0_5_MM_NO_WIPE
 	//BENCHY_CURA_RELATIVE_E_NOWIPE
-	//BENCHY_CURA_ABSOLUTE_E_NOWIPE
+	//BENCHY_GYROID_ABSOLUTE_E_NOWIPE
 	//BENCHY_GYROID_RELATIVE_E_NOWIPE
 	//BENCHY_STACK_RELATIVE
 	//BENCHY_STACK_ABSOLUTE