// Copyright (c) 2026 Petr Balvín <opensource@petrbalvin.org> (https://petrbalvin.org)
// SPDX-License-Identifier: MIT

package signal

import (
	"errors"
	"math"
	"strings"
	"testing"

	"sourcedock.dev/petrbalvin/tensor/internal/core"
)

// scalarKFRun is the scalar Kalman recursion written out by hand, the
// closed-form reference the library's matrix machinery is pinned
// against: the same predict-and-correct cycle in plain arithmetic.
func scalarKFRun(z []float64, f, q, h, r, x0, p0 float64) (xs, ps, inns, ss []float64, logLik float64) {
	x, p := x0, p0
	xs = make([]float64, len(z))
	ps = make([]float64, len(z))
	inns = make([]float64, len(z))
	ss = make([]float64, len(z))
	for t, zt := range z {
		xp := f * x
		pp := f*p*f + q
		s := h*pp*h + r
		k := pp * h / s
		inn := zt - h*xp
		x = xp + k*inn
		p = pp - k*s*k // the standard form: (1−k·h)·pp
		xs[t], ps[t], inns[t], ss[t] = x, p, inn, s
		logLik += -0.5 * (math.Log(2*math.Pi*s) + inn*inn/s)
	}
	return xs, ps, inns, ss, logLik
}

// cholPD reports whether a symmetric row-major block factors as
// positive definite.
func cholPD(a []float64, n int) bool {
	l := make([]float64, n*n)
	for i := range n {
		for j := range i + 1 {
			total := a[i*n+j]
			for k := range j {
				total -= l[i*n+k] * l[j*n+k]
			}
			if i == j {
				if !(total > 0) {
					return false
				}
				l[i*n+i] = math.Sqrt(total)
			} else {
				l[i*n+j] = total / l[j*n+j]
			}
		}
	}
	return true
}

// kalmanResultBlock reads one (rows × cols) block out of a result
// stack at step t.
func kalmanResultBlock(t *testing.T, a *core.Array, step, rows, cols int) []float64 {
	t.Helper()
	block := make([]float64, rows*cols)
	for i := range rows * cols {
		block[i] = a.FloatAt(step*rows*cols + i)
	}
	return block
}

// TestKalmanScalarSteadyState pins the Riccati recursion against the
// scalar steady state solved by hand: for x_{t+1} = x_t + w,
// z_t = x_t + v with unit variances the fixed point of p⁻ = p + 1,
// p = p⁻·r/(p⁻ + r) is the golden ratio, and the filtered covariance
// settles on its reciprocal.
func TestKalmanScalarSteadyState(t *testing.T) {
	const n = 400
	g := core.NewGenerator(3)
	z := make([]float64, n)
	for i := range z {
		z[i] = g.NormalUnit()
	}
	f, err := KalmanFilter(mustFloats(t, z), mustFloats(t, []float64{1}, 1, 1), mustFloats(t, []float64{1}, 1, 1),
		KalmanOptions{ProcessNoise: mustFloats(t, []float64{1}, 1, 1), MeasurementNoise: mustFloats(t, []float64{1}, 1, 1)})
	if err != nil {
		t.Fatalf("KalmanFilter: %v", err)
	}
	wantP := (math.Sqrt(5) - 1) / 2 // 1/φ
	if got := f.Covariances.FloatAt(n - 1); math.Abs(got-wantP) > 1e-9 {
		t.Fatalf("steady filtered covariance = %.12f, want %.12f", got, wantP)
	}
	// The state path must match the hand-run scalar recursion.
	xs, _, _, _, _ := scalarKFRun(z, 1, 1, 1, 1, 0, 1)
	if got, want := f.States.FloatAt(n-1), xs[n-1]; math.Abs(got-want) > 1e-9 {
		t.Fatalf("steady filtered state = %.12f, hand recursion %.12f", got, want)
	}
}

