circlefit/main.go

package main

import (
	"bufio"
	"fmt"
	"io"
	"log"
	"math"
	"os"
	"regexp"
	"strconv"
	"strings"
)

type Word struct {
	Address string
	Number  float64
}

type GCodeFile struct {
	Blocks []*Block
}

type Block struct {
	Words []Word
}

const (
	ScaleInch float64 = 25.4
	ScaleMM   float64 = 1
)

func (b *Block) String() string {
	s := []string{}
	for _, cmd := range b.Words {
		_, frac := math.Modf(cmd.Number)
		if frac == 0 {
			s = append(s, fmt.Sprintf("%s%0.0f", cmd.Address, cmd.Number))
		} else {
			s = append(s, fmt.Sprintf("%s%f", cmd.Address, cmd.Number))
		}
	}
	return strings.Join(s, " ")
}

func (l *Block) Get(Address string) (float64, bool) {
	for _, c := range l.Words {
		if c.Address == Address {
			return c.Number, true
		}
	}
	return 0, false
}

func (l *Block) Has(Address string, Number float64) bool {
	fmt.Println(l)
	for _, c := range l.Words {
		if c.Address == Address && c.Number == Number {
			return true
		}
	}
	return false
}

type point struct {
	x, y       float64
	lineNumber int
	scale      float64
}

func (p point) scaleTo(s float64) point {
	return point{
		x:          p.x * p.scale / s,
		y:          p.y * p.scale / s,
		lineNumber: p.lineNumber,
		scale:      s,
	}
}

func (p point) millimeters() point {
	return p.scaleTo(ScaleMM)
}

func (p point) inches() point {
	return p.scaleTo(ScaleInch)
}

func main() {

	gc, err := ReadGCodeFile("testwrench.nc")
	if err != nil {
		log.Fatal(err)
	}

	mode := ""
	var x, y float64
	var lastPoints []point

	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
				}
			}

			newPt := point{x: x, y: y, lineNumber: lineNumber, scale: scale}

			if zChanged || modeChanged {
				if len(lastPoints) > 0 {
					break
				}
			} else if mode == "g1" && xyChanged {
				lastPoints = append(lastPoints, newPt)
			}

		}

		if len(lastPoints) == 0 {
			done = true
			break
		}

		arc := findArc(lastPoints, scale)

		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)

		spliced := gc.Blocks[:arc.startBlock]

		// G1 to move to the beginning point of the arc
		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
		}
		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
		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)

}

func ReadGCodeFile(filename string) (*GCodeFile, error) {
	file, err := os.Open(filename)
	if err != nil {
		return nil, err
	}
	defer file.Close()

	r := regexp.MustCompile(`([A-Z])(\-?[0-9]+\.?[0-9]*)`)

	blocks := make([]*Block, 0)
	scanner := bufio.NewScanner(file)
	lineNumber := 0
	for scanner.Scan() {
		lineStr := scanner.Text()
		l := &Block{}
		matches := r.FindAllStringSubmatch(lineStr, -1)
		for _, cmd := range matches {
			address := cmd[1]
			number, err := strconv.ParseFloat(cmd[2], 64)
			if err != nil {
				return nil, err
			}
			l.Words = append(l.Words, Word{Address: address, Number: number})
		}
		blocks = append(blocks, l)
		lineNumber++

	}

	log.Printf("Read %d lines.", len(blocks))
	if err := scanner.Err(); err != nil {
		return nil, err
	}

	return &GCodeFile{Blocks: blocks}, nil
}

func (g *GCodeFile) Write(w io.Writer) error {
	maxBlock := 0
	for n := range g.Blocks {
		if n > maxBlock {
			maxBlock = n
		}
	}
	for i := 0; i <= maxBlock; i++ {
		if g.Blocks[i] != nil {
			_, err := fmt.Fprintf(w, "%s\n", g.Blocks[i].String())
			if err != nil {
				return err
			}
		}
	}
	return nil
}

type arc struct {
	startBlock int
	endBlock   int
	startPt    point
	endPt      point
	radius     float64
	centre     point
	clockwise  bool
	bestSD     float64
	scale      float64
}

func (a *arc) scaleTo(s float64) *arc {
	n := *a
	n.startPt = a.startPt.scaleTo(s)
	n.endPt = a.endPt.scaleTo(s)
	n.centre = a.centre.scaleTo(s)
	n.radius = a.radius * a.scale / s
	n.scale = s
	return &n
}

type arcMidpoint struct {
	midpoint int
	width    int
}

