/**
 **
 ** IntersceneLmaFloat - Primitive float pose projection/Jacobian oracle
 **
 ** Copyright (C) 2026 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  IntersceneLmaFloat.java is free software: you can redistribute it and/or modify
 **  it under the terms of the GNU General Public License as published by
 **  the Free Software Foundation, either version 3 of the License, or
 **  (at your option) any later version.
 **
 **  This program is distributed in the hope that it will be useful,
 **  but WITHOUT ANY WARRANTY; without even the implied warranty of
 **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 **  GNU General Public License for more details.
 **
 **  You should have received a copy of the GNU General Public License
 **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ** -----------------------------------------------------------------------------**
 **
 */

package com.elphel.imagej.tileprocessor;

import java.util.Arrays;

import Jama.Matrix;

/**
 * Primitive single-precision oracle for the CUAS three-angle pose Jacobian.
 *
 * <p>This is deliberately not a second general-purpose LMA implementation. It
 * clones only the useful {@code IntersceneLma.getFxDerivs()} boundary for the
 * current rotation-only pose fit: one independent projection/Jacobian result
 * per selected tile, flattened exactly as the CUDA implementation will consume
 * it. The legacy double implementation remains the behavioral oracle.</p>
 */
public final class IntersceneLmaFloat {
	public static final int NUM_PARAMS = 3;
	public static final int NUM_COMPONENTS = 2;
	public static final int LMA_PRODUCTS = NUM_PARAMS * NUM_PARAMS + NUM_PARAMS;
	public static final int LMA_RESULT_FLOATS = 1 + LMA_PRODUCTS + 2 * NUM_PARAMS;
	public static final int LMA_RESIDENT_RESULT_FLOATS = LMA_RESULT_FLOATS + 6;
	public static final int PREP_RESULT_FLOATS = 8; // rung C1 resident prepare compact result // By Claude on 07/17/2026
	public static final float INFINITY_DISPARITY = 0.01f;

	static final int ERS_XYZ = 0;
	static final int ERS_XYZ_DT = 3;
	static final int ERS_QUAT = 6;
	static final int ERS_QUAT_DT = 10;
	static final int ERS_LINE_STRIDE = 14;

	private static final float SCENE_UNITS_SCALE = 0.001f;
	private static final float LINE_ERR = 0.001f;
	private static final float PROJECTION_EPS = 1.0e-10f;
	private static final int PROJECTION_MAX_STEPS = 100;

	private IntersceneLmaFloat() {}

	/** Immutable primitive camera state captured after ERS setup. */
	public static final class Camera {
		public final int width;
		public final int height;
		public final float focalLength;
		public final float pixelSize;
		public final float distortionRadius;
		public final float disparityRadius;
		public final float lineTime;
		public final float stepR;
		public final float [] radial;
		public final float [] rByRDist;
		public final float [] ers;

		private Camera(final ErsCorrection source) {
			width = source.pixelCorrectionWidth;
			height = source.pixelCorrectionHeight;
			focalLength = (float) source.focalLength;
			pixelSize = (float) source.pixelSize;
			distortionRadius = (float) source.distortionRadius;
			disparityRadius = (float) source.disparityRadius;
			lineTime = (float) source.line_time;
			stepR = (float) source.getStepR();
			radial = new float [] {
					(float) source.distortionC,
					(float) source.distortionB,
					(float) source.distortionA,
					(float) source.distortionA5,
					(float) source.distortionA6,
					(float) source.distortionA7,
					(float) source.distortionA8};
			double [] table = source.getRByRDist();
			if (table == null) {
				source.getRByRDist(0.0, false); // lazily build the legacy table
				table = source.getRByRDist();
			}
			rByRDist = new float [table.length];
			for (int i = 0; i < table.length; i++) rByRDist[i] = (float) table[i];
			ers = source.copyErsPoseStateFloat();
		}

		public static Camera capture(final ErsCorrection source) {
			if (source == null) throw new IllegalArgumentException("null ErsCorrection");
			return new Camera(source);
		}
	}

	/** Flattened output: fx[2*n], optional parameter-major jt[3][2*n], and tile validity. */
	public static final class Result {
		public final int numTiles;
		public final float [] fx;
		public final float [] jt;
		public final byte [] valid;
		public final int numValid;

		public Result(
				final int numTiles,
				final float [] fx,
				final float [] jt,
				final byte [] valid,
				final int numValid) {
			this.numTiles = numTiles;
			this.fx = fx;
			this.jt = jt;
			this.valid = valid;
			this.numValid = numValid;
		}
	}

	/** Statistics for comparing two flattened float implementations. */
	public static final class ResultComparison {
		public int compared;
		public int actualOnlyValid;
		public int expectedOnlyValid;
		public float maxAbsFx;
		public float maxAbsJt;

		public String format(final String actualLabel, final String expectedLabel) {
			return String.format(
					"compared=%d, %s-only=%d, %s-only=%d, max|fx|=%g, max|J|=%g",
					compared, actualLabel, actualOnlyValid, expectedLabel, expectedOnlyValid,
					maxAbsFx, maxAbsJt);
		}
	}

	/** Statistics for the optional float-vs-current-double comparison gate. */
	public static final class Comparison {
		public int compared;
		public int floatOnlyValid;
		public int doubleOnlyValid;
		public float maxAbsFx;
		public float maxAbsJt;

		@Override
		public String toString() {
			return String.format(
					"float getFxDerivs: compared=%d, float-only=%d, double-only=%d, max|fx|=%g, max|J|=%g",
					compared, floatOnlyValid, doubleOnlyValid, maxAbsFx, maxAbsJt);
		}
	}

	/** One fixed three-angle LMA candidate: raw H/b, delta and updated parameter vector. */
	public static final class LmaStepResult {
		public final boolean valid;
		public final float [] products; // row-major H[3][3], then b[3], before damping
		public final float [] delta;
		public final float [] candidate;

		public LmaStepResult(
				final boolean valid,
				final float [] products,
				final float [] delta,
				final float [] candidate) {
			this.valid = valid;
			this.products = products;
			this.delta = delta;
			this.candidate = candidate;
		}
	}

	/** Statistics for checking the one-thread CUDA step against the Java float clone. */
	public static final class LmaStepComparison {
		public boolean validityMatch;
		public boolean bitExact;
		public float maxAbsProducts;
		public float maxAbsDelta;
		public float maxAbsCandidate;

		public String format() {
			return String.format(
					"valid-match=%s, bit-exact=%s, max|H,b|=%g, max|delta|=%g, max|candidate|=%g",
					validityMatch, bitExact, maxAbsProducts, maxAbsDelta, maxAbsCandidate);
		}
	}

	/** Float eigen/residual preparation immediately downstream of raw resident fx/J. */
	public static final class PreparedLma {
		public final float [] jt;
		public final float [] ymfxWeighted;
		public final float rms;
		public final float pureRms;

		public PreparedLma(
				final float [] jt,
				final float [] ymfxWeighted,
				final float rms,
				final float pureRms) {
			this.jt = jt;
			this.ymfxWeighted = ymfxWeighted;
			this.rms = rms;
			this.pureRms = pureRms;
		}
	}

	/** Compact resident step plus optional prepared arrays for a one-shot oracle gate. */
	/** Rung C1 (design 2026-07-16): compact result of the resident prepareLMA.
	 *  capturedY/Weights/Eigen are non-null only on the one-shot oracle call.
	 *  By Claude on 07/17/2026. */
	public static final class PrepareResidentResult {
		public final int numTiles;
		public final boolean valid;
		public final int numMeas;
		public final float sumWRaw;
		public final float pureWeight;
		public final float fullWeight;
		public final float [] regWeights;
		public final float [] capturedY;
		public final float [] capturedWeights;
		public final float [] capturedEigen;

