dwarf-go/dwarfctl/internal/odometry/odometry_test.go

package odometry

import (
	"image"
	"math"
	"testing"
)

// makeTextured makes a deterministic, highly-textured synthetic image of the
// given size. It mixes multiple sine gratings at different orientations so that
// phase correlation has rich frequency content to lock onto.
func makeTextured(w, h int) *image.RGBA {
	img := image.NewRGBA(image.Rect(0, 0, w, h))
	for y := 0; y < h; y++ {
		for x := 0; x < w; x++ {
			// Sum of 3 gratings — pseudo-random but deterministic texture.
			v := 0.0
			v += 0.5 + 0.5*math.Sin(float64(x)*0.13+float64(y)*0.07)
			v += 0.5 + 0.5*math.Sin(float64(x)*0.05-float64(y)*0.11+1.3)
			v += 0.5 + 0.5*math.Sin(float64(x)*0.21+2.1)
			v /= 3.0
			b := uint8(v * 255)
			// Add some discrete structure (a grid of bright dots) too.
			if x%32 < 3 && y%32 < 3 {
				b = 255
			}
			off := img.PixOffset(x, y)
			img.Pix[off+0] = b
			img.Pix[off+1] = b
			img.Pix[off+2] = b
			img.Pix[off+3] = 255
		}
	}
	return img
}

// shiftImage returns a copy of src translated by (dx,dy) pixels, with wrapped
// (toroidal) borders — this models a pure translation that phase correlation
// can recover exactly (no edge artifacts).
func shiftImage(src *image.RGBA, dx, dy int) *image.RGBA {
	w, h := src.Bounds().Dx(), src.Bounds().Dy()
	dst := image.NewRGBA(src.Bounds())
	for y := 0; y < h; y++ {
		for x := 0; x < w; x++ {
			sx := ((x-dx)%w + w) % w
			sy := ((y-dy)%h + h) % h
			so := src.PixOffset(sx, sy)
			do := dst.PixOffset(x, y)
			dst.Pix[do+0] = src.Pix[so+0]
			dst.Pix[do+1] = src.Pix[so+1]
			dst.Pix[do+2] = src.Pix[so+2]
			dst.Pix[do+3] = 255
		}
	}
	return dst
}

func TestFFT_RoundTrip(t *testing.T) {
	// FFT followed by inverse FFT must recover the original signal.
	const n = 64
	a := make([]complex128, n)
	for i := range a {
		a[i] = complex(math.Sin(float64(i)*0.3)+0.5*math.Cos(float64(i)*0.7), 0)
	}
	orig := make([]complex128, n)
	copy(orig, a)

	fft(a, false)
	fft(a, true)

	for i := range a {
		if math.Abs(real(a[i])-real(orig[i])) > 1e-9 || math.Abs(imag(a[i])-imag(orig[i])) > 1e-9 {
			t.Fatalf("FFT round-trip failed at %d: got %v want %v", i, a[i], orig[i])
		}
	}
}

func TestIsPow2(t *testing.T) {
	cases := map[int]bool{0: false, 1: true, 2: true, 3: false, 4: true, 16: true, 255: false, 256: true, 257: false}
	for n, want := range cases {
		if got := isPow2(n); got != want {
			t.Errorf("isPow2(%d)=%v want %v", n, got, want)
		}
	}
}

func TestEstimate_KnownShift(t *testing.T) {
	const size = 128
	const fovX, fovY = 10.0, 8.0

	base := makeTextured(size, size)

	cases := []struct{ dx, dy int }{
		{5, 0},
		{0, 4},
		{-6, 3},
		{10, -7},
		{0, 0}, // identical => zero shift
	}
	for _, c := range cases {
		shifted := shiftImage(base, c.dx, c.dy)
		r, err := Estimate(base, shifted, fovX, fovY, size)
		if err != nil {
			t.Fatalf("Estimate(dx=%d,dy=%d): %v", c.dx, c.dy, err)
		}
		// We expect the scene shift to equal the translation we applied. Positive
		// PanPx = content moved right. shifting the image by +dx moves content
		// to the right by dx pixels, so PanPx should be ≈ +dx.
		tolPx := 1.5 // sub-pixel estimator can be off by ~1px on a 128 grid.
		if math.Abs(r.PanPx-float64(c.dx)) > tolPx {
			t.Errorf("dx=%d dy=%d: PanPx=%.3f want ≈%d (±%.1f)", c.dx, c.dy, r.PanPx, c.dx, tolPx)
		}
		if math.Abs(r.TiltPx-float64(c.dy)) > tolPx {
			t.Errorf("dx=%d dy=%d: TiltPx=%.3f want ≈%d (±%.1f)", c.dx, c.dy, r.TiltPx, c.dy, tolPx)
		}
		// Degree conversion: px * fov / size.
		wantPanDeg := float64(c.dx) * fovX / size
		if math.Abs(r.PanDeg-wantPanDeg) > (tolPx*fovX/size) {
			t.Errorf("dx=%d: PanDeg=%.4f want ≈%.4f", c.dx, r.PanDeg, wantPanDeg)
		}
	}
}

