seems to be working nice

main.go

@@ -3,6 +3,7 @@ package main
 import (
 	"bufio"
 	"fmt"
+	"io"
 	"log"
 	"math"
 	"os"
@@ -11,45 +12,50 @@ import (
 	"strings"
 )
 
-type command struct {
-	verb string
-	noun float64
+type Word struct {
+	Address string
+	Number  float64
 }
 
-type line struct {
-	commands []command
+type GCodeFile struct {
+	Blocks map[int]*Block
 }
 
-func (l *line) Empty() bool {
-	return len(l.commands) == 0
+type Block struct {
+	Words []Word
 }
 
-func (l *line) String() string {
+const (
+	ScaleInch float64 = 25.4
+	ScaleMM   float64 = 1
+)
+
+func (b *Block) String() string {
 	s := []string{}
-	for _, cmd := range l.commands {
-		_, frac := math.Modf(cmd.noun)
+	for _, cmd := range b.Words {
+		_, frac := math.Modf(cmd.Number)
 		if frac == 0 {
-			s = append(s, fmt.Sprintf("%s%0.0f", cmd.verb, cmd.noun))
+			s = append(s, fmt.Sprintf("%s%0.0f", cmd.Address, cmd.Number))
 		} else {
-			s = append(s, fmt.Sprintf("%s%f", cmd.verb, cmd.noun))
+			s = append(s, fmt.Sprintf("%s%f", cmd.Address, cmd.Number))
 		}
 	}
 	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
+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 *line) has(verb string, noun float64) bool {
+func (l *Block) Has(Address string, Number float64) bool {
 	fmt.Println(l)
-	for _, c := range l.commands {
-		if c.verb == verb && c.noun == noun {
+	for _, c := range l.Words {
+		if c.Address == Address && c.Number == Number {
 			return true
 		}
 	}
@@ -59,63 +65,47 @@ func (l *line) has(verb string, noun float64) bool {
 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) IsZero() bool {
-	return p.x == 0 && p.y == 0
+func (p point) millimeters() point {
+	return p.scaleTo(ScaleMM)
 }
 
-var scale = 1.0
+func (p point) inches() point {
+	return p.scaleTo(ScaleInch)
+}
 
 func main() {
-	file, err := os.Open("tp3.nc")
 
+	gc, err := ReadGCodeFile("testwrench.nc")
 	if err != nil {
 		log.Fatal(err)
 	}
-	defer file.Close()
-
-	r := regexp.MustCompile(`([A-Z])(\-?[0-9]+\.?[0-9]*)`)
-
-	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{}
+	scale := ScaleMM
 
-	for lineNumber := 0; lineNumber < len(lines); lineNumber++ {
-		line := lines[lineNumber]
+	for lineNumber := 0; lineNumber < len(gc.Blocks); lineNumber++ {
+		block := gc.Blocks[lineNumber]
 		xyChanged, zChanged, modeChanged := false, false, false
-		//oldPt := point{x:x,y:y}
-		for _, cmd := range line.commands {
+		for _, cmd := range block.Words {
 			oldMode := mode
-			switch cmd.verb {
+			switch cmd.Address {
 			case "G":
-				switch cmd.noun {
+				switch cmd.Number {
 				case 0:
 					mode = "g0"
 				case 1:
@@ -125,19 +115,18 @@ func main() {
 				case 3:
 					mode = "g3"
 				case 20:
-					//fmt.Println("inches!")
-					scale = 25.4
+					scale = ScaleInch
 				case 21:
-					scale = 1
+					scale = ScaleMM
 				}
 			case "X":
-				x = cmd.noun
+				x = cmd.Number
 				xyChanged = true
 			case "Y":
-				y = cmd.noun
+				y = cmd.Number
 				xyChanged = true
 			case "Z":
-				//z = cmd.noun
+				//z = cmd.Number
 				zChanged = true
 			}
 			if mode != oldMode {
@@ -145,11 +134,11 @@ func main() {
 			}
 		}
 
-		newPt := point{x: x, y: y, lineNumber: lineNumber}
+		newPt := point{x: x, y: y, lineNumber: lineNumber, scale: scale}
 
 		if zChanged || modeChanged {
 			if len(lastPoints) > 0 {
-				arcs = append(arcs, checkPoints4(lastPoints)...)
+				arcs = append(arcs, findArcs(lastPoints, scale)...)
 				lastPoints = make([]point, 0)
 			}
 		} else if mode == "g1" && xyChanged {
@@ -159,211 +148,331 @@ func main() {
 	}
 
 	if len(lastPoints) > 0 {
-		arcs = append(arcs, checkPoints4(lastPoints)...)
+		arcs = append(arcs, findArcs(lastPoints, scale)...)
 	}
 
-	// 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 {
-		i := arc.centre.x - arc.startPt.x
-		j := arc.centre.y - arc.startPt.y
+
+		i := (arc.centre.x - arc.startPt.x)
+		j := (arc.centre.y - arc.startPt.y)
 
 		// clear out old commands
-		for i := arc.startLine; i <= arc.endLine; i++ {
-			delete(lines, i)
+		for i := arc.startBlock; i <= arc.endBlock; i++ {
+			delete(gc.Blocks, i)
 		}
 
-		//
-		lines[arc.startLine] = &line{commands: []command{
-			{verb: "G", noun: 1},
-			{verb: "X", noun: arc.startPt.x},
-			{verb: "Y", noun: arc.startPt.y},
+		// G1 to move to the beginning point of the arc
+		gc.Blocks[arc.startBlock] = &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
 		}
-		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},
+		gc.Blocks[arc.startBlock+1] = &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},
 		}}
-		lines[arc.startLine+2] = &line{commands: []command{
-			{verb: "G", noun: 1},
+
+		// shove in a G1 for the surviving X and Y commands after the arc
+		gc.Blocks[arc.startBlock+2] = &Block{Words: []Word{
+			{Address: "G", Number: 1},
 		}}
 	}
 
-	for i := 0; i < numLines; i++ {
-		if lines[i] != nil {
-			fmt.Println(lines[i].String())
+	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]*)`)
+
+	lines := make(map[int]*Block)
+	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})
 		}
+		lines[lineNumber] = l
+		lineNumber++
+
 	}
+
+	log.Printf("Read %d lines.", len(lines))
+	if err := scanner.Err(); err != nil {
+		return nil, err
+	}
+
+	return &GCodeFile{Blocks: lines}, nil
 }
 
-// func  (p *cr)dist(n cr) float64 {
-// 	return math.Sqrt(math.Pow((p.r-n.r), 2))
-// }
+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 {
-	startLine  int
-	endLine    int
+	startBlock int
+	endBlock   int
 	startPt    point
-	secondPt   point
 	endPt      point
 	radius     float64
 	centre     point
 	clockwise  bool
-	startIndex int
-	endIndex   int
 	bestSD     float64
+	scale      float64
 }
 
-type curveInfo struct {
+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
 }
 
-func checkPoints4(points []point) []*arc {
-
-	numTests := 3
-
-	state := "searching"
-
-	curveMidPoints := []curveInfo{}
-
-	curveStart := 0
+// 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 findArcs(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{}
 
-	for i := 0; i < len(points)-3*numTests; i++ {
+	// 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)
-		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)
+			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
+			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
+
+			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)
+			arcs = append(arcs, arc)
+
+		} else if !inArc && maxDev < thresh {
+			inArc = true
+			arcStart = i
 		}
-		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"
-			}
+	return arcs
+}
 
+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)
+
 	}
-	//fmt.Println(curveMidPoints)
-	spillover := 0
 
-	arcs := []*arc{}
+	return bestStart, bestEnd, bestCenter, bestRadius, bestSD
+}
 
-	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
-			}
+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: avg(vX), y: avg(vY)}
-			bestRadius = avg(vR)
-			bestSD = sd1
+			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)
 
-			//	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
+}
 
-		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,
+func max(num []float64) float64 {
+	m := 0.0
+	for _, n := range num {
+		if n > m {
+			m = n
 		}
-		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 )
+	}
+	return m
+}
 
-		arcs = append(arcs, &arc)
+func min(num []float64) float64 {
+	m := num[0]
+	for _, n := range num {
+		if n < m {
+			m = n
+		}
 	}
-	return arcs
+	return m
 }
 
-func stdDev(num []float64) float64 {
+func mean(num []float64) float64 {
 	sum := 0.0
 	for _, n := range num {
 		sum += n
 	}
-	mean := sum / float64(len(num))
+	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)
@@ -372,20 +481,13 @@ func stdDev(num []float64) float64 {
 	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)
+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
 }