Former/face_reconstruct.go

package main

import (
	"fmt"
	"math"
	"os"
	"path/filepath"
	"sort"
	"strings"
)

// FaceAdjacency describes which faces share edges.
// Key: face number, Value: list of {neighbor face num, edge index on this face, edge index on neighbor}.
type FaceAdjacency map[int][]EdgeLink

type EdgeLink struct {
	Neighbor     int // neighbor face number
	EdgeIdx      int // edge index on this face (0 = edge from vertex 0→1)
	NeighborEdge int // edge index on the neighbor face
}

// ReconstructedObject holds the 3D reconstruction from traced faces.
type ReconstructedObject struct {
	Faces       []TracedFace
	Adjacency   FaceAdjacency
	Vertices3D  [][3]float64 // unique 3D vertices
	FaceIndices [][]int      // per-face vertex index lists
	SCAD        string       // generated SCAD source
}

// faceEdgeInfo holds parallelogram parameters extracted from a traced face.
type faceEdgeInfo struct {
	len1, len2 float64 // edge lengths, len1 >= len2
	angle      float64 // angle between them in radians, normalized to (0, π/2]
}

// InferCuboid reconstructs a 6-faced solid from traced face outlines.
// Fits quadrilateral corners to each face, extracts edge lengths and angles,
// and builds a parallelepiped (generalized cuboid with non-right angles).
func InferCuboid(faces []TracedFace, totalFaces int) (*ReconstructedObject, string, error) {
	if totalFaces != 6 {
		return nil, "", fmt.Errorf("cuboid inference requires 6-faced object")
	}
	if len(faces) < 1 {
		return nil, "", fmt.Errorf("need at least 1 face")
	}

	var infos []faceEdgeInfo
	for _, f := range faces {
		info := measureFaceEdges(f.Outline)
		infos = append(infos, info)
		debugLog("InferCuboid: face %d → %.1f × %.1f mm, angle=%.1f°",
			f.FaceNum, info.len1, info.len2, info.angle*180/math.Pi)
	}

	if len(faces) < 6 {
		var dims [][2]float64
		for _, info := range infos {
			dims = append(dims, [2]float64{info.len1, info.len2})
		}
		W, H, D := fitCuboidPartial(dims)
		return buildRectCuboidResult(W, H, D, faces)
	}

	assign := fitParallelepipedAssignment(infos)
	debugLog("InferCuboid: assignment err=%.1f, W=%.1f H=%.1f D=%.1f",
		assign.err, assign.W, assign.H, assign.D)
	debugLog("InferCuboid: angles AB=%.1f° AC=%.1f° BC=%.1f°",
		assign.angleAB*180/math.Pi, assign.angleAC*180/math.Pi, assign.angleBC*180/math.Pi)

	va, vb, vc := reconstructEdgeVectors(assign)
	debugLog("InferCuboid: vectors a=(%.2f,%.2f,%.2f) b=(%.2f,%.2f,%.2f) c=(%.2f,%.2f,%.2f)",
		va[0], va[1], va[2], vb[0], vb[1], vb[2], vc[0], vc[1], vc[2])

	verts := parallelepipedVerts(va, vb, vc)
	faceIdxs := parallelepipedFaceIndices()
	scad := buildParallelepipedSCAD(verts, faceIdxs)

	obj := &ReconstructedObject{
		Faces:       faces,
		Vertices3D:  verts,
		FaceIndices: faceIdxs,
		SCAD:        scad,
	}
	msg := fmt.Sprintf("parallelepiped: %.1f × %.1f × %.1f mm (angles %.0f° %.0f° %.0f°) from %d faces",
		assign.W, assign.H, assign.D,
		assign.angleAB*180/math.Pi, assign.angleAC*180/math.Pi, assign.angleBC*180/math.Pi,
		len(faces))
	return obj, msg, nil
}

// GuessFacePairing returns the auto-detected best pairing of 6 faces into
// 3 opposite pairs (WH, WD, HD). Returns face numbers (not indices).
func GuessFacePairing(faces []TracedFace) *FacePairingJS {
	if len(faces) < 6 {
		return nil
	}
	var infos []faceEdgeInfo
	for _, f := range faces {
		infos = append(infos, measureFaceEdges(f.Outline))
	}
	assign := fitParallelepipedAssignment(infos)
	return &FacePairingJS{
		Pair1: [2]int{faces[assign.whPair[0]].FaceNum, faces[assign.whPair[1]].FaceNum},
		Pair2: [2]int{faces[assign.wdPair[0]].FaceNum, faces[assign.wdPair[1]].FaceNum},
		Pair3: [2]int{faces[assign.hdPair[0]].FaceNum, faces[assign.hdPair[1]].FaceNum},
	}
}

// quadMeasure holds full edge info for a traced quad (not reduced to parallelogram).
type quadMeasure struct {
	corners [4][2]float64
	edges   [4]float64 // |c0→c1|, |c1→c2|, |c2→c3|, |c3→c0|
	height  float64    // perpendicular distance between the most-parallel pair of opposite edges
	topLen  float64    // shorter of the two parallel edges (top of trapezoid)
	botLen  float64    // longer of the two parallel edges (bottom of trapezoid)
	isRect  bool       // both opposite edge pairs roughly equal
}