		public PrepareResidentResult(
				final int numTiles,
				final boolean valid,
				final int numMeas,
				final float sumWRaw,
				final float pureWeight,
				final float fullWeight,
				final float [] regWeights,
				final float [] capturedY,
				final float [] capturedWeights,
				final float [] capturedEigen) {
			this.numTiles = numTiles;
			this.valid = valid;
			this.numMeas = numMeas;
			this.sumWRaw = sumWRaw;
			this.pureWeight = pureWeight;
			this.fullWeight = fullWeight;
			this.regWeights = regWeights;
			this.capturedY = capturedY;
			this.capturedWeights = capturedWeights;
			this.capturedEigen = capturedEigen;
		}
	}

	/** Serial float clone of the GPU resident prepare - the bit-exact oracle for
	 *  the captured buffers (assemble scatter is order-free; both tails are
	 *  serial in ascending order on the GPU by design). By Claude on 07/17/2026. */
	public static final class PrepareResidentOracle {
		public final float [] y;
		public final float [] weights;
		public final float [] eigen;
		public final int numMeas;
		public final float sumWRaw;
		public final float pureWeight;
		public final float fullWeight;
		public final float [] regWeights;

		PrepareResidentOracle(final float [] y, final float [] weights, final float [] eigen,
				final int numMeas, final float sumWRaw, final float pureWeight,
				final float fullWeight, final float [] regWeights) {
			this.y = y;
			this.weights = weights;
			this.eigen = eigen;
			this.numMeas = numMeas;
			this.sumWRaw = sumWRaw;
			this.pureWeight = pureWeight;
			this.fullWeight = fullWeight;
			this.regWeights = regWeights;
		}
	}

	public static PrepareResidentOracle prepareResidentOracle(
			final double [][] vectorXYSDS, // exact float widenings of the peak dx/dy/str
			final double [][] eigenSrc,    // {e0x,e0y,l0,l1} per tile, or null = identity mode
			final Result raw,              // raw float fx/J/valid at the current pose (readback)
			final float eigMinSqrt,
			final float eigMaxSqrt,
			final float minConfidence,
			final boolean sameWeights,
			final float [] pullTargets,
			final float [] paramRegweights) {
		final int numTiles = raw.numTiles;
		final int ns = NUM_COMPONENTS * numTiles;
		final int numValues = ns + NUM_PARAMS;
		final float [] y = new float [numValues];
		final float [] weights = new float [numValues];
		final float [] eigen = new float [4 * numTiles];
		// assemble (per-tile, order-free scatter on the GPU)
		final boolean useEigen = (eigenSrc != null);
		for (int tile = 0; tile < numTiles; tile++) {
			if (vectorXYSDS[tile] == null) continue; // unmeasured / kernel-rejected
			final float str = (float) vectorXYSDS[tile][2];
			if (!(str >= minConfidence)) continue;   // NaN fails here too
			float e0x = 0f, e0y = 0f, l0 = 0f, l1 = 0f;
			if (useEigen) {
				if (eigenSrc[tile] == null) continue; // null transform skips the tile
				e0x = (float) eigenSrc[tile][0];
				e0y = (float) eigenSrc[tile][1];
				l0 =  (float) eigenSrc[tile][2];
				l1 =  (float) eigenSrc[tile][3];
				if (!Float.isFinite(e0x) || !Float.isFinite(e0y) ||
						!Float.isFinite(l0) || !Float.isFinite(l1)) continue;
			}
			if (!(str > 0.0f)) continue;
			final float w = sameWeights ? 1.0f : str;
			final int vi = NUM_COMPONENTS * tile;
			weights[vi] = w;
			y[vi] =     (float) vectorXYSDS[tile][0];
			y[vi + 1] = (float) vectorXYSDS[tile][1];
			final int ei = 4 * tile;
			if (useEigen) {
				final float k0inv = 1.0f / eigMaxSqrt;
				final float k0 = Math.max(0.0f,
						1.0f / Math.max(eigMinSqrt, (float) Math.sqrt(l0)) - k0inv);
				final float k1 = Math.max(0.0f,
						1.0f / Math.max(eigMinSqrt, (float) Math.sqrt(l1)) - k0inv);
				eigen[ei]     = k0 * e0x;
				eigen[ei + 1] = k0 * e0y;
				eigen[ei + 2] = k1 * e0y;
				eigen[ei + 3] = k1 * -e0x;
			} else {
				eigen[ei] = 1.0f;
				eigen[ei + 3] = 1.0f;
			}
		}
		// tail 1: sample-weight normalization (serial ascending, GPU order)
		float sum = 0.0f;
		int numMeas = 0;
		for (int tile = 0; tile < numTiles; tile++) {
			final float w = weights[NUM_COMPONENTS * tile];
			sum += w + w;
			if (w > 0.0f) numMeas++;
		}
		final float sNorm = 1.0f / sum;
		for (int tile = 0; tile < numTiles; tile++) {
			final int vi = NUM_COMPONENTS * tile;
			final float w = weights[vi] * sNorm;
			weights[vi] = w;
			weights[vi + 1] = w;
		}
		for (int par = 0; par < NUM_PARAMS; par++) {
			y[ns + par] = pullTargets[par];
		}
		// tail 2: y += fx (measured+valid), H diagonal, reg weights, full normalize
		for (int tile = 0; tile < numTiles; tile++) {
			final int vi = NUM_COMPONENTS * tile;
			if ((weights[vi] > 0.0f) && (raw.valid[tile] != 0)) {
				y[vi] = y[vi] + raw.fx[vi];
				y[vi + 1] = y[vi + 1] + raw.fx[vi + 1];
			}
		}
		final float [] h = new float [NUM_PARAMS];
		for (int tile = 0; tile < numTiles; tile++) {
			final int vi = NUM_COMPONENTS * tile;
			final float w = weights[vi];
			if (!(w > 0.0f)) continue;
			final boolean haveRaw = raw.valid[tile] != 0;
			final int ei = 4 * tile;
			for (int par = 0; par < NUM_PARAMS; par++) {
				final int rawI = par * ns + vi;
				final float jx = haveRaw ? raw.jt[rawI] : 0.0f;
				final float jy = haveRaw ? raw.jt[rawI + 1] : 0.0f;
				final float tj0 = eigen[ei] * jx + eigen[ei + 1] * jy;
				final float tj1 = eigen[ei + 2] * jx + eigen[ei + 3] * jy;
				h[par] = h[par] + w * (tj0 * tj0);
				h[par] = h[par] + w * (tj1 * tj1);
			}
		}
		final float [] regWeights = new float [NUM_PARAMS];
		for (int par = 0; par < NUM_PARAMS; par++) {
			regWeights[par] = (h[par] > 0.0f) ?
					(paramRegweights[par] / (float) Math.sqrt(h[par])) : 0.0f;
			weights[ns + par] = regWeights[par];
		}
		float pure = 0.0f;
		for (int i = 0; i < ns; i++) pure += weights[i];
		float full = pure;
		for (int par = 0; par < NUM_PARAMS; par++) full += weights[ns + par];
		final float scale = 1.0f / full;
		for (int i = 0; i < numValues; i++) weights[i] *= scale;
		return new PrepareResidentOracle(y, weights, eigen,
				numMeas, sum, pure / full, full, regWeights);
	}

