reworked to do arc at a time

main.go

@@ -18,7 +18,7 @@ type Word struct {
 }
 
 type GCodeFile struct {
-	Blocks map[int]*Block
+	Blocks []*Block
 }
 
 type Block struct {
@@ -95,96 +95,110 @@ func main() {
 	mode := ""
 	var x, y float64
 	var lastPoints []point
-	arcs := []*arc{}
+
 	scale := ScaleMM
 
-	for lineNumber := 0; lineNumber < len(gc.Blocks); lineNumber++ {
-		block := gc.Blocks[lineNumber]
-		xyChanged, zChanged, modeChanged := false, false, false
-		for _, cmd := range block.Words {
-			oldMode := mode
-			switch cmd.Address {
-			case "G":
-				switch cmd.Number {
-				case 0:
-					mode = "g0"
-				case 1:
-					mode = "g1"
-				case 2:
-					mode = "g2"
-				case 3:
-					mode = "g3"
-				case 20:
-					scale = ScaleInch
-				case 21:
-					scale = ScaleMM
+	done := false
+	startBlock := 0
+
+	for !done {
+		lastPoints = nil
+		for lineNumber := startBlock; lineNumber < len(gc.Blocks); lineNumber++ {
+			block := gc.Blocks[lineNumber]
+			xyChanged, zChanged, modeChanged := false, false, false
+			for _, cmd := range block.Words {
+				oldMode := mode
+				switch cmd.Address {
+				case "G":
+					switch cmd.Number {
+					case 0:
+						mode = "g0"
+					case 1:
+						mode = "g1"
+					case 2:
+						mode = "g2"
+					case 3:
+						mode = "g3"
+					case 20:
+						scale = ScaleInch
+					case 21:
+						scale = ScaleMM
+					}
+				case "X":
+					x = cmd.Number
+					xyChanged = true
+				case "Y":
+					y = cmd.Number
+					xyChanged = true
+				case "Z":
+					//z = cmd.Number
+					zChanged = true
+				}
+				if mode != oldMode {
+					modeChanged = true
 				}
-			case "X":
-				x = cmd.Number
-				xyChanged = true
-			case "Y":
-				y = cmd.Number
-				xyChanged = true
-			case "Z":
-				//z = cmd.Number
-				zChanged = true
-			}
-			if mode != oldMode {
-				modeChanged = true
 			}
-		}
 
-		newPt := point{x: x, y: y, lineNumber: lineNumber, scale: scale}
+			newPt := point{x: x, y: y, lineNumber: lineNumber, scale: scale}
 
-		if zChanged || modeChanged {
-			if len(lastPoints) > 0 {
-				arcs = append(arcs, findArcs(lastPoints, scale)...)
-				lastPoints = make([]point, 0)
+			if zChanged || modeChanged {
+				if len(lastPoints) > 0 {
+					break
+				}
+			} else if mode == "g1" && xyChanged {
+				lastPoints = append(lastPoints, newPt)
 			}
-		} else if mode == "g1" && xyChanged {
-			lastPoints = append(lastPoints, newPt)
+
 		}
 
-	}
+		if len(lastPoints) == 0 {
+			done = true
+			break
+		}
 
-	if len(lastPoints) > 0 {
-		arcs = append(arcs, findArcs(lastPoints, scale)...)
-	}
+		arc := findArc(lastPoints, scale)
 
-	for _, arc := range arcs {
+		if arc == nil {
+			if len(gc.Blocks) == startBlock+len(lastPoints) {
+				break
+			}
+			continue
+		}
 
 		i := (arc.centre.x - arc.startPt.x)
 		j := (arc.centre.y - arc.startPt.y)
 
-		// clear out old commands
-		for i := arc.startBlock; i <= arc.endBlock; i++ {
-			delete(gc.Blocks, i)
-		}
+		spliced := gc.Blocks[:arc.startBlock]
 
 		// G1 to move to the beginning point of the arc
-		gc.Blocks[arc.startBlock] = &Block{Words: []Word{
+		spliced = append(spliced, &Block{Words: []Word{
 			{Address: "G", Number: 1},
 			{Address: "X", Number: arc.startPt.x},
 			{Address: "Y", Number: arc.startPt.y},
-		}}
+		}})
 
 		// G2/G3 to do the arc
 		dir := 3.0
 		if arc.clockwise {
 			dir = 2.0
 		}
-		gc.Blocks[arc.startBlock+1] = &Block{Words: []Word{
+		spliced = append(spliced, &Block{Words: []Word{
 			{Address: "G", Number: dir},
 			{Address: "I", Number: i},
 			{Address: "J", Number: j},
 			{Address: "X", Number: arc.endPt.x},
 			{Address: "Y", Number: arc.endPt.y},
-		}}
+		}})
 
 		// shove in a G1 for the surviving X and Y commands after the arc
-		gc.Blocks[arc.startBlock+2] = &Block{Words: []Word{
+		spliced = append(spliced, &Block{Words: []Word{
 			{Address: "G", Number: 1},
-		}}
+		}})
+		fmt.Println("len(gc.Blocks) =", len(gc.Blocks))
+		fmt.Println("arc.startBlock =", arc.startBlock)
+		fmt.Println("arc.endBlock =", arc.endBlock)
+		gc.Blocks = append(spliced, gc.Blocks[arc.endBlock+1:]...)
+		startBlock = len(spliced) + 1
 	}
 
 	gc.Write(os.Stdout)
@@ -200,7 +214,7 @@ func ReadGCodeFile(filename string) (*GCodeFile, error) {
 
 	r := regexp.MustCompile(`([A-Z])(\-?[0-9]+\.?[0-9]*)`)
 
-	lines := make(map[int]*Block)
+	blocks := make([]*Block, 0)
 	scanner := bufio.NewScanner(file)
 	lineNumber := 0
 	for scanner.Scan() {
@@ -215,17 +229,17 @@ func ReadGCodeFile(filename string) (*GCodeFile, error) {
 			}
 			l.Words = append(l.Words, Word{Address: address, Number: number})
 		}
-		lines[lineNumber] = l
+		blocks = append(blocks, l)
 		lineNumber++
 
 	}
 
-	log.Printf("Read %d lines.", len(lines))
+	log.Printf("Read %d lines.", len(blocks))
 	if err := scanner.Err(); err != nil {
 		return nil, err
 	}
 
-	return &GCodeFile{Blocks: lines}, nil
+	return &GCodeFile{Blocks: blocks}, nil
 }
 
 func (g *GCodeFile) Write(w io.Writer) error {
@@ -283,13 +297,13 @@ type arcMidpoint struct {
 // then the arc is grown from the midpoint until it becomes too un-arc-like to
 // be considered an arc.
 
-func findArcs(points []point, scale float64) []*arc {
+func findArc(points []point, scale float64) *arc {
 
 	numTests := 4      // how many circles to test
 	thresh := 5.0      // 5.0 is definitely not an arc. This value was determined empirically because I'm a slob
 	growThresh := 0.01 // while in an arc, anything that causes the sd to increase above growThresh terminates the arc
-	growThresh = 0.05
-	arcs := []*arc{}
+	//	growThresh = 0.01
+	//	arcs := []*arc{}
 
 	// first find things that aren't an arc
 	inArc := false
@@ -308,10 +322,16 @@ func findArcs(points []point, scale float64) []*arc {
 		if inArc && (maxDev >= thresh || i == numTestPoints-1) {
 			fmt.Println("=====")
 			inArc = false
+			//arcStart-=2*numTests
+			arcStart += 6
 			mid := arcStart + ((i - arcStart) / 2)
 
-			midpoint := mid + (3*numTests)/2         // hand tuned compensation for algorithm lag
-			width := (i - arcStart) + (5 * numTests) // hand tuned compensation for algorithm sensitivity
+			if mid < 0 {
+				continue
+			}
+
+			midpoint := mid                          //+ (3*numTests)/2     // hand tuned compensation for algorithm lag
+			width := (i - arcStart) + (4 * numTests) // hand tuned compensation for algorithm sensitivity
 
 			bestStart, bestEnd, bestCenter, bestRadius, bestSD := growArc(points, midpoint, width, growThresh)
 			fmt.Printf("TEST %d 0 %d %d %0.8f\n", points[midpoint].lineNumber, bestStart, bestEnd, bestSD)
@@ -344,7 +364,7 @@ func findArcs(points []point, scale float64) []*arc {
 				clockwise:  clockwise,
 				scale:      ScaleMM,
 			}).scaleTo(scale)
-			arcs = append(arcs, arc)
+			return arc
 
 		} else if !inArc && maxDev < thresh {
 			inArc = true
@@ -353,7 +373,7 @@ func findArcs(points []point, scale float64) []*arc {
 
 	}
 
-	return arcs
+	return nil
 }
 
 func growArc(points []point, midpoint int, width int, thresh float64) (int, int, point, float64, float64) {