func measureQuad(outline [][2]float64) quadMeasure {
	c := fitQuadCorners(outline)
	var q quadMeasure
	q.corners = c
	for i := 0; i < 4; i++ {
		j := (i + 1) % 4
		dx := c[j][0] - c[i][0]
		dy := c[j][1] - c[i][1]
		q.edges[i] = math.Sqrt(dx*dx + dy*dy)
	}

	// Check how parallel each pair of opposite edges is.
	// Pair A: edge 0 (c0→c1) vs edge 2 (c2→c3)
	// Pair B: edge 1 (c1→c2) vs edge 3 (c3→c0)
	perpDist := func(a0, a1, b0, b1 [2]float64) float64 {
		// Average perpendicular distance between two line segments
		dx := a1[0] - a0[0]
		dy := a1[1] - a0[1]
		l := math.Sqrt(dx*dx + dy*dy)
		if l < 1e-9 {
			return 0
		}
		nx, ny := -dy/l, dx/l
		d1 := (b0[0]-a0[0])*nx + (b0[1]-a0[1])*ny
		d2 := (b1[0]-a0[0])*nx + (b1[1]-a0[1])*ny
		return math.Abs(d1+d2) / 2
	}

	hA := perpDist(c[0], c[1], c[3], c[2])
	hB := perpDist(c[1], c[2], c[0], c[3])

	// A trapezoid has one pair of parallel sides: pick the pair with more similar directions
	dir := func(a, b [2]float64) [2]float64 {
		dx := b[0] - a[0]
		dy := b[1] - a[1]
		l := math.Sqrt(dx*dx + dy*dy)
		if l < 1e-9 {
			return [2]float64{1, 0}
		}
		return [2]float64{dx / l, dy / l}
	}
	d0 := dir(c[0], c[1])
	d2 := dir(c[2], c[3])
	d1 := dir(c[1], c[2])
	d3 := dir(c[3], c[0])
	// Parallelism = |dot product| (1 = perfectly parallel)
	parA := math.Abs(d0[0]*d2[0] + d0[1]*d2[1])
	parB := math.Abs(d1[0]*d3[0] + d1[1]*d3[1])

	diff02 := math.Abs(q.edges[0]-q.edges[2]) / math.Max(q.edges[0], q.edges[2]+1e-9)
	diff13 := math.Abs(q.edges[1]-q.edges[3]) / math.Max(q.edges[1], q.edges[3]+1e-9)
	q.isRect = diff02 < 0.15 && diff13 < 0.15

	if parA >= parB {
		// edges 0,2 are the parallel pair (top/bottom of trapezoid)
		q.height = hA
		if q.edges[0] >= q.edges[2] {
			q.botLen, q.topLen = q.edges[0], q.edges[2]
		} else {
			q.botLen, q.topLen = q.edges[2], q.edges[0]
		}
	} else {
		// edges 1,3 are the parallel pair
		q.height = hB
		if q.edges[1] >= q.edges[3] {
			q.botLen, q.topLen = q.edges[1], q.edges[3]
		} else {
			q.botLen, q.topLen = q.edges[3], q.edges[1]
		}
	}
	return q
}

// assignWH determines which of two edge measurements (e1, e2) is width vs height
// by matching against existing measurements. If no prior measurements, returns (larger, smaller).
func assignWH(e1, e2, priorW, priorH float64) (w, h float64) {
	if priorW == 0 && priorH == 0 {
		if e1 >= e2 {
			return e1, e2
		}
		return e2, e1
	}
	err12 := sqDiff(e1, priorW) + sqDiff(e2, priorH)
	err21 := sqDiff(e2, priorW) + sqDiff(e1, priorH)
	if err12 <= err21 {
		return e1, e2
	}
	return e2, e1
}

