initial commit

README.md

@@ -0,0 +1,3 @@
+# circlefit
+
+circlefit analyzes a G-code file, then replaces blocks of successive G1 commands that represent arcs with the appropriate G2 or G3 arc commands.

go.mod

@@ -0,0 +1,12 @@
+module code.beefchicken.com/circlefit
+
+go 1.20
+
+require (
+	bitbucket.wtfast.net/go/fit v1.0.1 // indirect
+	bitbucket.wtfast.net/go/geo v1.0.1 // indirect
+	github.com/256dpi/gcode v0.3.0 // indirect
+	gonum.org/v1/gonum v0.13.0 // indirect
+	google.golang.org/genproto v0.0.0-20230410155749-daa745c078e1 // indirect
+	google.golang.org/protobuf v1.30.0 // indirect
+)

main.go

@@ -0,0 +1,925 @@
+package main
+
+import (
+	"bitbucket.wtfast.net/go/fit"
+	"bufio"
+	"fmt"
+//	"gonum.org/v1/gonum/mat"
+	"log"
+	"math"
+	"os"
+	"regexp"
+	"strconv"
+	"strings"
+)
+
+type command struct {
+	verb string
+	noun float64
+}
+
+type line struct {
+	commands []command
+}
+
+func (l *line) Empty() bool {
+	return len(l.commands) == 0
+}
+
+func (l *line) String() string {
+	s := []string{}
+	for _, cmd := range l.commands {
+		_, frac := math.Modf(cmd.noun)
+		if frac == 0 {
+			s = append(s, fmt.Sprintf("%s%0.0f", cmd.verb, cmd.noun))
+		} else {
+			s = append(s, fmt.Sprintf("%s%f", cmd.verb, cmd.noun))
+		}
+	}
+	return strings.Join(s, " ")
+}
+
+func (l *line) get(verb string) (float64, bool) {
+	for _, c := range l.commands {
+		if c.verb == verb {
+			return c.noun, true
+		}
+	}
+	return 0, false
+}
+
+func (l *line) has(verb string, noun float64) bool {
+	fmt.Println(l)
+	for _, c := range l.commands {
+		if c.verb == verb && c.noun == noun {
+			return true
+		}
+	}
+	return false
+}
+
+type point struct {
+	x, y       float64
+	lineNumber int
+}
+
+func (p *point) IsZero() bool {
+	return p.x == 0 && p.y == 0
+}
+
+var scale = 1.0
+
+func main() {
+	file, err := os.Open("tp3.nc")
+	//file, err := os.Open("tp3.nc")
+	if err != nil {
+		log.Fatal(err)
+	}
+	defer file.Close()
+
+	r := regexp.MustCompile(`([A-Z])(\-?[0-9]+\.?[0-9]*)`)
+
+	//lines := make([]line, 0)
+	lines := make(map[int]*line)
+	scanner := bufio.NewScanner(file)
+	lineNumber := 0
+	for scanner.Scan() {
+		lineStr := scanner.Text()
+		l := &line{}
+		matches := r.FindAllStringSubmatch(lineStr, -1)
+		for _, cmd := range matches {
+			verb := cmd[1]
+			noun, err := strconv.ParseFloat(cmd[2], 64)
+			if err != nil {
+				log.Fatal(err)
+			}
+			l.commands = append(l.commands, command{verb: verb, noun: noun})
+		}
+		lines[lineNumber] = l
+		lineNumber++
+
+	}
+
+	log.Printf("Read %d lines.", len(lines))
+	if err := scanner.Err(); err != nil {
+		log.Fatal(err)
+	}
+
+	mode := ""
+	var x, y float64
+	var lastPoints []point
+	arcs := []*arc{}
+	
+	for lineNumber:=0;lineNumber<len(lines);lineNumber++ {
+		line := lines[lineNumber]
+		xyChanged, zChanged, modeChanged := false, false, false
+		//oldPt := point{x:x,y:y}
+		for _, cmd := range line.commands {
+			oldMode := mode
+			switch cmd.verb {
+			case "G":
+				switch cmd.noun {
+				case 0:
+					mode = "g0"
+				case 1:
+					mode = "g1"
+				case 2:
+					mode = "g2"
+				case 3:
+					mode = "g3"
+				case 20:
+					//fmt.Println("inches!")
+					scale = 25.4
+				case 21:
+					scale = 1
+				}
+			case "X":
+				x = cmd.noun
+				xyChanged = true
+			case "Y":
+				y = cmd.noun
+				xyChanged = true
+			case "Z":
+				//z = cmd.noun
+				zChanged = true
+			}
+			if mode != oldMode {
+				modeChanged = true
+			}
+		}
+
+		newPt := point{x: x, y: y, lineNumber: lineNumber}
+
+		if zChanged || modeChanged {
+			if len(lastPoints) > 0 {
+				arcs = append(arcs, checkPoints4(lastPoints)...)
+				lastPoints = make([]point, 0)
+			}
+		} else if mode == "g1" && xyChanged {
+			lastPoints = append(lastPoints, newPt)
+		}
+
+	}
+
+	if len(lastPoints) > 0 {
+		arcs = append(arcs, checkPoints4(lastPoints)...)
+	}
+	
+
+	// for _, arc := range arcs {
+// 		fmt.Printf("-----------------------------\n")
+// 		fmt.Printf("     Lines: %d-%d\n",arc.startLine, arc.endLine)
+// 		fmt.Printf("    Centre: %0.2f, %0.2f\n", arc.centre.x,arc.centre.y )
+// 		fmt.Printf("        SD: %0.4f\n", arc.bestSD )
+// 	}
+// 	
+	
+	
+	numLines := len(lines)
+	
+	for _, arc := range arcs {
+		// fmt.Println("start", arc.startPt)
+// 		fmt.Println("end", arc.endPt)
+		i := arc.centre.x - arc.startPt.x
+		j := arc.centre.y - arc.startPt.y
+		
+		// fmt.Printf("G0 X%0.4f Y%0.4f\n", arc.startPt.x, arc.startPt.y)
+		//
+		// 		fmt.Printf("G2 I%0.4f J%0.4f X%0.4f Y%0.4f\n", -i, -j, arc.endPt.x, arc.endPt.y)
+
+		// clear out old commands
+		for i := arc.startLine; i <= arc.endLine; i++ {
+			delete(lines, i)
+		}
+
+		//
+		lines[arc.startLine]=&line{commands:[]command{
+			{verb: "G", noun: 1},
+			{verb: "X", noun: arc.startPt.x},
+			{verb: "Y", noun: arc.startPt.y},
+		}}
+		dir := 3.0
+		if arc.clockwise {
+			dir = 2.0
+		}
+		lines[arc.startLine+1]=&line{commands:[]command{
+			{verb: "G", noun: dir},
+			{verb: "I", noun: i},
+			{verb: "J", noun: j},
+			{verb: "X", noun: arc.endPt.x},
+			{verb: "Y", noun: arc.endPt.y},
+		}}
+		lines[arc.startLine+2]=&line{commands:[]command{
+			{verb: "G", noun: 1},
+		}}
+	}
+
+	for i:=0;i<numLines;i++ {
+		if lines[i] != nil {
+			fmt.Println(lines[i].String())
+		}
+	}
+}
+
+// func  (p *cr)dist(n cr) float64 {
+// 	return math.Sqrt(math.Pow((p.r-n.r), 2))
+// }
+
+type arc struct {
+	startLine  int
+	endLine    int
+	startPt    point
+	secondPt   point
+	endPt      point
+	radius     float64
+	centre     point
+	clockwise  bool
+	startIndex int
+	endIndex   int
+	bestSD     float64
+}
+
+type curveInfo struct {
+	midpoint int
+	width int
+}
+
+func checkPoints4(points []point) []*arc {
+
+
+	numTests := 3
+
+	state := "searching"
+	
+	curveMidPoints := []curveInfo{}
+	
+	curveStart := 0
+	
+	for i := 0; i < len(points)-3*numTests; i++ {
+
+	
+		vX := make([]float64, numTests)
+		vY := make([]float64, numTests)
+		vR := make([]float64, numTests)
+		vD := make([]float64, numTests)
+		for j := 0; j < numTests; j++ {
+			vX[j], vY[j], vR[j] = circleFromThreePointsXYR(points[i+j], points[i+j+numTests], points[i+j+(2*numTests)])
+			vX[j] *= scale
+			vY[j] *= scale
+			vR[j] *= scale
+			vD[j] =  (vX[j]-points[i+j].x)*(points[i+j+1].y-points[i+j].y) - (vY[j]-points[i+j].y)*(points[i+j+1].x-points[i+j].x)
+		}
+		sd1 := math.Sqrt(math.Pow(stdDev(vX), 2) + math.Pow(stdDev(vY), 2))
+	//	sd2 := math.Sqrt(math.Pow(stdDev(vR), 2))
+		
+		//fmt.Printf("%d\t%f\n",points[i+9].lineNumber,sd1)
+		
+		if i == len(points)-3*numTests-1 && state == "curve" {
+			sd1 = 100 // kludge
+		} 
+		thresh := 2.5 // 2.5
+		switch state {
+		case "searching":
+			if sd1 >= thresh {
+				state = "deadzone"
+			} else {
+				state = "curve"
+				curveStart = i
+			}
+		case "deadzone":
+			if sd1 < thresh {
+				state = "curve"
+				curveStart = i
+			}
+		case "curve":
+			if sd1 >= thresh {
+				mid := curveStart+((i-curveStart)/2)
+				//fmt.Printf("curve start = %d end = %d mid = %d\n", points[curveStart+9].lineNumber, points[i+9].lineNumber, points[mid+9].lineNumber)
+				curveMidPoints=append(curveMidPoints,curveInfo{
+					midpoint: mid+(3*numTests)/2-1,
+					width:(i-curveStart)+(4*numTests),
+				})
+				state = "deadzone"
+			}
+			 
+		}
+	}
+	//fmt.Println(curveMidPoints)
+	spillover := 0
+	
+	arcs := []*arc{}
+
+
+
+	for _, midpoint := range curveMidPoints {
+		vX := make([]float64, 0)
+		vY := make([]float64, 0)
+		vR := make([]float64, 0)
+
+		
+		var bestStart, bestEnd int 
+		var bestCenter point
+		var bestRadius float64
+		thresh := .01
+		bestSD := 100.0
+		
+		
+	//	fmt.Println("----------")
+		for i:=midpoint.width/3;i<midpoint.width/2+spillover;i++ {
+			lowerPt  := midpoint.midpoint-i
+			if lowerPt < 0 {
+				lowerPt = 0
+			}
+			upperPt := midpoint.midpoint+i
+			if upperPt > len(points)-1  {
+				upperPt = len(points)-1
+			}
+			x,y,r := circleFromThreePointsXYR(points[lowerPt], points[midpoint.midpoint], points[upperPt])
+			vX = append(vX, x*scale)
+			vY = append(vY, y*scale)
+			vR = append(vR, r*scale)
+		//	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)
+			sd1 := math.Sqrt(math.Pow(stdDev(vX), 2) + math.Pow(stdDev(vY), 2) + math.Pow(stdDev(vR), 2))
+			
+			//fmt.Printf("Averages: %d, %d %d = %0.4f\n", points[lowerPt].lineNumber, points[midpoint.midpoint].lineNumber, points[upperPt].lineNumber, sd1)
+			if sd1 > thresh {
+				break
+			}
+			bestStart, bestEnd = lowerPt, upperPt
+			bestCenter = point{x:avg(vX), y:avg(vY)}
+			bestRadius = avg(vR)
+			bestSD = sd1
+			
+		
+		
+		//	fmt.Printf("Line %d SD=%0.4f start=%d end=%d\n",points[midpoint.midpoint].lineNumber, sd1, points[lowerPt].lineNumber, points[upperPt].lineNumber)
+		
+		}
+		
+		
+		arc := arc{
+			startPt:   points[bestStart],
+			secondPt:  points[bestStart+2],
+			endPt:     points[bestEnd],
+			startLine: points[bestStart].lineNumber,
+			endLine:   points[bestEnd].lineNumber,
+			startIndex: bestStart,
+			endIndex:  bestEnd,
+			centre:    point{x:bestCenter.x/scale,y:bestCenter.y/scale},
+			radius:    bestRadius/scale,
+			bestSD:    bestSD,
+		}
+		d := (arc.centre.x-arc.startPt.x)*(arc.secondPt.y-arc.startPt.y) - (arc.centre.y-arc.startPt.y)*(arc.secondPt.x-arc.startPt.x)
+		arc.clockwise = d > 0
+			//	fmt.Println("QQQ", bestStart, bestEnd, d, arc.clockwise )
+
+		arcs = append(arcs, &arc)
+	}
+return arcs
+}
+
+
+func checkPoints3(points []point) []*arc {
+
+
+	numTests := 3
+	//foo(points)
+	
+	arcs := []*arc{}
+
+	offset := 0
+	searching := true
+	for searching {
+		if len(points)-offset < 3*numTests {
+			searching = false
+			break
+		}
+
+		arcStart := 0
+		var cX, cY, radius float64
+
+		// find start of a circle
+		
+		foundArc := false
+		for i := offset; i < len(points)-3*numTests; 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], points[i+j+numTests], points[i+j+(2*numTests)])
+				vX[j] *= scale
+				vY[j] *= scale
+				vR[j] *= scale
+			}
+
+			sd := math.Sqrt(math.Pow(stdDev(vX), 2) + math.Pow(stdDev(vY), 2) + math.Pow(stdDev(vR), 2))
+
+			if sd < 1 {
+				// looks like a circle
+				fmt.Printf("Found candidate arc starting at %d (line %d) (SD=%0.4f)\n", i, points[i].lineNumber+1, sd)
+				arcStart = i
+				cX = avg(vX)
+				cY = avg(vY)								
+				fmt.Println(vR)
+				radius = avg(vR)
+				foundArc = true
+				break
+			}
+		}
+		
+		if !foundArc {
+			searching = false
+			break
+		}
+
+		arcSize := numTests*3 // size of arc that came from the search above
+
+		
+		
+		var sd float64
+		
+		mX := []float64{}
+		mY := []float64{}
+		mR := []float64{}
+		
+		for i := arcStart; i < len(points)-(numTests*3); i++ {
+			x, y, r := circleFromThreePointsXYR(points[arcStart], points[arcStart+(arcSize)/2], points[arcStart+arcSize])
+		
+			x *= scale
+			y *= scale
+			r *= scale
+				
+			mX = append(mX, x)
+			mY = append(mY, y)
+			mR = append(mR, r)
+		
+		
+			sd = math.Sqrt(math.Pow(avg(mX)-x, 2) + math.Pow(avg(mY)-y, 2) + math.Pow(avg(mR)-r, 2))
+			sd = math.Sqrt(math.Pow(cX-x, 2) + math.Pow(cY-y, 2) + math.Pow(radius-r, 2))
+			fmt.Printf("testing %d-%d-%d (SD=%0.2f\n", arcStart, arcStart+(arcSize)/2, arcStart+arcSize, sd)
+			fmt.Printf("   center 1 X=%0.2f, Y=%0.2f\n", points[arcStart].x, points[arcStart].y)
+			fmt.Printf("   center 2 X=%0.2f, Y=%0.2f\n", points[arcStart+(arcSize)/2].x, points[arcStart+(arcSize)/2].y)
+			fmt.Printf("   center 3 X=%0.2f, Y=%0.2f\n", points[arcStart+arcSize].x, points[arcStart+arcSize].y)
+			
+			if sd >= .4 {
+				break
+			}
+			
+			arcSize++
+		}
+		
+		cX = avg(mX[:len(mX)-1])
+		cY = avg(mY[:len(mY)-1])
+		radius = avg(mR[:len(mR)-1])
+		
+		arcStart++
+		
+
+		if arcStart+arcSize > len(points) {
+			searching = false
+			break
+		}
+		
+		// 	if arcStart+arcSize == len(points) {
+// 				// end condition
+		if arcSize > 0 {
+			fmt.Printf("Found arc: %d to %d, refining (line %d to %d)...\n", arcStart, arcStart+arcSize-1, points[arcStart].lineNumber+1, points[arcStart+arcSize-1].lineNumber+1)
+			bestStart, bestEnd, _, bestRadius, bestCenter := refineArc(points, sd, arcStart, arcSize-1, cX, cY, radius)
+			arc := arc{
+				startPt:   points[bestStart-1],
+				secondPt:  points[bestStart],
+				endPt:     points[bestEnd],
+				startLine: points[bestStart].lineNumber,
+				endLine:   points[bestEnd].lineNumber,
+				centre:    point{x:bestCenter.x/scale,y:bestCenter.y/scale},
+				radius:    bestRadius/scale,
+			}
+			d := (arc.centre.x-arc.startPt.x)*(arc.secondPt.y-arc.startPt.y) - (arc.centre.y-arc.startPt.y)*(arc.secondPt.x-arc.startPt.x)
+			fmt.Println("d=",d)
+			arc.clockwise = d > 0
+			
+			arcs = append(arcs, &arc)
+			
+			offset = bestEnd 
+			fmt.Printf("Refined arc: %d to %d (Radius=%0.2f)\n", bestStart, bestEnd, bestRadius)
+			if arcStart+arcSize == len(points)-1 {
+				searching = false
+			}
+		}
+				
+// 			}
+
+		//}
+	}
+
+	for _, arc := range arcs {
+		d := (arc.centre.x-arc.startPt.x)*(arc.secondPt.y-arc.startPt.y) - (arc.centre.y-arc.startPt.y)*(arc.secondPt.x-arc.startPt.x)
+		arc.clockwise = d > 1
+	}
+
+	return arcs
+}
+
+
+// func foo(points []point) {
+// 		
+// 		
+// 	arcStart := 0
+// 	arcEnd  := 0
+// 	centre := point{}
+// 	
+// 	// find start of a circle
+// 	for ; arcStart < len(points)-9; arcStart++ {
+// 		
+// 		vX := make([]float64, 3)
+// 		vY := make([]float64, 3)
+// 		vR := make([]float64, 3)
+// 		for j := 0; j < 3; j++ {
+// 			vX[j], vY[j], vR[j] = circleFromThreePointsXYR(points[arcStart+j], points[arcStart+j+3], points[arcStart+j+6])
+// 		}
+// 
+// 		sd := math.Sqrt(math.Pow(stdDev(vX), 2) + math.Pow(stdDev(vY), 2) + math.Pow(stdDev(vR), 2))
+// 
+// 		if sd < 1 {
+// 			// looks like a circle
+// 			fmt.Printf("Found candidate arc starting at %d (SD=%0.4f)\n", arcStart, sd)
+// 			centre = point{x:avg(vX), y: avg(vY)}
+// 			arcEnd = arcStart+9
+// 		//	radius = avg(vR)
+// 			break
+// 		}
+// 	}
+// 		
+// 		
+// 		
+// 		
+// 		
+// 	for ;arcEnd<len(points)-9; arcEnd++ {
+// 	
+// 
+// 		centre2, chi2 := refineCenter(points[arcStart:arcEnd+1], centre)
+// 		fmt.Println("foo", chi2, centre, centre2, len(points[arcStart:arcEnd+1])	)
+// 		
+// 	}	
+// // 		
+// // 		arcEnd := arcStart+9
+// // 		for ; arcEnd < len(points)-9; arcEnd++ {
+// // 			centre, chi2 := refineCenter(points[arcStart:arcEnd+1], point{x:cX,y:cY})
+// // 			fmt.Println(centre, chi2)
+// // 			if chi2 > 1 {
+// // 				break
+// // 			}
+// // 		}
+// // 		arcEnd--
+// // 		
+// // 		centre, chi2 := refineCenter(points[arcStart:arcEnd+1], point{x:cX,y:cY})
+// // 
+// // 		fmt.Printf("Found arc: %d to %d (SD=%0.4f)\n", arcStart, arcEnd, chi2)
+// // 		arcs = append(arcs, &arc{
+// // 			startPt:   points[arcStart],
+// // 			secondPt:  points[arcStart+1],
+// // 			endPt:     points[arcEnd],
+// // 			startLine: points[arcStart].lineNumber,
+// // 			endLine:   points[arcEnd].lineNumber,
+// // 			centre:    centre,
+// // 			//radius:    bestRadius,
+// // 		})
+// // 		offset = arcEnd + 1		
+// }
+
+
+func refineArc(points []point, sd float64, arcStart, arcSize int, cX, cY, cR float64) (int, int, float64, float64, point) {
+
+
+	return arcStart, arcStart+arcSize-1, sd, cR, point{x:cX,y:cY}
+
+
+	var ax, ay, ar float64
+	n := 0
+	
+	for i:=arcStart+1; i<arcStart+(arcSize-1)/3; i++ {
+		//fmt.Println("offsets", i, i+(arcSize-1)/3, i+((arcSize-1)/3)*2)
+		x, y, r := circleFromThreePointsXYR(points[i], points[i+(arcSize-1)/3], points[i+((arcSize-1)/3)*2])
+	//	fmt.Println(x,y,r)
+		ax += x
+		ay += y
+		ar += r 
+		n++
+	}
+	
+	// fmt.Println("before", cX, cY, cR)
+// 	fmt.Println("average", ax/float64(n), ay/float64(n), ar/float64(n))
+// 	
+	return arcStart, arcStart+arcSize-1, sd, ar/float64(n), point{x:ax/float64(n),y:ay/float64(n)}
+	
+	//return arcStart, arcStart+arcSize-1, sd, cR, point{x:cX,y:cY}
+
+
+	bestSD := sd
+	bestEnd := 0
+	bestStart := 0
+	bestRadius := 0.0
+	bestCenter := point{}
+
+	refineDistance := 5
+
+	for startOffset := 0; startOffset < refineDistance; startOffset++ {
+		for endOffset := 0; endOffset < refineDistance; endOffset++ {
+
+			//fmt.Println("testing end:", arcStart+arcSize-q)
+
+			sd2, r2, c2, ok := checkArc(points[arcStart+startOffset], points[arcStart+(arcSize-1)/2], points[arcStart+(arcSize-1)-endOffset], cX, cY, cR)
+			if ok {
+				//	fmt.Println("GGG",sd2,r2,(arcSize-endOffset)-startOffset)
+			}
+			if ok && sd2 < bestSD {
+				bestSD = sd2
+				bestEnd = arcStart + arcSize - 1 - endOffset
+				bestStart = arcStart + startOffset
+				bestRadius = r2
+				bestCenter = c2
+			}
+		}
+	}
+	return bestStart, bestEnd, bestSD, bestRadius, bestCenter
+}
+
+func checkArc(p1, p2, p3 point, cX, cY, cR float64) (float64, float64, point, bool) {
+	if p1 == p2 || p1 == p3 || p2 == p3 {
+		return 0, 0, point{}, false
+	}
+	x2, y2, r2 := circleFromThreePointsXYR(p1, p2, p3)
+	sd2 := math.Sqrt(math.Pow(cX-x2, 2) + math.Pow(cY-y2, 2) + math.Pow(cR-r2, 2))
+	return sd2, r2, point{x: x2, y: y2}, true
+}
+
+// func checkPoints(points []point) {
+//
+// 	if len(points) < 3 {
+// 		return
+// 	}
+//
+// 	fmt.Println("LEN",len(points))
+//
+// 	crs := make([]cr,0)
+//
+//
+// 	for i:=0;i<len(points)-3;i++ {
+// 		crs = append(crs, circleFromThreePointsCr(points[i], points[i+1], points[i+2]))
+// 	}
+//
+// // 	for _, v := range crs {
+// // 		fmt.Print("source x: ", v.x)
+// // 		fmt.Print(" y: ", v.y)
+// // 		fmt.Println(" r: ", v.r)
+// // 	}
+// //
+//
+// //
+// // 	circlePoints := make([]cr,3) // start a new circle
+// // 	copy(circlePoints, crs[:3])
+// //
+// //
+// 	arcStart := 0
+// 	circleEnd := 3
+//
+// 	for i:=circleEnd;i<len(crs); {
+//
+// 		np := crs[i]
+//
+// 	// 	if circleEnd-arcStart < 3 {
+// // 			fmt.Printf("circle is too small (%d-%d)\n", arcStart, circleEnd)
+// // 			i++
+// // 			circleEnd = i
+// //
+// // 			continue
+// // 		}
+//
+// 		fmt.Println("====================")
+//
+// 		circlePoints := crs[arcStart:circleEnd]
+//
+// 		// for _, v := range circlePoints {
+// // 			fmt.Print("existing x: ", v.x)
+// // 			fmt.Print(" y: ", v.y)
+// // 			fmt.Println(" r: ", v.r)
+// // 		}
+//
+//
+// 		yVals := make([]float64, len(circlePoints))
+// 		for j :=0; j<len(circlePoints)-1;j++ {
+// 			yVals[j] = np.dist(circlePoints[j])
+// 		}
+// 		sd := stdDev(yVals)
+//
+// 		fmt.Print("testing x: ", np.x)
+// 		fmt.Print(" y: ", np.y)
+// 		fmt.Println(" r: ", np.r)
+//
+// 		fmt.Println("STDDEV:",sd)
+//
+//
+// 		if sd <  7 {
+// 			i++
+// 			circleEnd++
+// 			fmt.Printf("  growing to %d-%d\n", arcStart, circleEnd)
+// 			// fits the existing circle, so add it
+//
+// 		} else {
+//
+// 			dumpPoints (crs[arcStart:circleEnd])
+//
+// 			fmt.Printf("  discarding %d-%d\n", arcStart, circleEnd)
+// 			i++
+// 			arcStart=i
+// 			circleEnd=arcStart+3
+//
+// 			fmt.Printf("  starting new circle at %d-%d\n", arcStart, circleEnd)
+// 			// no fit, start a new circle
+// 			// crs[i+1].dump()
+// // 			crs[i].dump()
+// // 			crs[i-1].dump()
+// // 			crs[i-2].dump()
+//
+//
+// 		}
+//
+//
+// 	}
+//
+// 				dumpPoints (crs[arcStart:circleEnd])
+//
+// 	fmt.Printf("  discarding final %d-%d\n", arcStart, circleEnd)
+//
+// }
+//
+// func dumpPoints (circlePoints []cr) {
+// 	for _, v := range circlePoints {
+// 		fmt.Print("existing x: ", v.x)
+// 		fmt.Print(" y: ", v.y)
+// 		fmt.Println(" r: ", v.r)
+// 	}
+// }
+//
+// func (cr *cr) dump() {
+// 	fmt.Println("xxx OVER",cr.p1, cr.p2, cr.p3)
+// 	// fmt.Println("xxx OVER p1-p2",cr.p1.distance(&cr.p2))
+// // 	fmt.Println("xxx OVER p2-p3",cr.p2.distance(&cr.p3))
+// }
+//
+//
+//
+func refineCenter(points []point, centre point) (point, float64) {
+	
+//	fmt.Println("LEN",len(points), points)
+	
+	xVals := &fit.CartesianModel{}
+	
+	for _, pt := range points {
+		xVals.Points = append(xVals.Points, fit.Point{X:pt.x, Y:pt.y})
+	}
+	
+	yVals := []float64{}
+	for _, pt := range points {
+		yVals = append(yVals, pt.distance(&centre))
+	}
+	
+// 	//fmt.Println(yVals)
+	initialParams := make([]float64, 2)
+	for _, point := range xVals.Points {
+		initialParams[0] += point.X
+		initialParams[1] += point.Y
+	}
+	initialParams[0] /= float64(xVals.Len())
+	initialParams[1] /= float64(xVals.Len())
+// 	initialParams[0] = centre.x
+// 	initialParams[1] = centre.y
+	
+	
+	f, err := fit.NewFitter(xVals, yVals, nil, initialParams, fit.Options{fit.OptionDebug: false})
+	if err != nil {
+		panic(err)
+	}
+// 	stopReason := f.Run()
+// 	fmt.Println("foo Done! Stop reason: ", stopReason)
+	out := f.Params()
+
+	// fmt.Println(" x: ", out[0])
+// 	fmt.Println(" y: ", out[1])
+// 	fmt.Println(" cX: ", centre.x)
+// 	fmt.Println(" cY: ", centre.y)
+// 	fmt.Println(" chi2: ", f.Chi2())
+//	fmt.Println(" Rersiduals: ", f.Residuals())
+ 	return point{x:out[0],y:out[1]}, f.Chi2()
+
+}
+
+
+
+func stdDev(num []float64) float64 {
+	sum := 0.0
+	for _, n := range num {
+		sum += n
+	}
+	mean := sum / float64(len(num))
+	sd := 0.0
+	for _, n := range num {
+		sd += math.Pow(n-mean, 2)
+	}
+	sd = math.Sqrt(sd / float64(len(num)))
+	return sd
+}
+
+func avg(num []float64) float64 {
+	sum := 0.0
+	for _, n := range num {
+		sum += n
+	}
+	return sum / float64(len(num))
+}
+
+func circleFromThreePointsXYR(p1 point, p2 point, 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))
+	return x, y, r
+}
+
+//
+// func circleFromThreePointsCr(p1 point, p2 point, p3 point) cr {
+// 	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))
+// 	return cr{p1: p1, p2:p2, p3:p3, x: x, y: y, r: r}
+// }
+//
+//
+//
+//
+// func circleFromThreePoints(p1 point, p2 point, p3 point) (point, 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))
+// 	return point{x: x, y: y}, r
+// }
+//
+//
+//
+//
+
+func (p *point) distance(p2 *point) float64 {
+	return math.Sqrt(math.Pow(p.x-p2.x, 2) + math.Pow(p.y-p2.y, 2))
+}
+
+func (p *point) bearing(p2 *point) float64 {
+	return math.Atan2(p.y-p2.y, p.x-p2.x)
+}
+
+// type cr struct {
+// 	p1, p2, p3 point
+// 	x, y, r    float64
+// }
+// 
+// var _ fit.Model = &CircleModel{}
+// 
+// type CircleModel struct {
+// 	points []cr
+// }
+// 
+// func NewCircleModel() *CircleModel {
+// 	return &CircleModel{
+// 		points: make([]cr, 0),
+// 	}
+// }
+// func (c *CircleModel) Len() int {
+// 	return len(c.points)
+// }
+// 
+// func (c *CircleModel) Model(i int, params *mat.VecDense) float64 {
+// 	p := c.points[i]
+// 	return math.Sqrt(math.Pow((p.x-params.AtVec(0)), 2) + math.Pow((p.y-params.AtVec(1)), 2) + math.Pow((p.r-params.AtVec(2)), 2))
+// }
+// 
+// func (c *CircleModel) InitialParams() []float64 {
+// 	initialParams := make([]float64, 3)
+// 	if len(c.points) > 0 {
+// 		for _, p := range c.points {
+// 			initialParams[0] += p.x
+// 			initialParams[1] += p.y
+// 			initialParams[2] += p.r
+// 		}
+// 		initialParams[0] /= float64(len(c.points))
+// 		initialParams[1] /= float64(len(c.points))
+// 		initialParams[2] /= float64(len(c.points))
+// 	}
+// 	return initialParams
+// }