	/** One-shot comparison of the captured GPU prepare buffers vs the serial
	 *  float clone: exact float mismatch counts (0 expected). By Claude on 07/17/2026. */
	public static String comparePrepareResident(
			final PrepareResidentResult gpu,
			final PrepareResidentOracle oracle) {
		int badY = 0, badW = 0, badE = 0;
		for (int i = 0; i < oracle.y.length; i++) {
			if (Float.floatToIntBits(gpu.capturedY[i]) != Float.floatToIntBits(oracle.y[i])) badY++;
		}
		for (int i = 0; i < oracle.weights.length; i++) {
			if (Float.floatToIntBits(gpu.capturedWeights[i]) != Float.floatToIntBits(oracle.weights[i])) badW++;
		}
		for (int i = 0; i < oracle.eigen.length; i++) {
			if (Float.floatToIntBits(gpu.capturedEigen[i]) != Float.floatToIntBits(oracle.eigen[i])) badE++;
		}
		return String.format(
				"y mismatches=%d, weight mismatches=%d, eigen mismatches=%d; "+
				"num_meas %d/%d, pure_weight %g/%g, full_weight %g/%g",
				badY, badW, badE, gpu.numMeas, oracle.numMeas,
				gpu.pureWeight, oracle.pureWeight, gpu.fullWeight, oracle.fullWeight);
	}

	public static final class ResidentLmaStepResult {
		public final LmaStepResult step;
		public final PreparedLma prepared;
		public final float currentRms;
		public final float currentPureRms;
		public final float candidateRms;
		public final float candidatePureRms;
		public final boolean accepted;
		public final boolean stop;

		public ResidentLmaStepResult(
				final LmaStepResult step,
				final PreparedLma prepared,
				final float currentRms,
				final float currentPureRms,
				final float candidateRms,
				final float candidatePureRms,
				final boolean accepted,
				final boolean stop) {
			this.step = step;
			this.prepared = prepared;
			this.currentRms = currentRms;
			this.currentPureRms = currentPureRms;
			this.candidateRms = candidateRms;
			this.candidatePureRms = candidatePureRms;
			this.accepted = accepted;
			this.stop = stop;
		}
	}

	/** Statistics for the resident preparation arrays and current-point RMS. */
	public static final class PreparationComparison {
		public boolean bitExact;
		public float maxAbsJt;
		public float maxAbsYmfx;
		public float absRms;
		public float absPureRms;

		public String format() {
			return String.format(
					"bit-exact=%s, max|J|=%g, max|W(y-f)|=%g, |RMS|=%g, |pure RMS|=%g",
					bitExact, maxAbsJt, maxAbsYmfx, absRms, absPureRms);
		}
	}

	/**
	 * Primitive-float clone of the fixed three-angle candidate calculation in
	 * {@code IntersceneLma.lmaStep()}. It deliberately keeps all reductions and
	 * the explicit 3x3 solve serial so the matching CUDA kernel can use one
	 * thread; parallel tile preparation/reduction is a later residency layer.
	 */
	public static LmaStepResult lmaStep(
			final float lambda,
			final float [] weights,
			final float [] jt,
			final float [] ymfxWeighted,
			final float [] vector) {
		if ((weights == null) || (weights.length == 0) ||
				(jt == null) || (jt.length != NUM_PARAMS * weights.length) ||
				(ymfxWeighted == null) || (ymfxWeighted.length != weights.length) ||
				(vector == null) || (vector.length != NUM_PARAMS)) {
			throw new IllegalArgumentException("inconsistent fixed pose LMA arrays");
		}
		final int numValues = weights.length;
		final float [] products = new float [LMA_PRODUCTS];
		for (int row = 0; row < NUM_PARAMS; row++) {
			final int ri = row * numValues;
			for (int column = row; column < NUM_PARAMS; column++) {
				final int ci = column * numValues;
				float sum = 0.0f;
				for (int value = 0; value < numValues; value++) {
					sum += (weights[value] * jt[ri + value]) * jt[ci + value];
				}
				products[row * NUM_PARAMS + column] = sum;
				products[column * NUM_PARAMS + row] = sum;
			}
			float sum = 0.0f;
			for (int value = 0; value < numValues; value++) {
				sum += jt[ri + value] * ymfxWeighted[value];
			}
			products[NUM_PARAMS * NUM_PARAMS + row] = sum;
		}

		final float [] damped = Arrays.copyOf(products, NUM_PARAMS * NUM_PARAMS);
		for (int i = 0; i < NUM_PARAMS; i++) {
			final int diagonal = i * NUM_PARAMS + i;
			damped[diagonal] += damped[diagonal] * lambda;
		}
		final float [] inverse = inverse3x3(damped);
		final float [] delta = new float [] {Float.NaN, Float.NaN, Float.NaN};
		final float [] candidate = vector.clone();
		if (inverse == null) return new LmaStepResult(false, products, delta, candidate);
		final int bi = NUM_PARAMS * NUM_PARAMS;
		for (int row = 0; row < NUM_PARAMS; row++) {
			delta[row] = inverse[row * NUM_PARAMS] * products[bi] +
					inverse[row * NUM_PARAMS + 1] * products[bi + 1] +
					inverse[row * NUM_PARAMS + 2] * products[bi + 2];
			candidate[row] += delta[row];
		}
		return new LmaStepResult(allFinite(delta) && allFinite(candidate), products, delta, candidate);
	}

	/**
	 * Primitive-float oracle for the resident per-tile preparation kernel. Raw
	 * projection/Jacobian values are eigen-transformed, weighted residuals are
	 * formed, and the three pull rows are appended without any general matrix API.
	 */
	public static PreparedLma prepareLma(
			final Result raw,
			final float [] y,
			final float [] weights,
			final float [] eigen,
			final float [] vector,
			final float pureWeight) {
		if ((raw == null) || (raw.numTiles <= 0) || (raw.fx == null) ||
				(raw.fx.length != NUM_COMPONENTS * raw.numTiles) || (raw.jt == null) ||
				(raw.jt.length != NUM_PARAMS * NUM_COMPONENTS * raw.numTiles) ||
				(raw.valid == null) || (raw.valid.length != raw.numTiles) ||
				(y == null) || (weights == null) ||
				(y.length != NUM_COMPONENTS * raw.numTiles + NUM_PARAMS) ||
				(weights.length != y.length) || (eigen == null) ||
				(eigen.length != 4 * raw.numTiles) || (vector == null) ||
				(vector.length != NUM_PARAMS) || !finite(pureWeight) || !(pureWeight > 0.0f)) {
			throw new IllegalArgumentException("inconsistent resident pose LMA preparation arrays");
		}
		final int numValues = y.length;
		final float [] transformedJt = new float [NUM_PARAMS * numValues];
		final float [] ymfx = new float [numValues];
		float pureL2 = 0.0f;
		for (int tile = 0; tile < raw.numTiles; tile++) {
			final int vi = NUM_COMPONENTS * tile;
			final int ei = 4 * tile;
			final boolean useTile = weights[vi] > 0.0f;
			final boolean haveRaw = useTile && (raw.valid[tile] != 0);
			final float rawX = haveRaw ? raw.fx[vi] : 0.0f;
			final float rawY = haveRaw ? raw.fx[vi + 1] : 0.0f;
			float residual0 = 0.0f;
			float residual1 = 0.0f;
			if (useTile) {
				final float dx = y[vi] - rawX;
				final float dy = y[vi + 1] - rawY;
				residual0 = eigen[ei] * dx + eigen[ei + 1] * dy;
				residual1 = eigen[ei + 2] * dx + eigen[ei + 3] * dy;
			}
			ymfx[vi] = residual0 * weights[vi];
			ymfx[vi + 1] = residual1 * weights[vi + 1];
			pureL2 += residual0 * ymfx[vi];
			pureL2 += residual1 * ymfx[vi + 1];
			for (int par = 0; par < NUM_PARAMS; par++) {
				final int rawIndex = par * NUM_COMPONENTS * raw.numTiles + vi;
				final float jx = haveRaw ? raw.jt[rawIndex] : 0.0f;
				final float jy = haveRaw ? raw.jt[rawIndex + 1] : 0.0f;
				final int outIndex = par * numValues + vi;
				transformedJt[outIndex] = eigen[ei] * jx + eigen[ei + 1] * jy;
				transformedJt[outIndex + 1] = eigen[ei + 2] * jx + eigen[ei + 3] * jy;
			}
		}
		float fullL2 = pureL2;
		for (int par = 0; par < NUM_PARAMS; par++) {
			final int value = NUM_COMPONENTS * raw.numTiles + par;
			transformedJt[par * numValues + value] = 1.0f;
			final float residual = y[value] - vector[par];
			ymfx[value] = residual * weights[value];
			fullL2 += residual * ymfx[value];
		}
		return new PreparedLma(
				transformedJt, ymfx,
				(float) Math.sqrt(fullL2),
				(float) Math.sqrt(pureL2 / pureWeight));
	}