// TestKalmanAR1Reference pins the filtered mean of a scalar AR(1) plus
// noise against the closed-form recursion, and the accumulated
// log-likelihood against the direct sum of the Gaussian log densities
// of the innovations and innovation variances the result itself
// publishes.
func TestKalmanAR1Reference(t *testing.T) {
	const (
		phi = 0.7
		q   = 0.04
		r   = 0.25
		n   = 80
	)
	g := core.NewGenerator(5)
	z := make([]float64, n)
	for i := range z {
		z[i] = g.NormalUnit()
	}
	f, err := KalmanFilter(mustFloats(t, z), mustFloats(t, []float64{phi}, 1, 1),
		mustFloats(t, []float64{1}, 1, 1),
		KalmanOptions{
			ProcessNoise:     mustFloats(t, []float64{q}, 1, 1),
			MeasurementNoise: mustFloats(t, []float64{r}, 1, 1),
		})
	if err != nil {
		t.Fatalf("KalmanFilter: %v", err)
	}
	xs, ps, inns, ss, wantLL := scalarKFRun(z, phi, q, 1, r, 0, 1)
	for step := range n {
		if got := f.States.FloatAt(step); math.Abs(got-xs[step]) > 1e-9 {
			t.Fatalf("filtered mean at %d = %.12f, want %.12f", step, got, xs[step])
		}
		if got := f.Covariances.FloatAt(step); math.Abs(got-ps[step]) > 1e-9 {
			t.Fatalf("filtered variance at %d = %.12f, want %.12f", step, got, ps[step])
		}
		if got := f.Innovations.FloatAt(step); math.Abs(got-inns[step]) > 1e-9 {
			t.Fatalf("innovation at %d = %.12f, want %.12f", step, got, inns[step])
		}
		if got := f.InnovationCovariances.FloatAt(step); math.Abs(got-ss[step]) > 1e-9 {
			t.Fatalf("innovation variance at %d = %.12f, want %.12f", step, got, ss[step])
		}
	}
	if math.Abs(f.LogLikelihood-wantLL) > 1e-7 {
		t.Fatalf("log-likelihood = %.10f, closed form %.10f", f.LogLikelihood, wantLL)
	}
	// The published likelihood must be the sum of the published
	// innovations' own Gaussian log densities.
	total := 0.0
	for t := range n {
		inn := f.Innovations.FloatAt(t)
		s := f.InnovationCovariances.FloatAt(t)
		total += -0.5 * (math.Log(2*math.Pi*s) + inn*inn/s)
	}
	if math.Abs(total-f.LogLikelihood) > 1e-9 {
		t.Fatalf("log-likelihood %.12f is not the innovations' own sum %.12f", f.LogLikelihood, total)
	}
}

// TestKalmanMatchesUKFLinear runs the unscented filter on an exactly
// linear model: the sigma-point transforms are exact there, so the
// UKF must degenerate to the Kalman answer to rounding.
func TestKalmanMatchesUKFLinear(t *testing.T) {
	const n = 120
	fMat := mustFloats(t, []float64{1, 1, 0, 1}, 2, 2)
	hMat := mustFloats(t, []float64{1, 0}, 1, 2)
	opts := KalmanOptions{
		InitialState:      mustFloats(t, []float64{1, 0}),
		InitialCovariance: mustFloats(t, []float64{4, 0, 0, 1}, 2, 2),
		ProcessNoise:      mustFloats(t, []float64{1e-4, 0, 0, 0.01}, 2, 2),
		MeasurementNoise:  mustFloats(t, []float64{0.25}, 1, 1),
	}
	g := core.NewGenerator(9)
	z := make([]float64, n)
	for i := range z {
		z[i] = g.NormalUnit()
	}
	zArr := mustFloats(t, z)
	kf, err := KalmanFilter(zArr, fMat, hMat, opts)
	if err != nil {
		t.Fatalf("KalmanFilter: %v", err)
	}
	linear := func(m *core.Array) StateFunc {
		return func(x *core.Array) (*core.Array, error) {
			rows := m.Len() / x.Len()
			out := make([]float64, rows)
			for i := range out {
				total := 0.0
				for j := range x.Len() {
					total += m.FloatAt(i*x.Len()+j) * x.FloatAt(j)
				}
				out[i] = total
			}
			return core.FromFloats(out, len(out))
		}
	}
	ukf, err := UnscentedKalmanFilter(zArr, linear(fMat), linear(hMat), opts)
	if err != nil {
		t.Fatalf("UnscentedKalmanFilter: %v", err)
	}
	for step := range n {
		for i := range 2 {
			got, want := ukf.States.FloatAt(step*2+i), kf.States.FloatAt(step*2+i)
			if math.Abs(got-want) > 1e-8 {
				t.Fatalf("UKF state (%d, %d) = %.10f against KF %.10f", step, i, got, want)
			}
		}
		for i := range 4 {
			got, want := ukf.Covariances.FloatAt(step*4+i), kf.Covariances.FloatAt(step*4+i)
			if math.Abs(got-want) > 1e-8 {
				t.Fatalf("UKF covariance (%d, %d) = %.10f against KF %.10f", step, i, got, want)
			}
		}
	}
	if math.Abs(ukf.LogLikelihood-kf.LogLikelihood) > 1e-6 {
		t.Fatalf("UKF log-likelihood %.10f against KF %.10f", ukf.LogLikelihood, kf.LogLikelihood)
	}
	// The extended filter with central-difference Jacobians sees the
	// same linear maps exactly, so it must land on the KF too.
	ekf, err := ExtendedKalmanFilter(zArr, linear(fMat), linear(hMat), opts)
	if err != nil {
		t.Fatalf("ExtendedKalmanFilter: %v", err)
	}
	for step := range n {
		for i := range 2 {
			got, want := ekf.States.FloatAt(step*2+i), kf.States.FloatAt(step*2+i)
			if math.Abs(got-want) > 1e-8 {
				t.Fatalf("EKF state (%d, %d) = %.10f against KF %.10f", step, i, got, want)
			}
		}
	}
}