// InferCuboidWithPairing reconstructs a general hexahedron from traced faces.
// Every face contributes equally — side trapezoids, top/bottom rectangles all
// provide measurements of the 5 unknowns: wBot, hBot, wTop, hTop, Z.
// Pair 1 = front/back, Pair 2 = left/right, Pair 3 = top/bottom.
func InferCuboidWithPairing(faces []TracedFace, pairing FacePairingJS) (*ReconstructedObject, string, error) {
	faceMap := map[int]TracedFace{}
	for _, f := range faces {
		faceMap[f.FaceNum] = f
	}

	getQuad := func(fn int) (quadMeasure, bool) {
		f, ok := faceMap[fn]
		if !ok || len(f.Outline) < 3 {
			return quadMeasure{}, false
		}
		return measureQuad(f.Outline), true
	}

	avg := func(v []float64) float64 {
		if len(v) == 0 {
			return 0
		}
		s := 0.0
		for _, x := range v {
			s += x
		}
		return s / float64(len(v))
	}

	// Collect ALL measurements of each unknown from every face, based on its role.
	// Front/back face (trapezoid): parallel edges → wBot, wTop
	// Left/right face (trapezoid): parallel edges → hBot, hTop
	// Top face (rectangle): edges → wTop, hTop
	// Bottom face (rectangle): edges → wBot, hBot
	var wBm, wTm, hBm, hTm []float64
	var fbSlants, lrSlants []float64

	// Front/back faces: parallel edges measure wBot and wTop
	for _, fn := range [2]int{pairing.Pair1[0], pairing.Pair1[1]} {
		if fn == 0 {
			continue
		}
		q, ok := getQuad(fn)
		if !ok {
			continue
		}
		wBm = append(wBm, q.botLen)
		wTm = append(wTm, q.topLen)
		fbSlants = append(fbSlants, q.height)
		debugLog("  face %d (front/back): wB=%.1f wT=%.1f slant=%.1f", fn, q.botLen, q.topLen, q.height)
	}

	// Left/right faces: parallel edges measure hBot and hTop
	for _, fn := range [2]int{pairing.Pair2[0], pairing.Pair2[1]} {
		if fn == 0 {
			continue
		}
		q, ok := getQuad(fn)
		if !ok {
			continue
		}
		hBm = append(hBm, q.botLen)
		hTm = append(hTm, q.topLen)
		lrSlants = append(lrSlants, q.height)
		debugLog("  face %d (left/right): hB=%.1f hT=%.1f slant=%.1f", fn, q.botLen, q.topLen, q.height)
	}

	// Top/bottom faces: rectangles contributing wBot/wTop and hBot/hTop
	tbNums := [2]int{pairing.Pair3[0], pairing.Pair3[1]}
	seen := map[int]bool{}
	var tbQuads []quadMeasure
	for _, fn := range tbNums {
		if fn == 0 || seen[fn] {
			continue
		}
		seen[fn] = true
		q, ok := getQuad(fn)
		if !ok {
			continue
		}
		tbQuads = append(tbQuads, q)
		debugLog("  face %d (top/bottom): edges=[%.1f, %.1f, %.1f, %.1f]",
			fn, q.edges[0], q.edges[1], q.edges[2], q.edges[3])
	}

	// Incorporate top/bottom face measurements.
	// A TB face is a rectangle — its two edge pair averages are two of {wBot, hBot, wTop, hTop}.
	// Use side face measurements (if any) to determine which edge is w vs h.
	if len(tbQuads) == 2 {
		q1, q2 := tbQuads[0], tbQuads[1]
		a1 := q1.edges[0] * q1.edges[1]
		a2 := q2.edges[0] * q2.edges[1]
		bot, top := q1, q2
		if a2 > a1 {
			bot, top = q2, q1
		}
		be1 := (bot.edges[0] + bot.edges[2]) / 2
		be2 := (bot.edges[1] + bot.edges[3]) / 2
		te1 := (top.edges[0] + top.edges[2]) / 2
		te2 := (top.edges[1] + top.edges[3]) / 2

		// Determine w vs h assignment by matching to side face data
		bw, bh := assignWH(be1, be2, avg(wBm), avg(hBm))
		tw, th := assignWH(te1, te2, avg(wTm), avg(hTm))
		wBm = append(wBm, bw)
		hBm = append(hBm, bh)
		wTm = append(wTm, tw)
		hTm = append(hTm, th)
		debugLog("  TB 2 faces: bot=%.1f×%.1f top=%.1f×%.1f", bw, bh, tw, th)
	} else if len(tbQuads) == 1 {
		q := tbQuads[0]
		e1 := (q.edges[0] + q.edges[2]) / 2
		e2 := (q.edges[1] + q.edges[3]) / 2

		// Same face for both top and bottom — it represents one of them.
		// Add as both-end constraint only if we don't have side data for that dimension.
		w, h := assignWH(e1, e2, avg(wBm), avg(hBm))
		if len(wBm) == 0 {
			wBm = append(wBm, w)
		}
		if len(wTm) == 0 {
			wTm = append(wTm, w)
		}
		if len(hBm) == 0 {
			hBm = append(hBm, h)
		}
		if len(hTm) == 0 {
			hTm = append(hTm, h)
		}
		debugLog("  TB 1 face: w=%.1f h=%.1f (fill only missing dims)", w, h)
	}

	wBot := avg(wBm)
	wTop := avg(wTm)
	hBot := avg(hBm)
	hTop := avg(hTm)

	debugLog("  averaged: wBot=%.1f wTop=%.1f hBot=%.1f hTop=%.1f", wBot, wTop, hBot, hTop)

	// Solve Z from side trapezoid slant heights.
	// Front/back: slantHeight² = Z² + ((hBot-hTop)/2)²
	// Left/right: slantHeight² = Z² + ((wBot-wTop)/2)²
	var Z float64
	zCount := 0

	fbAvg := avg(fbSlants)
	if fbAvg > 0 {
		hDiff := (hBot - hTop) / 2
		zSq := fbAvg*fbAvg - hDiff*hDiff
		if zSq > 0 {
			Z += math.Sqrt(zSq)
		} else {
			Z += fbAvg
		}
		zCount++
		debugLog("  Z from front/back: %.1f (slant=%.1f, hDiff=%.1f)", Z, fbAvg, hDiff)
	}
	lrAvg := avg(lrSlants)
	if lrAvg > 0 {
		wDiff := (wBot - wTop) / 2
		zSq := lrAvg*lrAvg - wDiff*wDiff
		if zSq > 0 {
			Z += math.Sqrt(zSq)
		} else {
			Z += lrAvg
		}
		zCount++
		debugLog("  Z from left/right: %.1f (slant=%.1f, wDiff=%.1f)", Z/float64(zCount), lrAvg, wDiff)
	}

	if zCount > 0 {
		Z /= float64(zCount)
	} else if wBot > 0 {
		Z = math.Min(wBot, hBot) / 2
	}

	debugLog("InferCuboidWithPairing: frustum bot=%.1f×%.1f top=%.1f×%.1f Z=%.1f",
		wBot, hBot, wTop, hTop, Z)

	// Build vertices: bottom centered at z=0, top centered at z=Z
	verts := [][3]float64{
		{-wBot / 2, -hBot / 2, 0},
		{wBot / 2, -hBot / 2, 0},
		{wBot / 2, hBot / 2, 0},
		{-wBot / 2, hBot / 2, 0},
		{-wTop / 2, -hTop / 2, Z},
		{wTop / 2, -hTop / 2, Z},
		{wTop / 2, hTop / 2, Z},
		{-wTop / 2, hTop / 2, Z},
	}
	faceIdxs := [][]int{
		{3, 2, 1, 0}, // bottom (outward normal = -Z)
		{4, 5, 6, 7}, // top (outward normal = +Z)
		{0, 1, 5, 4}, // front
		{2, 3, 7, 6}, // back
		{3, 0, 4, 7}, // left
		{1, 2, 6, 5}, // right
	}

	scad := buildHexahedronSCAD(verts, faceIdxs, wBot, hBot, wTop, hTop, Z)

	obj := &ReconstructedObject{
		Faces:       faces,
		Vertices3D:  verts,
		FaceIndices: faceIdxs,
		SCAD:        scad,
	}
	msg := fmt.Sprintf("frustum: bottom %.1f×%.1f, top %.1f×%.1f, height %.1f mm",
		wBot, hBot, wTop, hTop, Z)
	return obj, msg, nil
}

func buildHexahedronSCAD(verts [][3]float64, faceIdxs [][]int, wBot, hBot, wTop, hTop, Z float64) string {
	var b strings.Builder
	b.WriteString(fmt.Sprintf("// Frustum: bottom %.2f×%.2f, top %.2f×%.2f, height %.2f\n\n",
		wBot, hBot, wTop, hTop, Z))
	b.WriteString("polyhedron(\n  points = [\n")
	for i, v := range verts {
		comma := ","
		if i == len(verts)-1 {
			comma = ""
		}
		b.WriteString(fmt.Sprintf("    [%.4f, %.4f, %.4f]%s\n", v[0], v[1], v[2], comma))
	}
	b.WriteString("  ],\n  faces = [\n")
	for i, face := range faceIdxs {
		comma := ","
		if i == len(faceIdxs)-1 {
			comma = ""
		}
		b.WriteString("    [")
		for j, idx := range face {
			if j > 0 {
				b.WriteString(", ")
			}
			b.WriteString(fmt.Sprintf("%d", idx))
		}
		b.WriteString("]" + comma + "\n")
	}
	b.WriteString("  ]\n);\n")
	return b.String()
}

func buildRectCuboidResult(W, H, D float64, faces []TracedFace) (*ReconstructedObject, string, error) {
	var b strings.Builder
	b.WriteString(fmt.Sprintf("// Cuboid from %d of 6 traced faces\n", len(faces)))
	b.WriteString(fmt.Sprintf("// Dimensions: %.2f × %.2f × %.2f mm\n\n", W, H, D))
	b.WriteString(fmt.Sprintf("translate([%.4f, %.4f, %.4f])\n", -W/2, -H/2, -D/2))
	b.WriteString(fmt.Sprintf("  cube([%.4f, %.4f, %.4f]);\n", W, H, D))

	verts, faceIdxs := cuboidGeometry(W, H, D)
	allFaces := cuboidTracedFaces(W, H, D, faces)
	obj := &ReconstructedObject{
		Faces: allFaces, Vertices3D: verts, FaceIndices: faceIdxs, SCAD: b.String(),
	}
	msg := fmt.Sprintf("cuboid inferred: %.1f × %.1f × %.1f mm from %d of 6 faces", W, H, D, len(faces))
	return obj, msg, nil
}