	public static LmaStepComparison compareLmaSteps(
			final LmaStepResult actual,
			final LmaStepResult expected) {
		if ((actual == null) || (expected == null) ||
				(actual.products == null) || (expected.products == null) ||
				(actual.products.length != expected.products.length) ||
				(actual.delta == null) || (expected.delta == null) ||
				(actual.delta.length != expected.delta.length) ||
				(actual.candidate == null) || (expected.candidate == null) ||
				(actual.candidate.length != expected.candidate.length)) {
			throw new IllegalArgumentException("incompatible float LMA step results");
		}
		final LmaStepComparison comparison = new LmaStepComparison();
		comparison.validityMatch = actual.valid == expected.valid;
		comparison.bitExact = comparison.validityMatch;
		comparison.maxAbsProducts = compareFloatArrays(actual.products, expected.products, comparison);
		comparison.maxAbsDelta = compareFloatArrays(actual.delta, expected.delta, comparison);
		comparison.maxAbsCandidate = compareFloatArrays(actual.candidate, expected.candidate, comparison);
		return comparison;
	}

	public static PreparationComparison comparePreparation(
			final PreparedLma actual,
			final PreparedLma expected) {
		if ((actual == null) || (expected == null) || (actual.jt == null) ||
				(expected.jt == null) || (actual.jt.length != expected.jt.length) ||
				(actual.ymfxWeighted == null) || (expected.ymfxWeighted == null) ||
				(actual.ymfxWeighted.length != expected.ymfxWeighted.length)) {
			throw new IllegalArgumentException("incompatible resident pose LMA preparation results");
		}
		final PreparationComparison comparison = new PreparationComparison();
		comparison.bitExact = true;
		comparison.maxAbsJt = comparePreparedArrays(actual.jt, expected.jt, comparison);
		comparison.maxAbsYmfx = comparePreparedArrays(
				actual.ymfxWeighted, expected.ymfxWeighted, comparison);
		comparison.absRms = Math.abs(actual.rms - expected.rms);
		comparison.absPureRms = Math.abs(actual.pureRms - expected.pureRms);
		return comparison;
	}

	private static float comparePreparedArrays(
			final float [] actual,
			final float [] expected,
			final PreparationComparison comparison) {
		float maximum = 0.0f;
		for (int i = 0; i < actual.length; i++) {
			final boolean sameBits = Float.floatToRawIntBits(actual[i]) ==
					Float.floatToRawIntBits(expected[i]);
			if (!sameBits) comparison.bitExact = false;
			if (finite(actual[i]) && finite(expected[i])) {
				maximum = Math.max(maximum, Math.abs(actual[i] - expected[i]));
			} else if (!sameBits) {
				maximum = Float.POSITIVE_INFINITY;
			}
		}
		return maximum;
	}

	private static float compareFloatArrays(
			final float [] actual,
			final float [] expected,
			final LmaStepComparison comparison) {
		float maximum = 0.0f;
		for (int i = 0; i < actual.length; i++) {
			final boolean sameBits = Float.floatToRawIntBits(actual[i]) ==
					Float.floatToRawIntBits(expected[i]);
			if (!sameBits) comparison.bitExact = false;
			if (finite(actual[i]) && finite(expected[i])) {
				maximum = Math.max(maximum, Math.abs(actual[i] - expected[i]));
			} else if (!sameBits) {
				maximum = Float.POSITIVE_INFINITY;
			}
		}
		return maximum;
	}

	/** Convert the existing sparse center representation to CUDA-style xyz triples. */
	public static float [] flattenCenters(final double [][] centers) {
		final float [] flat = new float [3 * centers.length];
		Arrays.fill(flat, Float.NaN);
		for (int tile = 0; tile < centers.length; tile++) {
			if ((centers[tile] == null) || (centers[tile].length < 3)) continue;
			for (int i = 0; i < 3; i++) flat[3 * tile + i] = (float) centers[tile][i];
		}
		return flat;
	}

	public static float [] toFloat3(final double [] vector) {
		if ((vector == null) || (vector.length < 3)) throw new IllegalArgumentException("expected 3-vector");
		return new float [] {(float) vector[0], (float) vector[1], (float) vector[2]};
	}

	/**
	 * Calculate scene pixel coordinates and derivatives by scene azimuth, tilt
	 * and roll. Centers are packed xyz triples; a null selection means all finite
	 * centers. Optional eigen transforms are packed per tile as 2x2 row-major.
	 */
	public static Result getFxDerivs(
			final Camera reference,
			final Camera scene,
			final float [] referenceXyz,
			final float [] referenceAtr,
			final float [] sceneXyz,
			final float [] sceneAtr,
			final float [] centers,
			final boolean [] selection,
			final float [] eigenTransforms) {
		return getFxDerivs(reference, scene, referenceXyz, referenceAtr, sceneXyz, sceneAtr,
				centers, selection, eigenTransforms, true);
	}