// TestUnscentedQuadraticMoment pins the sigma-point transform on a
// genuinely nonlinear map: with x ~ N(0, 2) and f(x) = x², the exact
// predicted second moment is 2, and with α = 1, κ = 0 the symmetric
// sigma pair reproduces it exactly. The inert measurement noise keeps
// the correction from moving the answer.
func TestUnscentedQuadraticMoment(t *testing.T) {
	square := func(x *core.Array) (*core.Array, error) {
		return core.FromFloats([]float64{x.FloatAt(0) * x.FloatAt(0)}, 1)
	}
	identity := func(x *core.Array) (*core.Array, error) {
		return core.FromFloats([]float64{x.FloatAt(0)}, 1)
	}
	f, err := UnscentedKalmanFilter(mustFloats(t, []float64{0}), square, identity, KalmanOptions{
		InitialState:      mustFloats(t, []float64{0}),
		InitialCovariance: mustFloats(t, []float64{2}, 1, 1),
		ProcessNoise:      mustFloats(t, []float64{0}, 1, 1),
		MeasurementNoise:  mustFloats(t, []float64{1e18}, 1, 1),
		SigmaAlpha:        1,
		SigmaBeta:         2,
		SigmaKappa:        0,
	})
	if err != nil {
		t.Fatalf("UnscentedKalmanFilter: %v", err)
	}
	if got, want := f.States.FloatAt(0), 2.0; math.Abs(got-want) > 1e-9 {
		t.Fatalf("predicted second moment = %.12f, want %.12f", got, want)
	}
}