// measureFaceEdges fits a quadrilateral to the outline and returns edge info.
func measureFaceEdges(outline [][2]float64) faceEdgeInfo {
	corners := fitQuadCorners(outline)
	e1 := [2]float64{corners[1][0] - corners[0][0], corners[1][1] - corners[0][1]}
	e2 := [2]float64{corners[3][0] - corners[0][0], corners[3][1] - corners[0][1]}

	l1 := math.Sqrt(e1[0]*e1[0] + e1[1]*e1[1])
	l2 := math.Sqrt(e2[0]*e2[0] + e2[1]*e2[1])
	if l1 < 1e-9 || l2 < 1e-9 {
		return faceEdgeInfo{l1, l2, math.Pi / 2}
	}

	cosA := (e1[0]*e2[0] + e1[1]*e2[1]) / (l1 * l2)
	angle := math.Acos(math.Max(-1, math.Min(1, cosA)))
	if angle > math.Pi/2 {
		angle = math.Pi - angle
	}

	if l1 < l2 {
		l1, l2 = l2, l1
	}
	return faceEdgeInfo{l1, l2, angle}
}

// fitQuadCorners finds the 4 dominant corners of a roughly quadrilateral outline.
func fitQuadCorners(outline [][2]float64) [4][2]float64 {
	n := len(outline)
	if n <= 4 {
		var corners [4][2]float64
		for i := 0; i < 4; i++ {
			corners[i] = outline[i%n]
		}
		return corners
	}

	// Diameter: two farthest points
	maxDist := 0.0
	c0, c2 := 0, 0
	for i := 0; i < n; i++ {
		for j := i + 1; j < n; j++ {
			dx := outline[i][0] - outline[j][0]
			dy := outline[i][1] - outline[j][1]
			d := dx*dx + dy*dy
			if d > maxDist {
				maxDist = d
				c0, c2 = i, j
			}
		}
	}

	// Perpendicular extremes from the diameter line
	dx := outline[c2][0] - outline[c0][0]
	dy := outline[c2][1] - outline[c0][1]
	lineLen := math.Sqrt(dx*dx + dy*dy)
	if lineLen < 1e-9 {
		return [4][2]float64{outline[0], outline[n/4], outline[n/2], outline[3*n/4]}
	}
	nx, ny := -dy/lineLen, dx/lineLen

	maxPos, maxNeg := 0.0, 0.0
	c1, c3 := c0, c0
	for i, p := range outline {
		d := (p[0]-outline[c0][0])*nx + (p[1]-outline[c0][1])*ny
		if d > maxPos {
			maxPos = d
			c1 = i
		}
		if d < maxNeg {
			maxNeg = d
			c3 = i
		}
	}

	// Order CCW by angle from centroid
	pts := [4]int{c0, c1, c2, c3}
	var cx, cy float64
	for _, idx := range pts {
		cx += outline[idx][0]
		cy += outline[idx][1]
	}
	cx /= 4
	cy /= 4

	type ca struct {
		idx   int
		angle float64
	}
	cas := [4]ca{}
	for i, idx := range pts {
		cas[i] = ca{idx, math.Atan2(outline[idx][1]-cy, outline[idx][0]-cx)}
	}
	sort.Slice(cas[:], func(i, j int) bool { return cas[i].angle < cas[j].angle })

	return [4][2]float64{
		outline[cas[0].idx], outline[cas[1].idx],
		outline[cas[2].idx], outline[cas[3].idx],
	}
}

type ppAssignment struct {
	W, H, D                   float64
	angleAB, angleAC, angleBC float64
	err                       float64
	whPair, wdPair, hdPair    [2]int // indices into the faceEdgeInfo slice
}

// FacePairingJS is the JSON-friendly pairing for the frontend.
type FacePairingJS struct {
	Pair1 [2]int `json:"pair1"` // WH: face numbers
	Pair2 [2]int `json:"pair2"` // WD: face numbers
	Pair3 [2]int `json:"pair3"` // HD: face numbers
}

// fitParallelepipedAssignment brute-forces all 90 partitions of 6 faces into
// 3 opposite pairs (WH, WD, HD), returning the assignment with minimum error.
func fitParallelepipedAssignment(infos []faceEdgeInfo) ppAssignment {
	best := ppAssignment{err: math.Inf(1)}

	for a := 0; a < 6; a++ {
		for b := a + 1; b < 6; b++ {
			var rem [4]int
			ri := 0
			for k := 0; k < 6; k++ {
				if k != a && k != b {
					rem[ri] = k
					ri++
				}
			}
			for ci := 0; ci < 4; ci++ {
				for di := ci + 1; di < 4; di++ {
					var hd [2]int
					hi := 0
					for k := 0; k < 4; k++ {
						if k != ci && k != di {
							hd[hi] = rem[k]
							hi++
						}
					}

					W := (infos[a].len1 + infos[b].len1 +
						infos[rem[ci]].len1 + infos[rem[di]].len1) / 4
					H := (infos[a].len2 + infos[b].len2 +
						infos[hd[0]].len1 + infos[hd[1]].len1) / 4
					D := (infos[rem[ci]].len2 + infos[rem[di]].len2 +
						infos[hd[0]].len2 + infos[hd[1]].len2) / 4

					err := sqDiff(infos[a].len1, W) + sqDiff(infos[a].len2, H) +
						sqDiff(infos[b].len1, W) + sqDiff(infos[b].len2, H) +
						sqDiff(infos[rem[ci]].len1, W) + sqDiff(infos[rem[ci]].len2, D) +
						sqDiff(infos[rem[di]].len1, W) + sqDiff(infos[rem[di]].len2, D) +
						sqDiff(infos[hd[0]].len1, H) + sqDiff(infos[hd[0]].len2, D) +
						sqDiff(infos[hd[1]].len1, H) + sqDiff(infos[hd[1]].len2, D)

					if err < best.err {
						aAB := (infos[a].angle + infos[b].angle) / 2
						aAC := (infos[rem[ci]].angle + infos[rem[di]].angle) / 2
						aBC := (infos[hd[0]].angle + infos[hd[1]].angle) / 2
						best = ppAssignment{
							W: W, H: H, D: D,
							angleAB: aAB, angleAC: aAC, angleBC: aBC,
							err:    err,
							whPair: [2]int{a, b},
							wdPair: [2]int{rem[ci], rem[di]},
							hdPair: [2]int{hd[0], hd[1]},
						}
					}
				}
			}
		}
	}
	return best
}