	/**
	 * Shared projection path with optional Jacobian output, matching the nullable
	 * {@code jt} contract of the legacy method. The pose-global matrices and all
	 * per-tile projection work are common; {@code calculateJacobian=false} stops
	 * after fx and validity have been produced.
	 */
	public static Result getFxDerivs(
			final Camera reference,
			final Camera scene,
			final float [] referenceXyz,
			final float [] referenceAtr,
			final float [] sceneXyz,
			final float [] sceneAtr,
			final float [] centers,
			final boolean [] selection,
			final float [] eigenTransforms,
			final boolean calculateJacobian) {
		if ((centers == null) || ((centers.length % 3) != 0)) {
			throw new IllegalArgumentException("centers should contain packed xyz triples");
		}
		final int numTiles = centers.length / 3;
		if ((selection != null) && (selection.length != numTiles)) {
			throw new IllegalArgumentException("selection length");
		}
		if ((eigenTransforms != null) && (eigenTransforms.length != 4 * numTiles)) {
			throw new IllegalArgumentException("eigenTransforms length");
		}
		checkVector3(referenceXyz, "referenceXyz");
		checkVector3(referenceAtr, "referenceAtr");
		checkVector3(sceneXyz, "sceneXyz");
		checkVector3(sceneAtr, "sceneAtr");

		final float [] refInverse = rotationMatrices(referenceAtr, true);
		final float [] sceneInverse = rotationMatrices(sceneAtr, true);
		final float [] sceneDirect = rotationMatrices(sceneAtr, false);
		final float [] fx = new float [NUM_COMPONENTS * numTiles];
		final float [] jt = calculateJacobian ?
				new float [NUM_PARAMS * NUM_COMPONENTS * numTiles] : null;
		final byte [] valid = new byte [numTiles];
		Arrays.fill(fx, Float.NaN);
		if (jt != null) Arrays.fill(jt, Float.NaN);
		int numValid = 0;
		for (int tile = 0; tile < numTiles; tile++) {
			if ((selection != null) && !selection[tile]) continue;
			final int ci = 3 * tile;
			final float px = centers[ci];
			final float py = centers[ci + 1];
			final float disparity = centers[ci + 2];
			if (!finite(px) || !finite(py) || !finite(disparity)) continue;
			final boolean infinity = disparity < INFINITY_DISPARITY;
			final WorldDerivs referenceWorld = worldFromPixel(
					reference, px, py, disparity, infinity, referenceXyz, refInverse);
			if (referenceWorld == null) continue;
			final float [] scenePixel = pixelFromWorld(
					scene, referenceWorld.world, infinity, sceneXyz, sceneDirect);
			if (scenePixel == null) continue;
			final int vi = NUM_COMPONENTS * tile;
			fx[vi] = scenePixel[0];
			fx[vi + 1] = scenePixel[1];
			if (!calculateJacobian) {
				valid[tile] = 1;
				numValid++;
				continue;
			}
			final WorldDerivs sceneWorld = worldFromPixel(
					scene, scenePixel[0], scenePixel[1], scenePixel[2], infinity, sceneXyz, sceneInverse);
			if (sceneWorld == null) continue;

			final float [] dWorldDPixel = sceneWorld.dPixel.clone();
			final float [] dWorldDAtr = sceneWorld.dAtr.clone();
			if (infinity && !normalizeInfinityDerivatives(sceneWorld.world, dWorldDPixel, dWorldDAtr)) continue;
			final float [] dPixelDWorld = inverse3x3(columnsToRowMajor(dWorldDPixel));
			if (dPixelDWorld == null) continue;

			for (int par = 0; par < NUM_PARAMS; par++) {
				final int di = 3 * par;
				final float rawX = -(dPixelDWorld[0] * dWorldDAtr[di] +
						dPixelDWorld[1] * dWorldDAtr[di + 1] + dPixelDWorld[2] * dWorldDAtr[di + 2]);
				final float rawY = -(dPixelDWorld[3] * dWorldDAtr[di] +
						dPixelDWorld[4] * dWorldDAtr[di + 1] + dPixelDWorld[5] * dWorldDAtr[di + 2]);
				final int ji = par * NUM_COMPONENTS * numTiles + vi;
				if (eigenTransforms == null) {
					jt[ji] = rawX;
					jt[ji + 1] = rawY;
				} else {
					final int ei = 4 * tile;
					jt[ji] = eigenTransforms[ei] * rawX + eigenTransforms[ei + 1] * rawY;
					jt[ji + 1] = eigenTransforms[ei + 2] * rawX + eigenTransforms[ei + 3] * rawY;
				}
			}
			valid[tile] = 1;
			numValid++;
		}
		return new Result(numTiles, fx, jt, valid, numValid);
	}

	/**
	 * 3-B rung B1 (design 2026-07-16): serial float task-build oracle - the Java
	 * float clone of the pose_task_update CUDA kernel. Runs the SAME float
	 * projection chain as {@link #getFxDerivs} (worldFromPixel -> pixelFromWorld)
	 * over the per-sequence task template and packs the 6-float task headers of
	 * both MB sets exactly as the kernel does: centerXY = projected point + the
	 * uniform-MB descriptor offsets, target_disparity = scene disparity +
	 * disparity_corr, pre-encoded task/scale words, task=0 HOLES (all header
	 * floats zero except txy) for failed projections and margin-gate rejects.
	 * Differences vs the CUDA stream are float-scheduling only (FMA contraction);
	 * the one-shot leanMeasure compare prints the max field deltas.
	 * @return float[2][num_tasks*task_size] header-filled task streams
	 *         (set 0 = MB main, set 1 = MB partner); xy/disp_dist tails zero.
	 * By Claude on 07/16/2026.
	 */
	public static float [][] buildTasks(
			final Camera reference,
			final Camera scene,
			final float [] referenceXyz,
			final float [] referenceAtr,
			final float [] sceneXyz,
			final float [] sceneAtr,
			final int []   taskMap,      // packed txy: tx | (ty<<16)
			final float [] taskCenters,  // reference px,py,disparity per task
			final int taskSize,
			final float disparityCorr,
			final float minPx,
			final float maxPx,
			final float minPy,
			final float maxPy,
			final int taskCodeMain,
			final int taskCodeSub,
			final float dxMain,
			final float dyMain,
			final float dxSub,
			final float dySub,
			final float scaleMain,
			final float scaleSub) {
		if ((taskMap == null) || (taskCenters == null) ||
				(taskCenters.length != 3 * taskMap.length) || (taskSize < 6)) {
			throw new IllegalArgumentException("buildTasks: inconsistent template");
		}
		checkVector3(referenceXyz, "referenceXyz");
		checkVector3(referenceAtr, "referenceAtr");
		checkVector3(sceneXyz, "sceneXyz");
		checkVector3(sceneAtr, "sceneAtr");
		final int numTasks = taskMap.length;
		final float [] refInverse = rotationMatrices(referenceAtr, true);
		final float [] sceneDirect = rotationMatrices(sceneAtr, false);
		final float [][] streams = new float [2][numTasks * taskSize];
		for (int task = 0; task < numTasks; task++) {
			final int base = task * taskSize;
			streams[0][base + 1] = Float.intBitsToFloat(taskMap[task]); // txy in both sets
			streams[1][base + 1] = streams[0][base + 1];
			final int ci = 3 * task;
			final float px = taskCenters[ci];
			final float py = taskCenters[ci + 1];
			final float disparity = taskCenters[ci + 2];
			if (!finite(px) || !finite(py) || !finite(disparity)) continue; // hole
			final boolean infinity = disparity < INFINITY_DISPARITY;
			final WorldDerivs referenceWorld = worldFromPixel(
					reference, px, py, disparity, infinity, referenceXyz, refInverse);
			if (referenceWorld == null) continue; // hole
			final float [] scenePixel = pixelFromWorld(
					scene, referenceWorld.world, infinity, sceneXyz, sceneDirect);
			if (scenePixel == null) continue; // hole
			final float cxMain = scenePixel[0] + dxMain;
			final float cyMain = scenePixel[1] + dyMain;
			final float cxSub =  scenePixel[0] + dxSub;
			final float cySub =  scenePixel[1] + dySub;
			if ((cxMain < minPx) || (cxMain > maxPx) || (cyMain < minPy) || (cyMain > maxPy) ||
					(cxSub < minPx) || (cxSub > maxPx) || (cySub < minPy) || (cySub > maxPy)) {
				continue; // margin-gate hole
			}
			final float targetDisparity = scenePixel[2] + disparityCorr;
			if (!finite(targetDisparity)) continue; // hole
			streams[0][base] =     Float.intBitsToFloat(taskCodeMain);
			streams[1][base] =     Float.intBitsToFloat(taskCodeSub);
			streams[0][base + 2] = targetDisparity;
			streams[1][base + 2] = targetDisparity;
			streams[0][base + 3] = cxMain;
			streams[0][base + 4] = cyMain;
			streams[1][base + 3] = cxSub;
			streams[1][base + 4] = cySub;
			streams[0][base + 5] = scaleMain;
			streams[1][base + 5] = scaleSub;
		}
		return streams;
	}