// TestExtendedKalmanBearingOnly tracks a constant-velocity target from
// bearing-only measurements, the classical case where the observation
// map is genuinely nonlinear and the posterior is not Gaussian. The
// honest pin is Monte Carlo: a bootstrap particle filter over the same
// model and data approximates the true posterior mean, and both the
// extended and the unscented filter must land within a few posterior
// standard deviations of it, and of each other.
func TestExtendedKalmanBearingOnly(t *testing.T) {
	const (
		n       = 60
		sigmaB  = 0.005
		truthX0 = 2000.0
		truthY0 = 100.0
		truthVX = -15.0
		truthVY = 3.0
	)
	// Truth and bearings, through the house generator.
	g := core.NewGenerator(101)
	z := make([]float64, n)
	for t := range n {
		px := truthX0 + truthVX*float64(t+1)
		py := truthY0 + truthVY*float64(t+1)
		z[t] = math.Atan2(py, px) + sigmaB*g.NormalUnit()
	}
	cv := func(x *core.Array) (*core.Array, error) {
		return core.FromFloats([]float64{
			x.FloatAt(0) + x.FloatAt(2),
			x.FloatAt(1) + x.FloatAt(3),
			x.FloatAt(2),
			x.FloatAt(3),
		}, 4)
	}
	bearing := func(x *core.Array) (*core.Array, error) {
		return core.FromFloats([]float64{math.Atan2(x.FloatAt(1), x.FloatAt(0))}, 1)
	}
	bearingJac := func(x *core.Array) (*core.Array, error) {
		px, py := x.FloatAt(0), x.FloatAt(1)
		r2 := px*px + py*py
		return core.FromFloats([]float64{-py / r2, px / r2, 0, 0}, 1, 4)
	}
	opts := KalmanOptions{
		InitialState:        mustFloats(t, []float64{1900, 80, 0, 0}),
		InitialCovariance:   mustFloats(t, []float64{1e4, 0, 0, 0, 0, 1e4, 0, 0, 0, 0, 25, 0, 0, 0, 0, 25}, 4, 4),
		ProcessNoise:        mustFloats(t, []float64{1e-4, 0, 0, 0, 0, 1e-4, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0.01}, 4, 4),
		MeasurementNoise:    mustFloats(t, []float64{sigmaB * sigmaB}, 1, 1),
		ObservationJacobian: bearingJac,
	}
	zArr := mustFloats(t, z)
	ekf, err := ExtendedKalmanFilter(zArr, cv, bearing, opts)
	if err != nil {
		t.Fatalf("ExtendedKalmanFilter: %v", err)
	}
	ukf, err := UnscentedKalmanFilter(zArr, cv, bearing, opts)
	if err != nil {
		t.Fatalf("UnscentedKalmanFilter: %v", err)
	}
	// The bootstrap particle filter: 20000 particles propagated
	// through the same dynamics, weighted by the bearing likelihood,
	// systematically resampled every step.
	const particles = 20000
	pg := core.NewGenerator(202)
	px := make([]float64, particles)
	py := make([]float64, particles)
	vx := make([]float64, particles)
	vy := make([]float64, particles)
	weights := make([]float64, particles)
	for i := range particles {
		px[i] = 1900 + 100*pg.NormalUnit()
		py[i] = 80 + 100*pg.NormalUnit()
		vx[i] = 0 + 5*pg.NormalUnit()
		vy[i] = 0 + 5*pg.NormalUnit()
	}
	var meanX, meanY, varX, varY float64
	for t := range n {
		for i := range particles {
			px[i] += vx[i] + 0.01*pg.NormalUnit()
			py[i] += vy[i] + 0.01*pg.NormalUnit()
			vx[i] += 0.1 * pg.NormalUnit()
			vy[i] += 0.1 * pg.NormalUnit()
		}
		// Log weights against this step's bearing.
		maxLog := math.Inf(-1)
		for i := range particles {
			res := (z[t] - math.Atan2(py[i], px[i])) / sigmaB
			weights[i] = -0.5 * res * res
			maxLog = max(maxLog, weights[i])
		}
		total := 0.0
		for i := range particles {
			weights[i] = math.Exp(weights[i] - maxLog)
			total += weights[i]
		}
		// The weighted posterior moments, recorded at the last step.
		if t == n-1 {
			for i := range particles {
				w := weights[i] / total
				meanX += w * px[i]
				meanY += w * py[i]
			}
			for i := range particles {
				w := weights[i] / total
				varX += w * (px[i] - meanX) * (px[i] - meanX)
				varY += w * (py[i] - meanY) * (py[i] - meanY)
			}
		}
		// Systematic resampling: one uniform start, N evenly spaced
		// pointers walked through the cumulative weights.
		ancestorsX := make([]float64, particles)
		ancestorsY := make([]float64, particles)
		ancestorsVX := make([]float64, particles)
		ancestorsVY := make([]float64, particles)
		copy(ancestorsX, px)
		copy(ancestorsY, py)
		copy(ancestorsVX, vx)
		copy(ancestorsVY, vy)
		u := pg.Unit() / particles
		cum := 0.0
		j := 0
		cum += weights[0] / total
		for i := range particles {
			pointer := u + float64(i)/particles
			for cum < pointer && j < particles-1 {
				j++
				cum += weights[j] / total
			}
			px[i] = ancestorsX[j]
			py[i] = ancestorsY[j]
			vx[i] = ancestorsVX[j]
			vy[i] = ancestorsVY[j]
		}
	}
	// The EKF and UKF positions against the Monte Carlo posterior,
	// with a tolerance of five posterior standard deviations.
	for name, state := range map[string]float64{
		"ekf x": ekf.States.FloatAt((n-1)*4 + 0),
		"ekf y": ekf.States.FloatAt((n-1)*4 + 1),
		"ukf x": ukf.States.FloatAt((n-1)*4 + 0),
		"ukf y": ukf.States.FloatAt((n-1)*4 + 1),
	} {
		want, spread := meanX, 5*math.Sqrt(varX)
		if strings.HasSuffix(name, "y") {
			want, spread = meanY, 5*math.Sqrt(varY)
		}
		if math.Abs(state-want) > spread {
			t.Fatalf("%s = %.3f, Monte Carlo posterior mean %.3f with sd %.3f",
				name, state, want, math.Sqrt(spread*spread/25))
		}
	}
	// The two filters agree with each other to well under a posterior
	// standard deviation.
	for i, name := range []string{"x", "y"} {
		d := math.Abs(ekf.States.FloatAt((n-1)*4+i) - ukf.States.FloatAt((n-1)*4+i))
		spread := math.Sqrt(varX)
		if i == 1 {
			spread = math.Sqrt(varY)
		}
		if d > spread {
			t.Fatalf("EKF and UKF %s differ by %.3f, a posterior sd of %.3f", name, d, spread)
		}
	}
	// Plausible posterior: the filtered position tracks the truth
	// within four of its own standard deviations, and every published
	// covariance stayed symmetric and positive definite.
	cov := kalmanResultBlock(t, ekf.Covariances, n-1, 4, 4)
	errX := math.Abs(ekf.States.FloatAt((n-1)*4+0) - (truthX0 + truthVX*float64(n)))
	errY := math.Abs(ekf.States.FloatAt((n-1)*4+1) - (truthY0 + truthVY*float64(n)))
	if errX > 4*math.Sqrt(cov[0]) || errY > 4*math.Sqrt(cov[5]) {
		t.Fatalf("final position error (%.3f, %.3f) exceeds four filtered sds (%.3f, %.3f)",
			errX, errY, 4*math.Sqrt(cov[0]), 4*math.Sqrt(cov[5]))
	}
	if !cholPD(cov, 4) {
		t.Fatal("the final EKF covariance is not positive definite")
	}
}