// reconstructEdgeVectors builds 3D edge vectors from dimensions and angles.
// Places a along x-axis, b in xy-plane, c with z-component.
func reconstructEdgeVectors(a ppAssignment) ([3]float64, [3]float64, [3]float64) {
	va := [3]float64{a.W, 0, 0}

	vb := [3]float64{a.H * math.Cos(a.angleAB), a.H * math.Sin(a.angleAB), 0}

	cx := a.D * math.Cos(a.angleAC)
	sinAB := math.Sin(a.angleAB)
	cy := 0.0
	if math.Abs(sinAB) > 1e-9 {
		cy = (a.D*math.Cos(a.angleBC) - math.Cos(a.angleAB)*cx) / sinAB
	}
	cz2 := a.D*a.D - cx*cx - cy*cy
	cz := 0.0
	if cz2 > 1e-9 {
		cz = math.Sqrt(cz2)
	} else if cz2 > -1 {
		// Slightly negative from rounding — clamp to flat
		cz = 0
	}
	vc := [3]float64{cx, cy, cz}

	return va, vb, vc
}

func parallelepipedVerts(a, b, c [3]float64) [][3]float64 {
	half := [3]float64{
		(a[0] + b[0] + c[0]) / 2,
		(a[1] + b[1] + c[1]) / 2,
		(a[2] + b[2] + c[2]) / 2,
	}
	o := [3]float64{-half[0], -half[1], -half[2]}

	add := func(base [3]float64, vecs ...[3]float64) [3]float64 {
		r := base
		for _, v := range vecs {
			r[0] += v[0]
			r[1] += v[1]
			r[2] += v[2]
		}
		return r
	}

	return [][3]float64{
		o,               // 0: origin corner
		add(o, a),       // 1: +a
		add(o, a, b),    // 2: +a+b
		add(o, b),       // 3: +b
		add(o, c),       // 4: +c
		add(o, a, c),    // 5: +a+c
		add(o, a, b, c), // 6: +a+b+c
		add(o, b, c),    // 7: +b+c
	}
}

func parallelepipedFaceIndices() [][]int {
	return [][]int{
		{0, 3, 2, 1}, // -c face (ab)
		{4, 5, 6, 7}, // +c face
		{0, 1, 5, 4}, // -b face (ac)
		{2, 3, 7, 6}, // +b face
		{0, 4, 7, 3}, // -a face (bc)
		{1, 2, 6, 5}, // +a face
	}
}

func buildParallelepipedSCAD(verts [][3]float64, faces [][]int) string {
	var b strings.Builder
	b.WriteString("// Parallelepiped reconstructed from traced faces\n\n")
	b.WriteString("polyhedron(\n  points = [\n")
	for i, v := range verts {
		comma := ","
		if i == len(verts)-1 {
			comma = ""
		}
		b.WriteString(fmt.Sprintf("    [%.4f, %.4f, %.4f]%s\n", v[0], v[1], v[2], comma))
	}
	b.WriteString("  ],\n  faces = [\n")
	for i, f := range faces {
		comma := ","
		if i == len(faces)-1 {
			comma = ""
		}
		b.WriteString(fmt.Sprintf("    [%d, %d, %d, %d]%s\n", f[0], f[1], f[2], f[3], comma))
	}
	b.WriteString("  ]\n);\n")
	return b.String()
}

func sqDiff(a, b float64) float64 {
	d := a - b
	return d * d
}

// fitCuboidPartial handles 2-5 traced faces using pairwise dimension matching.
func fitCuboidPartial(faceDims [][2]float64) (float64, float64, float64) {
	if len(faceDims) >= 2 {
		W, H, D, ok := matchCuboidDims(faceDims[0], faceDims[1])
		if ok {
			dims := []float64{W, H, D}
			sort.Float64s(dims)
			return dims[2], dims[1], dims[0]
		}
	}
	w, h := faceDims[0][0], faceDims[0][1]
	return w, h, math.Sqrt(w * h)
}

// fitRectDimensions extracts approximate width and height of a traced face
// using PCA to find the principal axes, then measuring extent along each.
func fitRectDimensions(outline [][2]float64) (float64, float64) {
	if len(outline) < 3 {
		return 0, 0
	}

	var cx, cy float64
	for _, p := range outline {
		cx += p[0]
		cy += p[1]
	}
	n := float64(len(outline))
	cx /= n
	cy /= n

	var cxx, cxy, cyy float64
	for _, p := range outline {
		dx := p[0] - cx
		dy := p[1] - cy
		cxx += dx * dx
		cxy += dx * dy
		cyy += dy * dy
	}

	trace := cxx + cyy
	det := cxx*cyy - cxy*cxy
	disc := math.Sqrt(math.Max(0, trace*trace/4-det))

	var ux, uy float64
	if math.Abs(cxy) > 1e-9 {
		lambda1 := trace/2 + disc
		ux = cxy
		uy = lambda1 - cxx
	} else if cxx >= cyy {
		ux, uy = 1, 0
	} else {
		ux, uy = 0, 1
	}
	mag := math.Sqrt(ux*ux + uy*uy)
	if mag > 1e-9 {
		ux /= mag
		uy /= mag
	}

	var minA, maxA, minB, maxB float64
	for i, p := range outline {
		dx := p[0] - cx
		dy := p[1] - cy
		a := dx*ux + dy*uy
		pb := -dx*uy + dy*ux
		if i == 0 {
			minA, maxA = a, a
			minB, maxB = pb, pb
		} else {
			minA = math.Min(minA, a)
			maxA = math.Max(maxA, a)
			minB = math.Min(minB, pb)
			maxB = math.Max(maxB, pb)
		}
	}
	return maxA - minA, maxB - minB
}

// matchCuboidDims finds the shared dimension between two rectangular faces.
// Returns (dimA, shared, dimB, matched) where the cuboid is dimA × shared × dimB.
func matchCuboidDims(a, b [2]float64) (float64, float64, float64, bool) {
	tolerance := 0.25

	type match struct {
		d1, shared, d2, err float64
	}
	var best *match

	for i := 0; i < 2; i++ {
		for j := 0; j < 2; j++ {
			avg := (a[i] + b[j]) / 2
			if avg < 1 {
				continue
			}
			relErr := math.Abs(a[i]-b[j]) / avg
			if relErr < tolerance {
				if best == nil || relErr < best.err {
					m := match{a[1-i], avg, b[1-j], relErr}
					best = &m
				}
			}
		}
	}

	if best == nil {
		return 0, 0, 0, false
	}
	return best.d1, best.shared, best.d2, true
}