	/** Compare a float result with the exact legacy double geometry call. */
	public static Comparison compareToDouble(
			final Result result,
			final ErsCorrection reference,
			final ErsCorrection scene,
			final double [] referenceXyz,
			final double [] referenceAtr,
			final double [] sceneXyz,
			final double [] sceneAtr,
			final double [][] centers,
			final boolean [] selection,
			final float [] eigenTransforms,
			final int debugLevel) {
		if (result.numTiles != centers.length) throw new IllegalArgumentException("result/centers length");
		reference.setupERS();
		scene.setupERS();
		final Matrix [] refInverse = ErsCorrection.getInterRotDeriveMatrices(referenceAtr, true);
		final Matrix [] sceneInverse = ErsCorrection.getInterRotDeriveMatrices(sceneAtr, true);
		final Matrix sceneDirect = ErsCorrection.getInterRotDeriveMatrices(sceneAtr, false)[0];
		final Comparison comparison = new Comparison();
		for (int tile = 0; tile < centers.length; tile++) {
			if ((selection != null) && !selection[tile]) continue;
			double [][] expected = null;
			if ((centers[tile] != null) && (centers[tile].length >= 3)) {
				expected = reference.getDPxSceneDParameters(
						scene, true, centers[tile][2] < INFINITY_DISPARITY, centers[tile],
						referenceXyz, sceneXyz, refInverse, sceneInverse, sceneDirect, debugLevel);
			}
			final boolean doubleValid = expected != null;
			final boolean floatValid = result.valid[tile] != 0;
			if (floatValid && !doubleValid) {
				comparison.floatOnlyValid++;
				continue;
			}
			if (!floatValid && doubleValid) {
				comparison.doubleOnlyValid++;
				continue;
			}
			if (!floatValid) continue;
			comparison.compared++;
			final int vi = NUM_COMPONENTS * tile;
			comparison.maxAbsFx = Math.max(comparison.maxAbsFx,
					Math.max(Math.abs(result.fx[vi] - (float) expected[0][0]),
							Math.abs(result.fx[vi + 1] - (float) expected[0][1])));
			if (result.jt == null) continue;
			for (int par = 0; par < NUM_PARAMS; par++) {
				float ex = (float) expected[ErsCorrection.DP_DSAZ + par + 1][0];
				float ey = (float) expected[ErsCorrection.DP_DSAZ + par + 1][1];
				if (eigenTransforms != null) {
					final int ei = 4 * tile;
					final float tx = eigenTransforms[ei] * ex + eigenTransforms[ei + 1] * ey;
					final float ty = eigenTransforms[ei + 2] * ex + eigenTransforms[ei + 3] * ey;
					ex = tx;
					ey = ty;
				}
				final int ji = par * NUM_COMPONENTS * result.numTiles + vi;
				comparison.maxAbsJt = Math.max(comparison.maxAbsJt,
						Math.max(Math.abs(result.jt[ji] - ex), Math.abs(result.jt[ji + 1] - ey)));
			}
		}
		return comparison;
	}

	/** Compare raw fx/J and validity from two float implementations. */
	public static ResultComparison compareResults(final Result actual, final Result expected) {
		if ((actual == null) || (expected == null) || (actual.numTiles != expected.numTiles) ||
				(actual.fx.length != expected.fx.length) || (actual.valid.length != expected.valid.length) ||
				((actual.jt == null) != (expected.jt == null)) ||
				((actual.jt != null) && (actual.jt.length != expected.jt.length))) {
			throw new IllegalArgumentException("incompatible float getFxDerivs results");
		}
		final ResultComparison comparison = new ResultComparison();
		for (int tile = 0; tile < actual.numTiles; tile++) {
			final boolean actualValid = actual.valid[tile] != 0;
			final boolean expectedValid = expected.valid[tile] != 0;
			if (actualValid && !expectedValid) {
				comparison.actualOnlyValid++;
				continue;
			}
			if (!actualValid && expectedValid) {
				comparison.expectedOnlyValid++;
				continue;
			}
			if (!actualValid) continue;
			comparison.compared++;
			final int vi = NUM_COMPONENTS * tile;
			for (int component = 0; component < NUM_COMPONENTS; component++) {
				comparison.maxAbsFx = Math.max(comparison.maxAbsFx,
						Math.abs(actual.fx[vi + component] - expected.fx[vi + component]));
			}
			if (actual.jt == null) continue;
			for (int par = 0; par < NUM_PARAMS; par++) {
				final int ji = par * NUM_COMPONENTS * actual.numTiles + vi;
				for (int component = 0; component < NUM_COMPONENTS; component++) {
					comparison.maxAbsJt = Math.max(comparison.maxAbsJt,
							Math.abs(actual.jt[ji + component] - expected.jt[ji + component]));
				}
			}
		}
		return comparison;
	}

	private static final class WorldDerivs {
		final float [] world = new float [3];
		final float [] dPixel = new float [9]; // columns: d/d px, py, disparity
		final float [] dAtr = new float [9];   // columns: d/d azimuth, tilt, roll
	}