// TestKalmanMultiChannel pins the multi-channel path: two observed
// states with cross-coupled noise, against an independent two-by-two
// reference recursion written in the test.
func TestKalmanMultiChannel(t *testing.T) {
	const n = 100
	g := core.NewGenerator(13)
	z := make([]float64, 2*n)
	for i := range z {
		z[i] = g.NormalUnit()
	}
	f, err := KalmanFilter(mustFloats(t, z, n, 2), mustFloats(t, []float64{1, 1, 0, 1}, 2, 2),
		mustFloats(t, []float64{1, 0, 0, 1}, 2, 2),
		KalmanOptions{
			ProcessNoise:     mustFloats(t, []float64{1e-4, 0, 0, 1e-2}, 2, 2),
			MeasurementNoise: mustFloats(t, []float64{0.1, 0, 0, 0.4}, 2, 2),
		})
	if err != nil {
		t.Fatalf("KalmanFilter: %v", err)
	}
	// The independent reference: the same recursion with a
	// standard-form update and an explicit two-by-two solve.
	x := []float64{0, 0}
	p := []float64{1, 0, 0, 1}
	logLik := 0.0
	solve2 := func(s []float64, b []float64) []float64 {
		det := s[0]*s[3] - s[1]*s[2]
		return []float64{(s[3]*b[0] - s[1]*b[1]) / det, (s[0]*b[1] - s[2]*b[0]) / det}
	}
	// mat2 multiplies two row-major two-by-two matrices.
	mat2 := func(x, y []float64) []float64 {
		return []float64{
			x[0]*y[0] + x[1]*y[2], x[0]*y[1] + x[1]*y[3],
			x[2]*y[0] + x[3]*y[2], x[2]*y[1] + x[3]*y[3],
		}
	}
	for step := range n {
		xp := []float64{x[0] + x[1], x[1]}
		pp := []float64{
			p[0] + p[1] + p[2] + p[3] + 1e-4, p[1] + p[3],
			p[2] + p[3], p[3] + 1e-2,
		}
		inn := []float64{z[2*step] - xp[0], z[2*step+1] - xp[1]}
		s := []float64{pp[0] + 0.1, pp[1], pp[2], pp[3] + 0.4}
		k := [][]float64{
			solve2(s, []float64{pp[0], pp[2]}),
			solve2(s, []float64{pp[1], pp[3]}),
		}
		solved := solve2(s, inn)
		quad := inn[0]*solved[0] + inn[1]*solved[1]
		det := s[0]*s[3] - s[1]*s[2]
		logLik += -0.5 * (2*math.Log(2*math.Pi) + math.Log(det) + quad)
		x = []float64{xp[0] + k[0][0]*inn[0] + k[0][1]*inn[1], xp[1] + k[1][0]*inn[0] + k[1][1]*inn[1]}
		a := []float64{1 - k[0][0], -k[0][1], -k[1][0], 1 - k[1][1]}
		at := []float64{a[0], a[2], a[1], a[3]}
		kr := []float64{k[0][0] * 0.1, k[0][1] * 0.4, k[1][0] * 0.1, k[1][1] * 0.4}
		kt := []float64{k[0][0], k[1][0], k[0][1], k[1][1]}
		aph := mat2(a, pp)
		joseph := mat2(aph, at)
		krkt := mat2(kr, kt)
		p = []float64{joseph[0] + krkt[0], joseph[1] + krkt[1], joseph[2] + krkt[2], joseph[3] + krkt[3]}
		if got := f.States.FloatAt(2 * step); math.Abs(got-x[0]) > 1e-9 {
			t.Fatalf("filtered mean 0 at %d = %.10f, want %.10f", step, got, x[0])
		}
		if got := f.States.FloatAt(2*step + 1); math.Abs(got-x[1]) > 1e-9 {
			t.Fatalf("filtered mean 1 at %d = %.10f, want %.10f", step, got, x[1])
		}
	}
	if math.Abs(f.LogLikelihood-logLik) > 1e-6 {
		t.Fatalf("log-likelihood %.8f, reference %.8f", f.LogLikelihood, logLik)
	}
}