func cuboidTracedFaces(W, H, D float64, traced []TracedFace) []TracedFace {
	tracedMap := map[int]TracedFace{}
	for _, f := range traced {
		tracedMap[f.FaceNum] = f
	}

	// Face pairs: 1,2=W×H  3,4=W×D  5,6=H×D
	pairDims := [3][2]float64{{W, H}, {W, D}, {H, D}}

	var all []TracedFace
	for i := 1; i <= 6; i++ {
		if f, ok := tracedMap[i]; ok {
			all = append(all, f)
			continue
		}
		pair := (i - 1) / 2
		w, h := pairDims[pair][0], pairDims[pair][1]
		all = append(all, TracedFace{
			FaceNum: i,
			Outline: [][2]float64{
				{-w / 2, -h / 2}, {w / 2, -h / 2},
				{w / 2, h / 2}, {-w / 2, h / 2},
			},
		})
	}
	return all
}

func cuboidGeometry(W, H, D float64) ([][3]float64, [][]int) {
	w, h, d := W/2, H/2, D/2
	verts := [][3]float64{
		{-w, -h, -d}, {w, -h, -d}, {w, h, -d}, {-w, h, -d},
		{-w, -h, d}, {w, -h, d}, {w, h, d}, {-w, h, d},
	}
	faces := [][]int{
		{0, 3, 2, 1},
		{4, 5, 6, 7},
		{0, 1, 5, 4},
		{2, 3, 7, 6},
		{0, 4, 7, 3},
		{1, 2, 6, 5},
	}
	return verts, faces
}

// ReconstructPrism generates a prism from a single traced face cross-section.
func ReconstructPrism(face TracedFace, depth float64) (*ReconstructedObject, error) {
	if len(face.Outline) < 3 {
		return nil, fmt.Errorf("face needs at least 3 vertices, got %d", len(face.Outline))
	}

	debugLog("ReconstructPrism: face %d, %d vertices, depth=%.1f",
		face.FaceNum, len(face.Outline), depth)

	obj := &ReconstructedObject{
		Faces: []TracedFace{face},
	}
	obj.SCAD = obj.generatePrismSCAD(depth)
	return obj, nil
}

func (obj *ReconstructedObject) generatePrismSCAD(depth float64) string {
	var b strings.Builder
	face := obj.Faces[0]

	centered := centerOutline(face.Outline)

	b.WriteString("// Prism extruded from traced face cross-section\n\n")
	b.WriteString(fmt.Sprintf("linear_extrude(height=%.4f)\n", depth))
	b.WriteString("  polygon(points=[\n")
	for j, pt := range centered {
		comma := ","
		if j == len(centered)-1 {
			comma = ""
		}
		b.WriteString(fmt.Sprintf("    [%.4f, %.4f]%s\n", pt[0], pt[1], comma))
	}
	b.WriteString("  ]);\n")

	return b.String()
}

// ReconstructFromFaces takes traced face outlines and adjacency info,
// folds them into 3D, and generates SCAD.
func ReconstructFromFaces(faces []TracedFace, adj FaceAdjacency) (*ReconstructedObject, error) {
	if len(faces) < 1 {
		return nil, fmt.Errorf("need at least 1 face")
	}

	obj := &ReconstructedObject{
		Faces:     faces,
		Adjacency: adj,
	}

	if adj != nil && len(adj) > 0 {
		if err := obj.foldFaces(); err != nil {
			debugLog("ReconstructFromFaces: fold failed: %v, falling back to flat", err)
			obj.buildFlat()
		}
	} else {
		obj.buildFlat()
	}

	obj.SCAD = obj.generateSCAD()
	return obj, nil
}

// buildFlat places all faces flat on the XY plane, spaced apart.
func (obj *ReconstructedObject) buildFlat() {
	var allVerts [][3]float64
	var faceIdxs [][]int

	offsetX := 0.0
	for _, face := range obj.Faces {
		var maxW float64
		var idxs []int
		for _, pt := range face.Outline {
			idx := len(allVerts)
			allVerts = append(allVerts, [3]float64{pt[0] + offsetX, pt[1], 0})
			idxs = append(idxs, idx)
			if math.Abs(pt[0])*2 > maxW {
				maxW = math.Abs(pt[0]) * 2
			}
		}
		faceIdxs = append(faceIdxs, idxs)
		offsetX += maxW + 10 // 10mm gap between faces
	}

	obj.Vertices3D = allVerts
	obj.FaceIndices = faceIdxs
}