// findArcs identifies any arcs contained in the given points, and returns
// them as arc objects. Internally it operates in millimeters, but scales
// all inputs and outputs according to the given scale value
//
// The algorithm works by first locating the mid-points of all arcs. It's easy
// to find things that aren't arcs, so the algorithm assumes that the space
// between two not-arcs is an arc. The midpoint of the not-arc is determined,
// then the arc is grown from the midpoint until it becomes too un-arc-like to
// be considered an 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.01
	//	arcs := []*arc{}

	// first find things that aren't an arc
	inArc := false
	arcStart := 0
	numTestPoints := len(points) - 3*numTests
	for i := 0; i < numTestPoints; i++ {

		vX := make([]float64, numTests)
		vY := make([]float64, numTests)
		vR := make([]float64, numTests)
		for j := 0; j < numTests; j++ {
			vX[j], vY[j], vR[j] = circleFromThreePointsXYR(points[i+j].millimeters(), points[i+j+numTests].millimeters(), points[i+j+(2*numTests)].millimeters())
		}
		maxDev := max([]float64{stdDev(vX), stdDev(vY), stdDev(vR)})

		if inArc && (maxDev >= thresh || i == numTestPoints-1) {
			fmt.Println("=====")
			inArc = false
			//arcStart-=2*numTests
			arcStart += 6
			mid := arcStart + ((i - arcStart) / 2)

			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)

			// for q:=-8;q<=8;q++ {
			// 				for v:=-8;v<=8;v++ {
			// 					bestStart2, bestEnd2, bestCenter2, bestRadius2, bestSD2 := testArc(points, midpoint+v, width+q, bestSD)
			// 					fmt.Printf("TEST mid=%d tweak=%d tweak2=%d start=%d end=%d sd=%0.8f oldbest=%0.8f\n", points[midpoint+v].lineNumber, v, q, bestStart2, bestEnd2, bestSD2, bestSD)
			// 					if bestSD2 < bestSD {
			// 								fmt.Println("POOOP")
			//
			// 						bestStart, bestEnd, bestCenter, bestRadius, bestSD = bestStart2, bestEnd2, bestCenter2, bestRadius2, bestSD2
			// 					}
			// 				}
			// 			}

			startPt := points[bestStart].millimeters()
			endPt := points[bestEnd].millimeters()
			secondPt := points[bestStart+2].millimeters()
			clockwise := ((bestCenter.x-startPt.x)*(secondPt.y-startPt.y) - (bestCenter.y-startPt.y)*(secondPt.x-startPt.x)) > 0

			arc := (&arc{
				startPt:    startPt,
				endPt:      endPt,
				startBlock: startPt.lineNumber,
				endBlock:   endPt.lineNumber,
				centre:     bestCenter,
				radius:     bestRadius,
				bestSD:     bestSD,
				clockwise:  clockwise,
				scale:      ScaleMM,
			}).scaleTo(scale)
			return arc

		} else if !inArc && maxDev < thresh {
			inArc = true
			arcStart = i
		}

	}

	return nil
}

func growArc(points []point, midpoint int, width int, thresh float64) (int, int, point, float64, float64) {
	vX := make([]float64, 0)
	vY := make([]float64, 0)
	vR := make([]float64, 0)
	var bestStart, bestEnd int
	var bestCenter point
	var bestRadius float64

	bestSD := 100.0

	for i := width / 3; i < width/2; i++ {
		lowerPt := midpoint - i
		if lowerPt < 0 {
			lowerPt = 0
		}
		upperPt := midpoint + i
		if upperPt > len(points)-1 {
			upperPt = len(points) - 1
		}
		x, y, r := circleFromThreePointsXYR(points[lowerPt].millimeters(), points[midpoint].millimeters(), points[upperPt].millimeters())
		vX = append(vX, x)
		vY = append(vY, y)
		vR = append(vR, r)
		//	fmt.Printf("lines: %d, %d %d = %0.2f,%0.2f,%0.2f\n", points[lowerPt].lineNumber, points[midpoint.midpoint].lineNumber, points[upperPt].lineNumber, x*scale, y*scale, r*scale)
		//	sd := math.Sqrt(math.Pow(stdDev(vX), 2) + math.Pow(stdDev(vY), 2) + math.Pow(stdDev(vR), 2))

		sd := max([]float64{stdDev(vX), stdDev(vY), stdDev(vR)})

		//fmt.Printf("Averages: %d, %d %d = %0.4f\n", points[lowerPt].lineNumber, points[midpoint.midpoint].lineNumber, points[upperPt].lineNumber, sd)
		if sd > thresh {
			break
		}
		bestStart, bestEnd = lowerPt, upperPt
		bestCenter = point{x: mean(vX), y: mean(vY), scale: ScaleMM}
		bestRadius = mean(vR)
		bestSD = sd

		//	fmt.Printf("Line %d SD=%0.4f start=%d end=%d\n",points[midpoint.midpoint].lineNumber, sd1, points[lowerPt].lineNumber, points[upperPt].lineNumber)

	}

	return bestStart, bestEnd, bestCenter, bestRadius, bestSD
}