// TestKalmanCovarianceGuards walks a long filter and demands every
// published covariance come back symmetric and positive definite,
// then pins the input gates and the step-named failure reports.
func TestKalmanCovarianceGuards(t *testing.T) {
	const n = 200
	g := core.NewGenerator(17)
	z := make([]float64, n)
	for i := range z {
		z[i] = g.NormalUnit()
	}
	f, err := KalmanFilter(mustFloats(t, z), mustFloats(t, []float64{1, 1, 0, 1}, 2, 2),
		mustFloats(t, []float64{1, 0}, 1, 2),
		KalmanOptions{
			ProcessNoise:     mustFloats(t, []float64{1e-4, 0, 0, 1e-2}, 2, 2),
			MeasurementNoise: mustFloats(t, []float64{0.25}, 1, 1),
		})
	if err != nil {
		t.Fatalf("KalmanFilter: %v", err)
	}
	for step := range n {
		cov := kalmanResultBlock(t, f.Covariances, step, 2, 2)
		if math.Abs(cov[1]-cov[2]) > 1e-12*math.Max(math.Abs(cov[1]), math.Abs(cov[2])) {
			t.Fatalf("covariance at %d is not symmetric: %g against %g", step, cov[1], cov[2])
		}
		if !cholPD(cov, 2) {
			t.Fatalf("covariance at %d is not positive definite", step)
		}
		if !cholPD(kalmanResultBlock(t, f.InnovationCovariances, step, 1, 1), 1) {
			t.Fatalf("innovation covariance at %d is not positive definite", step)
		}
	}
	// The input gates.
	good := mustFloats(t, []float64{1, 1, 0, 1}, 2, 2)
	hGood := mustFloats(t, []float64{1, 0}, 1, 2)
	zOne := mustFloats(t, []float64{0.5})
	bad := []struct {
		name string
		run  func() (*KalmanResult, error)
	}{
		{"nil transition", func() (*KalmanResult, error) { return KalmanFilter(zOne, nil, hGood, KalmanOptions{}) }},
		{"non-square transition", func() (*KalmanResult, error) {
			return KalmanFilter(zOne, mustFloats(t, []float64{1, 1}, 1, 2), hGood, KalmanOptions{})
		}},
		{"observation width mismatch", func() (*KalmanResult, error) {
			return KalmanFilter(zOne, good, mustFloats(t, []float64{1, 0, 0}, 1, 3), KalmanOptions{})
		}},
		{"asymmetric process noise", func() (*KalmanResult, error) {
			return KalmanFilter(zOne, good, hGood, KalmanOptions{ProcessNoise: mustFloats(t, []float64{1, 1, 0, 1}, 2, 2)})
		}},
		{"singular measurement noise", func() (*KalmanResult, error) {
			return KalmanFilter(zOne, good, hGood, KalmanOptions{MeasurementNoise: mustFloats(t, []float64{0}, 1, 1)})
		}},
		{"non-positive-definite initial covariance", func() (*KalmanResult, error) {
			return KalmanFilter(zOne, good, hGood, KalmanOptions{
				InitialCovariance: mustFloats(t, []float64{1, 0, 0, -1}, 2, 2)})
		}},
		{"empty measurements", func() (*KalmanResult, error) {
			return KalmanFilter(mustFloats(t, nil), good, hGood, KalmanOptions{})
		}},
		{"non-finite measurement", func() (*KalmanResult, error) {
			return KalmanFilter(mustFloats(t, []float64{math.NaN()}), good, hGood, KalmanOptions{})
		}},
		{"rank-3 measurements", func() (*KalmanResult, error) {
			return KalmanFilter(mustFloats(t, []float64{1, 0, 1, 0, 1, 0, 1, 0}, 2, 2, 2), good, hGood, KalmanOptions{})
		}},
	}
	for _, b := range bad {
		if _, err := b.run(); err == nil {
			t.Errorf("%s accepted", b.name)
		}
	}
	// The nonlinear filters need the state dimension spelled out.
	cv := func(x *core.Array) (*core.Array, error) { return x, nil }
	id := func(x *core.Array) (*core.Array, error) { return x, nil }
	if _, err := ExtendedKalmanFilter(zOne, cv, id, KalmanOptions{}); err == nil {
		t.Error("the extended filter accepted no initial state")
	}
	if _, err := UnscentedKalmanFilter(zOne, cv, id, KalmanOptions{}); err == nil {
		t.Error("the unscented filter accepted no initial state")
	}
	if _, err := UnscentedKalmanFilter(zOne, cv, id, KalmanOptions{
		InitialState: mustFloats(t, []float64{0}),
		SigmaAlpha:   -1,
	}); err == nil {
		t.Error("the unscented filter accepted a negative alpha")
	}
	if _, err := UnscentedKalmanFilter(zOne, cv, id, KalmanOptions{
		InitialState: mustFloats(t, []float64{0}),
		SigmaAlpha:   1e-3,
		SigmaKappa:   -2,
	}); err == nil {
		t.Error("the unscented filter accepted a kappa that kills the sigma scale")
	}
	// A singular state covariance mid-run is refused with the step
	// named: a transition that annihilates every state with zero
	// process noise collapses the predicted spread onto a point, and
	// the next predict cannot factor it.
	annihilate := func(x *core.Array) (*core.Array, error) {
		return core.FromFloats([]float64{0}, 1)
	}
	_, err = UnscentedKalmanFilter(mustFloats(t, []float64{0, 0}), annihilate, id, KalmanOptions{
		InitialState:      mustFloats(t, []float64{0}),
		InitialCovariance: mustFloats(t, []float64{2}, 1, 1),
		ProcessNoise:      mustFloats(t, []float64{0}, 1, 1),
		MeasurementNoise:  mustFloats(t, []float64{1e18}, 1, 1),
		SigmaAlpha:        1,
	})
	if err == nil || !strings.Contains(err.Error(), "at step 1") {
		t.Fatalf("the collapsed sigma spread was not reported at step 1, got %v", err)
	}
}