	private static WorldDerivs worldFromPixel(
			final Camera camera,
			final float px,
			final float py,
			final float disparity,
			final boolean infinity,
			final float [] cameraXyz,
			final float [] inverseMatrices) {
		if (!finite(disparity)) return null;
		int line = Math.round(py);
		if (line < 0) line = 0;
		else if (line >= camera.height) line = camera.height - 1;
		final float pXcd = px - 0.5f * camera.width;
		final float pYcd = py - 0.5f * camera.height;
		final float rScale = 0.001f * camera.pixelSize / camera.distortionRadius;
		final float rDn = (float) Math.sqrt(pXcd * pXcd + pYcd * pYcd) * rScale;
		final float rND2RD = inverseDistortion(camera, rDn);
		if (!finite(rND2RD)) return null;
		final float pXc = pXcd * rND2RD;
		final float pYc = pYcd * rND2RD;
		final float rn = rDn * rND2RD;

		float derivative = 0.0f;
		float rr = 1.0f;
		for (int j = 0; j < camera.radial.length; j++) {
			derivative += camera.radial[j] * (j + 1) * rr;
			rr *= rn;
		}
		final float rD2RND = 1.0f / rND2RD;
		final float denominator = (rD2RND * rD2RND) * (derivative * rn + rD2RND);
		final float dInvDr = -derivative / denominator;
		final float dRdx;
		final float dRdy;
		if (rDn == 0.0f) {
			dRdx = 0.0f;
			dRdy = 0.0f;
		} else {
			dRdx = pXcd * rScale * rScale / rDn;
			dRdy = pYcd * rScale * rScale / rDn;
		}
		final float dPxcDx = rND2RD + pXcd * dInvDr * dRdx;
		final float dPxcDy = pXcd * dInvDr * dRdy;
		final float dPycDx = pYcd * dInvDr * dRdx;
		final float dPycDy = rND2RD + pYcd * dInvDr * dRdy;

		final float zh = -SCENE_UNITS_SCALE * camera.focalLength * camera.disparityRadius /
				(0.001f * camera.pixelSize);
		final float xh = SCENE_UNITS_SCALE * pXc * camera.disparityRadius;
		final float yh = -SCENE_UNITS_SCALE * pYc * camera.disparityRadius;
		final float dXhDx = SCENE_UNITS_SCALE * dPxcDx * camera.disparityRadius;
		final float dXhDy = SCENE_UNITS_SCALE * dPxcDy * camera.disparityRadius;
		final float dYhDx = -SCENE_UNITS_SCALE * dPycDx * camera.disparityRadius;
		final float dYhDy = -SCENE_UNITS_SCALE * dPycDy * camera.disparityRadius;
		final float [] xyz = new float [] {xh, yh, zh};
		final float [] dpx = new float [] {dXhDx, dYhDx, 0.0f};
		final float [] dpy = new float [] {dXhDy, dYhDy, 0.0f};
		final float [] dd = new float [3];
		if (!infinity) {
			if (disparity == 0.0f) return null;
			for (int i = 0; i < 3; i++) xyz[i] /= disparity;
			for (int i = 0; i < 2; i++) {
				dpx[i] /= disparity;
				dpy[i] /= disparity;
			}
			for (int i = 0; i < 3; i++) dd[i] = -xyz[i] / disparity;
		}

		final int eb = line * ERS_LINE_STRIDE;
		rotate(camera.ers, eb + ERS_QUAT, true, dpx, dpx);
		rotate(camera.ers, eb + ERS_QUAT, true, dpy, dpy);
		rotate(camera.ers, eb + ERS_QUAT, true, dd, dd);
		final float [] cameraNow = new float [3];
		rotate(camera.ers, eb + ERS_QUAT, true, xyz, cameraNow);
		final float ox = camera.ers[eb + ERS_QUAT_DT + 1];
		final float oy = camera.ers[eb + ERS_QUAT_DT + 2];
		final float oz = camera.ers[eb + ERS_QUAT_DT + 3];
		final float [] dwDt = new float [] {
				oy * cameraNow[2] - oz * cameraNow[1],
				oz * cameraNow[0] - ox * cameraNow[2],
				ox * cameraNow[1] - oy * cameraNow[0]};
		for (int i = 0; i < 3; i++) {
			dpy[i] += camera.lineTime * dwDt[i];
			if (!infinity) dpy[i] += camera.lineTime * camera.ers[eb + ERS_XYZ_DT + i];
			if (!infinity) cameraNow[i] += camera.ers[eb + ERS_XYZ + i];
		}

		final WorldDerivs result = new WorldDerivs();
		matVec(inverseMatrices, 0, cameraNow, result.world);
		matVec(inverseMatrices, 0, dpx, result.dPixel, 0);
		matVec(inverseMatrices, 0, dpy, result.dPixel, 3);
		matVec(inverseMatrices, 0, dd, result.dPixel, 6);
		if (!infinity) {
			for (int i = 0; i < 3; i++) result.world[i] += cameraXyz[i];
		}
		for (int par = 0; par < NUM_PARAMS; par++) {
			matVec(inverseMatrices, 9 * (par + 1), cameraNow, result.dAtr, 3 * par);
		}
		return allFinite(result.world) && allFinite(result.dPixel) && allFinite(result.dAtr) ? result : null;
	}

	private static float [] pixelFromWorld(
			final Camera camera,
			final float [] world,
			final boolean infinity,
			final float [] cameraXyz,
			final float [] directMatrices) {
		final float [] centered = world.clone();
		if (!infinity) {
			for (int i = 0; i < 3; i++) centered[i] -= cameraXyz[i];
		}
		final float [] centerLocal = new float [3];
		matVec(directMatrices, 0, centered, centerLocal);
		float line = 0.5f * camera.height;
		float error = line;
		float [] result = null;
		for (int step = 0; (step < PROJECTION_MAX_STEPS) && (error > LINE_ERR); step++) {
			int line0 = (int) Math.floor(line);
			if (line0 < 0) line0 = 0;
			else if (line0 >= camera.height) line0 = camera.height - 1;
			final int line1 = Math.min(line0 + 1, camera.height - 1);
			final float fraction = line - line0;
			final int e0 = line0 * ERS_LINE_STRIDE;
			final int e1 = line1 * ERS_LINE_STRIDE;
			final float [] now = centerLocal.clone();
			if (!infinity) {
				for (int i = 0; i < 3; i++) {
					final float motion = (1.0f - fraction) * camera.ers[e0 + ERS_XYZ + i] +
							fraction * camera.ers[e1 + ERS_XYZ + i];
					now[i] -= motion;
				}
			}
			final float [] quaternion = new float [4];
			for (int i = 0; i < 4; i++) {
				quaternion[i] = (1.0f - fraction) * camera.ers[e0 + ERS_QUAT + i] +
						fraction * camera.ers[e1 + ERS_QUAT + i];
			}
			final float [] local = new float [3];
			rotate(quaternion, 0, false, now, local);
			if (Math.abs(local[2]) < PROJECTION_EPS) return null;
			final float focalPixels = 1000.0f * camera.focalLength / camera.pixelSize;
			final float pXc = -focalPixels * local[0] / local[2];
			final float pYc = focalPixels * local[1] / local[2];
			final float disparity = infinity ? 0.0f :
					-focalPixels / local[2] * SCENE_UNITS_SCALE * camera.disparityRadius;
			final float rND = (float) Math.sqrt(pXc * pXc + pYc * pYc) * 0.001f * camera.pixelSize;
			final float distortion = forwardDistortion(camera, rND / camera.distortionRadius);
			final float px = pXc * distortion + 0.5f * camera.width;
			final float py = pYc * distortion + 0.5f * camera.height;
			if (!finite(px) || !finite(py) || !finite(disparity)) return null;
			result = new float [] {px, py, disparity};
			final float nextLine = Math.max(0.0f, Math.min(camera.height - 1.0f, py));
			error = Math.abs(nextLine - line);
			line = nextLine;
		}
		return result;
	}

	private static boolean normalizeInfinityDerivatives(
			final float [] world,
			final float [] dPixel,
			final float [] dAtr) {
		final float x = world[0];
		final float y = world[1];
		final float z = world[2];
		final float z2 = z * z;
		if (!finite(z2) || (z2 == 0.0f)) return false;
		for (int column = 0; column < 2; column++) {
			final int i = 3 * column;
			final float dx = dPixel[i];
			final float dy = dPixel[i + 1];
			final float dz = dPixel[i + 2];
			dPixel[i] = dx / z - x * dz / z2;
			dPixel[i + 1] = dy / z - y * dz / z2;
			dPixel[i + 2] = 0.0f;
		}
		dPixel[6] = 0.0f;
		dPixel[7] = 0.0f;
		dPixel[8] = 1.0f;
		for (int column = 0; column < NUM_PARAMS; column++) {
			final int i = 3 * column;
			final float dx = dAtr[i];
			final float dy = dAtr[i + 1];
			final float dz = dAtr[i + 2];
			dAtr[i] = dx / z - x * dz / z2;
			dAtr[i + 1] = dy / z - y * dz / z2;
			dAtr[i + 2] = 0.0f;
		}
		return allFinite(dPixel) && allFinite(dAtr);
	}