// foldFaces reconstructs 3D positions by folding faces along shared edges.
// Places face 1 flat, then BFS-folds adjacent faces.
func (obj *ReconstructedObject) foldFaces() error {
	if len(obj.Faces) == 0 {
		return fmt.Errorf("no faces")
	}

	type placedFace struct {
		faceNum int
		verts3D [][3]float64 // 3D positions of this face's vertices
	}

	faceMap := map[int]TracedFace{}
	for _, f := range obj.Faces {
		faceMap[f.FaceNum] = f
	}

	placed := map[int]*placedFace{}

	// Place first face flat on XY plane at z=0
	first := obj.Faces[0]
	pf := &placedFace{faceNum: first.FaceNum}
	for _, pt := range first.Outline {
		pf.verts3D = append(pf.verts3D, [3]float64{pt[0], pt[1], 0})
	}
	placed[first.FaceNum] = pf

	// BFS fold
	queue := []int{first.FaceNum}
	for len(queue) > 0 {
		cur := queue[0]
		queue = queue[1:]

		links, ok := obj.Adjacency[cur]
		if !ok {
			continue
		}

		curPlaced := placed[cur]
		curFace := faceMap[cur]

		for _, link := range links {
			if _, done := placed[link.Neighbor]; done {
				continue
			}
			nbFace, exists := faceMap[link.Neighbor]
			if !exists {
				continue
			}

			// Shared edge on current face
			nv := len(curFace.Outline)
			if link.EdgeIdx >= nv {
				continue
			}
			e0 := link.EdgeIdx
			e1 := (link.EdgeIdx + 1) % nv

			// 3D positions of shared edge endpoints (from already-placed face)
			p0 := curPlaced.verts3D[e0]
			p1 := curPlaced.verts3D[e1]

			// Shared edge on neighbor face
			nnv := len(nbFace.Outline)
			if link.NeighborEdge >= nnv {
				continue
			}
			ne0 := link.NeighborEdge
			ne1 := (link.NeighborEdge + 1) % nnv

			// 2D positions of shared edge on neighbor (should match lengths)
			nb2d0 := nbFace.Outline[ne0]
			nb2d1 := nbFace.Outline[ne1]

			// Fold the neighbor: align its shared edge to the 3D edge,
			// then rotate 90° outward (default dihedral = 90° for box-like objects)
			nbPlaced := foldFaceOntoEdge(nbFace.Outline, ne0, ne1, nb2d0, nb2d1, p0, p1, curPlaced.verts3D, e0, e1, curFace.Outline)
			placed[link.Neighbor] = &placedFace{faceNum: link.Neighbor, verts3D: nbPlaced}
			queue = append(queue, link.Neighbor)
		}
	}

	// Collect all vertices and face indices
	var allVerts [][3]float64
	var faceIdxs [][]int
	vertIdx := 0

	for _, face := range obj.Faces {
		pf, ok := placed[face.FaceNum]
		if !ok {
			// unplaced face — lay flat offset
			var idxs []int
			for _, pt := range face.Outline {
				allVerts = append(allVerts, [3]float64{pt[0], pt[1], -50})
				idxs = append(idxs, vertIdx)
				vertIdx++
			}
			faceIdxs = append(faceIdxs, idxs)
			continue
		}
		var idxs []int
		for _, v := range pf.verts3D {
			allVerts = append(allVerts, v)
			idxs = append(idxs, vertIdx)
			vertIdx++
		}
		faceIdxs = append(faceIdxs, idxs)
	}

	obj.Vertices3D = allVerts
	obj.FaceIndices = faceIdxs
	return nil
}

// foldFaceOntoEdge positions a neighbor face's 2D vertices into 3D space,
// aligning its shared edge to the given 3D edge and folding 90° outward.
func foldFaceOntoEdge(
	nbOutline [][2]float64, ne0, ne1 int, nb2d0, nb2d1 [2]float64,
	p0, p1 [3]float64,
	curVerts [][3]float64, ce0, ce1 int,
	curOutline [][2]float64,
) [][3]float64 {
	// Edge vector in 3D
	edgeVec := sub3(p1, p0)
	edgeLen := norm3(edgeVec)
	if edgeLen < 1e-9 {
		edgeLen = 1
	}
	edgeDir := scale3(edgeVec, 1.0/edgeLen)

	// Current face's outward normal: compute from face vertices
	// Use cross product of two edges of the current face
	curNormal := computeFaceNormal(curVerts)

	// Outward direction: perpendicular to edge, in the plane of the current face,
	// pointing away from the current face center
	inPlane := cross3(curNormal, edgeDir)
	inPlane = normalize3(inPlane)

	// The fold direction (90° fold): the neighbor face extends perpendicular
	// to the current face. The "up" direction for the new face is the current face's normal.
	// For a 90° dihedral (box), the new face's plane normal = -inPlane
	foldNormal := curNormal // the direction "up" from the edge

	// Build coordinate frame at edge: edgeDir, foldNormal, inPlane
	// Neighbor face's local 2D system:
	// - x-axis along the edge
	// - y-axis perpendicular (folded: along foldNormal for 90° fold)

	// Compute 2D alignment: neighbor's edge ne0→ne1 maps to p0→p1
	nb2dEdge := [2]float64{nb2d1[0] - nb2d0[0], nb2d1[1] - nb2d0[1]}
	nb2dEdgeLen := math.Sqrt(nb2dEdge[0]*nb2dEdge[0] + nb2dEdge[1]*nb2dEdge[1])
	if nb2dEdgeLen < 1e-9 {
		nb2dEdgeLen = 1
	}
	nb2dEdgeDir := [2]float64{nb2dEdge[0] / nb2dEdgeLen, nb2dEdge[1] / nb2dEdgeLen}
	nb2dNormDir := [2]float64{-nb2dEdgeDir[1], nb2dEdgeDir[0]} // 90° CCW

	result := make([][3]float64, len(nbOutline))
	for i, pt := range nbOutline {
		// Express point relative to ne0 in the edge-aligned 2D frame
		dx := pt[0] - nb2d0[0]
		dy := pt[1] - nb2d0[1]
		along := dx*nb2dEdgeDir[0] + dy*nb2dEdgeDir[1]
		perp := dx*nb2dNormDir[0] + dy*nb2dNormDir[1]

		// Map to 3D: along → edgeDir, perp → foldNormal (90° fold)
		scale := edgeLen / nb2dEdgeLen
		result[i] = [3]float64{
			p0[0] + along*scale*edgeDir[0] + perp*scale*foldNormal[0],
			p0[1] + along*scale*edgeDir[1] + perp*scale*foldNormal[1],
			p0[2] + along*scale*edgeDir[2] + perp*scale*foldNormal[2],
		}
	}
	return result
}

func computeFaceNormal(verts [][3]float64) [3]float64 {
	if len(verts) < 3 {
		return [3]float64{0, 0, 1}
	}
	v0 := sub3(verts[1], verts[0])
	v1 := sub3(verts[2], verts[0])
	n := cross3(v0, v1)
	return normalize3(n)
}

// --- SCAD generation ---

func (obj *ReconstructedObject) generateSCAD() string {
	var b strings.Builder

	b.WriteString("// Reconstructed object from traced face templates\n\n")

	hasFolding := obj.Adjacency != nil && len(obj.Adjacency) > 0

	// Face polygon modules — centered at origin
	for _, face := range obj.Faces {
		centered := centerOutline(face.Outline)
		b.WriteString(fmt.Sprintf("module face_%d() {\n", face.FaceNum))
		b.WriteString("  polygon(points=[\n")
		for j, pt := range centered {
			comma := ","
			if j == len(centered)-1 {
				comma = ""
			}
			b.WriteString(fmt.Sprintf("    [%.4f, %.4f]%s\n", pt[0], pt[1], comma))
		}
		b.WriteString("  ]);\n")
		b.WriteString("}\n\n")
	}

	if hasFolding && len(obj.Vertices3D) > 0 && len(obj.FaceIndices) > 0 {
		obj.writePolyhedronSCAD(&b)
	} else {
		// No adjacency — show faces side by side as thin extrusions
		offsetX := 0.0
		for _, face := range obj.Faces {
			centered := centerOutline(face.Outline)
			w := outlineWidth(centered)
			b.WriteString(fmt.Sprintf("translate([%.4f, 0, 0])\n", offsetX))
			b.WriteString(fmt.Sprintf("  linear_extrude(height=2) face_%d();\n\n", face.FaceNum))
			offsetX += w + 5
			_ = centered
		}
	}

	return b.String()
}