// TestKalmanNonlinearGates pins the validation branches the nonlinear
// filters own: callback presence, the state dimension's sources, the
// complex and shape gates on every array the filters read, and the
// step-named reports for failing or misbehaving callbacks.
func TestKalmanNonlinearGates(t *testing.T) {
	identity := func(x *core.Array) (*core.Array, error) {
		return core.FromFloats([]float64{x.FloatAt(0)}, 1)
	}
	doubling := func(x *core.Array) (*core.Array, error) {
		return core.FromFloats([]float64{2 * x.FloatAt(0)}, 1)
	}
	base := KalmanOptions{
		InitialState:      mustFloats(t, []float64{1}),
		InitialCovariance: mustFloats(t, []float64{1}, 1, 1),
		MeasurementNoise:  mustFloats(t, []float64{1}, 1, 1),
	}
	zOne := mustFloats(t, []float64{1})
	if _, err := ExtendedKalmanFilter(zOne, nil, identity, base); err == nil {
		t.Error("the extended filter accepted a nil transition")
	}
	if _, err := UnscentedKalmanFilter(zOne, identity, nil, base); err == nil {
		t.Error("the unscented filter accepted a nil observation")
	}
	// The state dimension may come from the initial covariance alone.
	withoutState := base
	withoutState.InitialState = nil
	run, err := UnscentedKalmanFilter(zOne, doubling, identity, withoutState)
	if err != nil {
		t.Fatalf("the unscented filter refused a dimension from the initial covariance: %v", err)
	}
	// The gain from a unit prior under the doubling map: pp = 4,
	// s = 5, K = 4/5, and the measurement 1 lands the state at 4/5.
	if math.Abs(run.States.FloatAt(0)-0.8) > 1e-12 {
		t.Fatalf("state %.12g from a covariance-only start, want 0.8", run.States.FloatAt(0))
	}
	badRank, _ := core.FromFloats([]float64{1}, 1)
	covBadRank := withoutState
	covBadRank.InitialCovariance = badRank
	if _, err := ExtendedKalmanFilter(zOne, doubling, identity, covBadRank); err == nil {
		t.Error("a rank-1 initial covariance accepted")
	}
	// The complex gates on the arrays the filters read.
	complexState, _ := core.ComplexFromArray([]complex128{1}, 1)
	withComplex := base
	withComplex.InitialState = complexState
	if _, err := ExtendedKalmanFilter(zOne, doubling, identity, withComplex); err == nil {
		t.Error("a complex initial state accepted")
	}
	complexCov, _ := core.ComplexFromArray([]complex128{1}, 1, 1)
	withComplexCov := withoutState
	withComplexCov.InitialCovariance = complexCov
	if _, err := ExtendedKalmanFilter(zOne, doubling, identity, withComplexCov); err == nil {
		t.Error("a complex initial covariance accepted")
	}
	complexZ, _ := core.ComplexFromArray([]complex128{1}, 1)
	if _, err := ExtendedKalmanFilter(complexZ, doubling, identity, base); err == nil {
		t.Error("complex measurements accepted")
	}
	if _, err := UnscentedKalmanFilter(complexZ, doubling, identity, base); err == nil {
		t.Error("complex measurements accepted by the unscented filter")
	}
	// A measurement stack with zero width is refused before the walk.
	empty, _ := core.FromFloats([]float64{}, 2, 0)
	if _, err := ExtendedKalmanFilter(empty, doubling, identity, base); err == nil {
		t.Error("zero-width measurements accepted")
	}
	// A misbehaving transition is reported with the step.
	exploding := func(x *core.Array) (*core.Array, error) {
		return nil, errors.New("no")
	}
	_, err = ExtendedKalmanFilter(mustFloats(t, []float64{1, 1}), exploding, identity, base)
	if err == nil || !strings.Contains(err.Error(), "the transition") || !strings.Contains(err.Error(), "at step 1") {
		t.Fatalf("a failing transition was not reported at its step, got %v", err)
	}
	// A transition with the wrong output length likewise.
	wrong := func(x *core.Array) (*core.Array, error) {
		return core.FromFloats([]float64{1, 2}, 2)
	}
	if _, err := ExtendedKalmanFilter(mustFloats(t, []float64{1, 1}), wrong, identity, base); err == nil {
		t.Error("a transition with the wrong output length accepted")
	}
	if _, err := UnscentedKalmanFilter(mustFloats(t, []float64{1, 1}), identity, wrong, base); err == nil {
		t.Error("an observation with the wrong output length accepted")
	}
	// A non-finite transition output is refused where it appears.
	nonFinite := func(x *core.Array) (*core.Array, error) {
		return core.FromFloats([]float64{math.NaN()}, 1)
	}
	if _, err := UnscentedKalmanFilter(mustFloats(t, []float64{1, 1}), nonFinite, identity, base); err == nil {
		t.Error("a non-finite transition output accepted")
	}
	// A Jacobian of the wrong shape or a failing one is reported.
	badJac := func(x *core.Array) (*core.Array, error) {
		return core.FromFloats([]float64{1, 1}, 2, 1)
	}
	withBadJac := base
	withBadJac.ObservationJacobian = badJac
	if _, err := ExtendedKalmanFilter(mustFloats(t, []float64{1, 1}), doubling, identity, withBadJac); err == nil {
		t.Error("an observation Jacobian of the wrong shape accepted")
	}
	errJac := func(x *core.Array) (*core.Array, error) {
		return nil, errors.New("no")
	}
	withErrJac := base
	withErrJac.TransitionJacobian = errJac
	_, err = ExtendedKalmanFilter(mustFloats(t, []float64{1, 1}), doubling, identity, withErrJac)
	if err == nil || !strings.Contains(err.Error(), "the transition Jacobian") {
		t.Fatalf("a failing transition Jacobian was not reported, got %v", err)
	}
	withErrObsJac := base
	withErrObsJac.ObservationJacobian = errJac
	_, err = ExtendedKalmanFilter(mustFloats(t, []float64{1, 1}), doubling, identity, withErrObsJac)
	if err == nil || !strings.Contains(err.Error(), "the observation Jacobian") {
		t.Fatalf("a failing observation Jacobian was not reported, got %v", err)
	}
	// The linear filter's rank gate on the transition matrix.
	if _, err := KalmanFilter(mustFloats(t, []float64{1}), mustFloats(t, []float64{1, 1}, 1, 2),
		mustFloats(t, []float64{1, 0}, 1, 2), KalmanOptions{}); err == nil {
		t.Error("a rank-1 transition matrix accepted")
	}
}