	private static float [] rotationMatrices(final float [] atr, final boolean invert) {
		final float ca = (float) Math.cos(atr[0]);
		final float sa = (float) Math.sin(atr[0]);
		final float ct = (float) Math.cos(atr[1]);
		final float st = (float) Math.sin(atr[1]);
		final float cr = (float) Math.cos(atr[2]);
		final float sr = (float) Math.sin(atr[2]);
		final float azSign = invert ? 1.0f : -1.0f;
		final float tiltSign = invert ? -1.0f : 1.0f;
		final float rollSign = invert ? -1.0f : 1.0f;
		final float [] az = new float [] {ca,0,sa*azSign, 0,1,0, -sa*azSign,0,ca};
		final float [] tilt = new float [] {1,0,0, 0,ct,st*tiltSign, 0,-st*tiltSign,ct};
		final float [] roll = new float [] {cr,sr*rollSign,0, -sr*rollSign,cr,0, 0,0,1};
		final float [] daz = new float [] {-sa,0,ca*azSign, 0,0,0, -ca*azSign,0,-sa};
		final float [] dtilt = new float [] {0,0,0, 0,-st,ct*tiltSign, 0,-ct*tiltSign,-st};
		final float [] droll = new float [] {-sr,cr*rollSign,0, -cr*rollSign,-sr,0, 0,0,0};
		final float [] result = new float [36];
		if (invert) {
			putMatrix(result, 0, multiply(az, multiply(tilt, roll)));
			putMatrix(result, 9, multiply(daz, multiply(tilt, roll)));
			putMatrix(result, 18, multiply(az, multiply(dtilt, roll)));
			putMatrix(result, 27, multiply(az, multiply(tilt, droll)));
		} else {
			putMatrix(result, 0, multiply(roll, multiply(tilt, az)));
			putMatrix(result, 9, multiply(roll, multiply(tilt, daz)));
			putMatrix(result, 18, multiply(roll, multiply(dtilt, az)));
			putMatrix(result, 27, multiply(droll, multiply(tilt, az)));
		}
		return result;
	}

	private static float [] inverse3x3(final float [] a) {
		final float c00 = a[4] * a[8] - a[5] * a[7];
		final float c01 = a[2] * a[7] - a[1] * a[8];
		final float c02 = a[1] * a[5] - a[2] * a[4];
		final float c10 = a[5] * a[6] - a[3] * a[8];
		final float c11 = a[0] * a[8] - a[2] * a[6];
		final float c12 = a[2] * a[3] - a[0] * a[5];
		final float c20 = a[3] * a[7] - a[4] * a[6];
		final float c21 = a[1] * a[6] - a[0] * a[7];
		final float c22 = a[0] * a[4] - a[1] * a[3];
		final float determinant = a[0] * c00 + a[1] * c10 + a[2] * c20;
		if (!finite(determinant) || (determinant == 0.0f)) return null;
		final float scale = 1.0f / determinant;
		return new float [] {c00*scale,c01*scale,c02*scale, c10*scale,c11*scale,c12*scale,
				c20*scale,c21*scale,c22*scale};
	}

	/** Convert three packed column vectors to the row-major matrix they form. */
	private static float [] columnsToRowMajor(final float [] columns) {
		return new float [] {
				columns[0], columns[3], columns[6],
				columns[1], columns[4], columns[7],
				columns[2], columns[5], columns[8]};
	}

	private static float inverseDistortion(final Camera camera, final float radius) {
		if (!finite(radius)) return Float.NaN;
		final float indexFloat = Math.max(0.0f, radius) / camera.stepR;
		final int index = (int) Math.floor(indexFloat);
		if (index >= camera.rByRDist.length - 2) return camera.rByRDist.length - 1;
		final float mu = indexFloat - index;
		final float mu2 = mu * mu;
		final float y0 = (index > 0) ? camera.rByRDist[index - 1] :
				2.0f * camera.rByRDist[index] - camera.rByRDist[index + 1];
		final float a0 = -0.5f*y0 + 1.5f*camera.rByRDist[index] -
				1.5f*camera.rByRDist[index + 1] + 0.5f*camera.rByRDist[index + 2];
		final float a1 = y0 - 2.5f*camera.rByRDist[index] +
				2.0f*camera.rByRDist[index + 1] - 0.5f*camera.rByRDist[index + 2];
		final float a2 = -0.5f*y0 + 0.5f*camera.rByRDist[index + 1];
		return a0*mu*mu2 + a1*mu2 + a2*mu + camera.rByRDist[index];
	}

	private static float forwardDistortion(final Camera camera, final float radius) {
		float constant = 1.0f;
		for (float coefficient : camera.radial) constant -= coefficient;
		float value = camera.radial[6];
		for (int i = 5; i >= 0; i--) value = value * radius + camera.radial[i];
		return value * radius + constant;
	}

	private static void rotate(
			final float [] quaternion,
			final int offset,
			final boolean inverse,
			final float [] vector,
			final float [] out) {
		float w = quaternion[offset];
		float x = quaternion[offset + 1];
		float y = quaternion[offset + 2];
		float z = quaternion[offset + 3];
		final float norm = (float) Math.sqrt(w*w + x*x + y*y + z*z);
		if (norm == 0.0f) {
			Arrays.fill(out, Float.NaN);
			return;
		}
		w /= norm; x /= norm; y /= norm; z /= norm;
		// Commons Math Rotation stores/applies the vector part with the opposite
		// convention from the textbook q*v*q^-1 formula used below.
		if (!inverse) { x = -x; y = -y; z = -z; }
		final float vx = vector[0];
		final float vy = vector[1];
		final float vz = vector[2];
		final float tx = 2.0f * (y * vz - z * vy);
		final float ty = 2.0f * (z * vx - x * vz);
		final float tz = 2.0f * (x * vy - y * vx);
		out[0] = vx + w * tx + (y * tz - z * ty);
		out[1] = vy + w * ty + (z * tx - x * tz);
		out[2] = vz + w * tz + (x * ty - y * tx);
	}

	private static float [] multiply(final float [] left, final float [] right) {
		final float [] result = new float [9];
		for (int row = 0; row < 3; row++) {
			for (int column = 0; column < 3; column++) {
				result[3 * row + column] = left[3 * row] * right[column] +
						left[3 * row + 1] * right[3 + column] + left[3 * row + 2] * right[6 + column];
			}
		}
		return result;
	}

	private static void putMatrix(final float [] destination, final int offset, final float [] matrix) {
		System.arraycopy(matrix, 0, destination, offset, 9);
	}

	private static void matVec(final float [] matrix, final int offset, final float [] vector, final float [] out) {
		matVec(matrix, offset, vector, out, 0);
	}

	private static void matVec(
			final float [] matrix,
			final int offset,
			final float [] vector,
			final float [] out,
			final int outOffset) {
		final float x = vector[0];
		final float y = vector[1];
		final float z = vector[2];
		out[outOffset] = matrix[offset] * x + matrix[offset + 1] * y + matrix[offset + 2] * z;
		out[outOffset + 1] = matrix[offset + 3] * x + matrix[offset + 4] * y + matrix[offset + 5] * z;
		out[outOffset + 2] = matrix[offset + 6] * x + matrix[offset + 7] * y + matrix[offset + 8] * z;
	}

	private static void checkVector3(final float [] vector, final String name) {
		if ((vector == null) || (vector.length < 3)) throw new IllegalArgumentException(name);
	}

	private static boolean allFinite(final float [] values) {
		for (float value : values) if (!finite(value)) return false;
		return true;
	}

	private static boolean finite(final float value) {
		return !Float.isNaN(value) && !Float.isInfinite(value);
	}
}