func centerOutline(pts [][2]float64) [][2]float64 {
	if len(pts) == 0 {
		return pts
	}
	var cx, cy float64
	for _, p := range pts {
		cx += p[0]
		cy += p[1]
	}
	n := float64(len(pts))
	cx /= n
	cy /= n
	out := make([][2]float64, len(pts))
	for i, p := range pts {
		out[i] = [2]float64{p[0] - cx, p[1] - cy}
	}
	return out
}

func outlineWidth(pts [][2]float64) float64 {
	if len(pts) == 0 {
		return 0
	}
	minX, maxX := pts[0][0], pts[0][0]
	for _, p := range pts[1:] {
		if p[0] < minX {
			minX = p[0]
		}
		if p[0] > maxX {
			maxX = p[0]
		}
	}
	return maxX - minX
}

func (obj *ReconstructedObject) placedVerts(faceIdx int) [][3]float64 {
	if faceIdx >= len(obj.FaceIndices) {
		return nil
	}
	var verts [][3]float64
	for _, idx := range obj.FaceIndices[faceIdx] {
		if idx < len(obj.Vertices3D) {
			verts = append(verts, obj.Vertices3D[idx])
		}
	}
	return verts
}

func (obj *ReconstructedObject) writePolyhedronSCAD(b *strings.Builder) {
	// Merge vertices that are within tolerance
	const eps = 0.01
	type mergedVert struct {
		pos [3]float64
	}
	var merged []mergedVert
	idxMap := make([]int, len(obj.Vertices3D))

	for i, v := range obj.Vertices3D {
		found := -1
		for j, m := range merged {
			d := sub3(v, m.pos)
			if norm3(d) < eps {
				found = j
				break
			}
		}
		if found >= 0 {
			idxMap[i] = found
		} else {
			idxMap[i] = len(merged)
			merged = append(merged, mergedVert{pos: v})
		}
	}

	b.WriteString("polyhedron(\n")
	b.WriteString("  points=[\n")
	for i, m := range merged {
		comma := ","
		if i == len(merged)-1 {
			comma = ""
		}
		b.WriteString(fmt.Sprintf("    [%.4f, %.4f, %.4f]%s\n", m.pos[0], m.pos[1], m.pos[2], comma))
	}
	b.WriteString("  ],\n")
	b.WriteString("  faces=[\n")
	for i, idxs := range obj.FaceIndices {
		comma := ","
		if i == len(obj.FaceIndices)-1 {
			comma = ""
		}
		b.WriteString("    [")
		for j, idx := range idxs {
			if j > 0 {
				b.WriteString(", ")
			}
			b.WriteString(fmt.Sprintf("%d", idxMap[idx]))
		}
		b.WriteString("]" + comma + "\n")
	}
	b.WriteString("  ]\n")
	b.WriteString(");\n")
}

// GenerateReconstructedSCAD writes the SCAD file to the project output directory.
func GenerateReconstructedSCAD(obj *ReconstructedObject, outputDir string) (string, error) {
	os.MkdirAll(outputDir, 0755)
	path := filepath.Join(outputDir, "reconstructed.scad")
	if err := os.WriteFile(path, []byte(obj.SCAD), 0644); err != nil {
		return "", fmt.Errorf("write SCAD: %w", err)
	}
	debugLog("GenerateReconstructedSCAD: wrote %s (%d bytes)", path, len(obj.SCAD))
	return path, nil
}

// Validate3N6 checks if the extracted faces satisfy the 3n-6 constraint.
// n = number of unique vertices, e = number of edges, f = number of faces.
// Euler: V - E + F = 2 for convex polyhedra.
func Validate3N6(faces []TracedFace, adj FaceAdjacency) (bool, string) {
	if len(faces) < 4 {
		return false, fmt.Sprintf("need at least 4 faces for a closed solid, got %d", len(faces))
	}

	nFaces := len(faces)
	totalVerts := 0
	for _, f := range faces {
		totalVerts += len(f.Outline)
	}

	// Count edges from adjacency
	edgeCount := 0
	if adj != nil {
		seen := map[[2]int]bool{}
		for fnum, links := range adj {
			for _, link := range links {
				key := [2]int{min(fnum, link.Neighbor), max(fnum, link.Neighbor)}
				if !seen[key] {
					seen[key] = true
					edgeCount++
				}
			}
		}
	} else {
		// Estimate: each face contributes len(outline) edges, each shared by 2 faces
		edgeCount = totalVerts / 2
	}

	// Estimate unique vertices (shared vertices at edges)
	// For a closed polyhedron: V = E - F + 2 (Euler's formula)
	eulerV := edgeCount - nFaces + 2

	// 3n-6 degrees of freedom for n vertices
	dof := 3*eulerV - 6
	measurements := totalVerts * 2 // 2 coordinates per vertex per face

	msg := fmt.Sprintf("F=%d, E=%d, V=%d(euler), DOF=3*%d-6=%d, measurements=%d",
		nFaces, edgeCount, eulerV, eulerV, dof, measurements)

	if measurements >= dof {
		return true, msg + " — sufficiently constrained"
	}
	return false, msg + " — underconstrained"
}

// --- 3D vector helpers ---

func sub3(a, b [3]float64) [3]float64 {
	return [3]float64{a[0] - b[0], a[1] - b[1], a[2] - b[2]}
}

func cross3(a, b [3]float64) [3]float64 {
	return [3]float64{
		a[1]*b[2] - a[2]*b[1],
		a[2]*b[0] - a[0]*b[2],
		a[0]*b[1] - a[1]*b[0],
	}
}

func norm3(v [3]float64) float64 {
	return math.Sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2])
}

func scale3(v [3]float64, s float64) [3]float64 {
	return [3]float64{v[0] * s, v[1] * s, v[2] * s}
}

func normalize3(v [3]float64) [3]float64 {
	n := norm3(v)
	if n < 1e-12 {
		return [3]float64{0, 0, 1}
	}
	return scale3(v, 1.0/n)
}