func testArc(points []point, midpoint int, width int, bestSD float64) (int, int, point, float64, float64) {
	vX := make([]float64, 0)
	vY := make([]float64, 0)
	vR := make([]float64, 0)
	var bestStart, bestEnd int
	var bestCenter point
	var bestRadius float64

	fmt.Printf("Testing %d points\n", width/2-width/3)
	n := 1
	for i := width / 3; i < width/2; i++ {
		lowerPt := midpoint - i
		if lowerPt < 0 {
			lowerPt = 0
		}
		upperPt := midpoint + i
		if upperPt > len(points)-1 {
			upperPt = len(points) - 1
		}
		x, y, r := circleFromThreePointsXYR(points[lowerPt].millimeters(), points[midpoint].millimeters(), points[upperPt].millimeters())
		vX = append(vX, x)
		vY = append(vY, y)
		vR = append(vR, r)
		fmt.Printf("blocks: %d, %d %d\n", points[lowerPt].lineNumber, points[midpoint].lineNumber, points[upperPt].lineNumber)
		//	sd := math.Sqrt(math.Pow(stdDev(vX), 2) + math.Pow(stdDev(vY), 2) + math.Pow(stdDev(vR), 2))
		sd := max([]float64{stdDev(vX), stdDev(vY), stdDev(vR)})
		fmt.Printf("sds [n=%d]: %0.4f  %0.4f  %0.4f (test=%0.8f, best=%0.8f)\n", n, stdDev(vX), stdDev(vY), stdDev(vR), sd, bestSD)

		//fmt.Printf("Averages: %d, %d %d = %0.4f\n", points[lowerPt].lineNumber, points[midpoint.midpoint].lineNumber, points[upperPt].lineNumber, sd)
		if sd < bestSD && n > 3 {
			bestStart, bestEnd = lowerPt, upperPt
			bestCenter = point{x: mean(vX), y: mean(vY), scale: ScaleMM}
			bestRadius = mean(vR)
			bestSD = sd
		}
		n++
		//	fmt.Printf("Line %d SD=%0.4f start=%d end=%d\n",points[midpoint.midpoint].lineNumber, sd1, points[lowerPt].lineNumber, points[upperPt].lineNumber)

	}

	return bestStart, bestEnd, bestCenter, bestRadius, bestSD
}

func max(num []float64) float64 {
	m := 0.0
	for _, n := range num {
		if n > m {
			m = n
		}
	}
	return m
}

func min(num []float64) float64 {
	m := num[0]
	for _, n := range num {
		if n < m {
			m = n
		}
	}
	return m
}

func mean(num []float64) float64 {
	sum := 0.0
	for _, n := range num {
		sum += n
	}
	return sum / float64(len(num))
}

func stdDev(num []float64) float64 {
	mean := mean(num)
	sd := 0.0
	for _, n := range num {
		sd += math.Pow(n-mean, 2)
	}
	sd = math.Sqrt(sd / float64(len(num)))
	return sd
}

func circleFromThreePointsXYR(p1, p2, p3 point) (float64, float64, float64) {
	a := p1.x*(p2.y-p3.y) - p1.y*(p2.x-p3.x) + p2.x*p3.y - p3.x*p2.y
	b := (math.Pow(p1.x, 2)+math.Pow(p1.y, 2))*(p3.y-p2.y) + (math.Pow(p2.x, 2)+math.Pow(p2.y, 2))*(p1.y-p3.y) + (math.Pow(p3.x, 2)+math.Pow(p3.y, 2))*(p2.y-p1.y)
	c := (math.Pow(p1.x, 2)+math.Pow(p1.y, 2))*(p2.x-p3.x) + (math.Pow(p2.x, 2)+math.Pow(p2.y, 2))*(p3.x-p1.x) + (math.Pow(p3.x, 2)+math.Pow(p3.y, 2))*(p1.x-p2.x)
	x := -1 * b / (2.0 * a)
	y := -1 * c / (2.0 * a)
	r := math.Sqrt(math.Pow((x-p1.x), 2) + math.Pow((y-p1.y), 2))
	//fmt.Println(r)
	return x, y, r
}