func TestEstimate_IdenticalImages_HighConfidence(t *testing.T) {
	const size = 128
	a := makeTextured(size, size)
	r, err := Estimate(a, a, 10, 8, size)
	if err != nil {
		t.Fatal(err)
	}
	if math.Abs(r.PanPx) > 0.5 || math.Abs(r.TiltPx) > 0.5 {
		t.Errorf("identical images should give ~0 shift, got pan=%.2f tilt=%.2f", r.PanPx, r.TiltPx)
	}
}

func TestEstimate_RejectsBadSize(t *testing.T) {
	img := makeTextured(16, 16)
	if _, err := Estimate(img, img, 10, 8, 100); err == nil { // 100 not pow2
		t.Error("expected error for non-power-of-two size")
	}
	if _, err := Estimate(img, img, 0, 0, 64); err == nil { // zero fov
		t.Error("expected error for zero fov")
	}
}

func TestTracker_IntegratesRotation(t *testing.T) {
	const size = 128
	const fovX, fovY = 10.0, 8.0

	tr := NewTracker(fovX, fovY, size)

	base := makeTextured(size, size)

	// Frame 0: primes the reference; orientation should be zero.
	if _, err := tr.Update(base); err != nil {
		t.Fatal(err)
	}
	o0 := tr.Orientation()
	if o0.PanDeg != 0 || o0.TiltDeg != 0 || !o0.Primed {
		t.Fatalf("after prime: %+v", o0)
	}

	// Frame 1: shift right by 5 px (content moves right). The telescope is
	// static, so the motor-equivalent pan is the NEGATIVE of the scene shift.
	// Scene +5px right  => tracker pan = -5*fov/size.
	f1 := shiftImage(base, 5, 0)
	if _, err := tr.Update(f1); err != nil {
		t.Fatal(err)
	}
	o1 := tr.Orientation()
	wantPan := -5.0 * fovX / size
	if math.Abs(o1.PanDeg-wantPan) > 0.3 {
		t.Errorf("frame1 pan=%.4f want≈%.4f", o1.PanDeg, wantPan)
	}

	// Frame 2: shift another 4 px right (cumulative content shift 9 px).
	f2 := shiftImage(base, 9, 0)
	if _, err := tr.Update(f2); err != nil {
		t.Fatal(err)
	}
	o2 := tr.Orientation()
	wantPan2 := -9.0 * fovX / size
	if math.Abs(o2.PanDeg-wantPan2) > 0.5 {
		t.Errorf("frame2 cumulative pan=%.4f want≈%.4f", o2.PanDeg, wantPan2)
	}
	if o2.Frames != 2 {
		t.Errorf("frames=%d want 2", o2.Frames)
	}
}

func TestTracker_Reset(t *testing.T) {
	tr := NewTracker(10, 8, 64)
	base := makeTextured(64, 64)
	tr.Update(base)
	tr.Update(shiftImage(base, 4, 0))
	if tr.Orientation().PanDeg == 0 {
		t.Fatal("expected nonzero pan before reset")
	}
	tr.Reset()
	o := tr.Orientation()
	if o.PanDeg != 0 || o.Primed || o.Frames != 0 {
		t.Errorf("after reset: %+v", o)
	}
}

func TestPhaseCorrelation_NoTexture(t *testing.T) {
	// Flat images: no signal. Should not panic and should return low confidence.
	const n = 64
	flat := make([][]float64, n)
	for i := range flat {
		flat[i] = make([]float64, n)
	}
	dx, dy, conf := phaseCorrelation(flat, flat, n, n)
	if conf > 0.01 {
		t.Errorf("flat images confidence=%.4f, expected ~0", conf)
	}
	if dx != 0 || dy != 0 {
		t.Errorf("flat images shift=(%.1f,%.1f) want (0,0)", dx, dy)
	}
}

func TestSignedShift(t *testing.T) {
	cases := map[int]int{0: 0, 1: 1, 3: 3, 4: -4, 5: -3, 7: -1, 8: 0}
	for in, want := range cases {
		if got := signedShift(in, 8); got != want {
			t.Errorf("signedShift(%d,8)=%d want %d", in, got, want)
		}
	}
}

// Ensure float comparison helpers behave sensibly on this toolchain.
func TestFloatHelpers(t *testing.T) {
	if math.Abs(-1.5) != 1.5 {
		t.Fatal("math.Abs broken")
	}
}