package com.elphel.imagej.tileprocessor;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
import java.util.HashSet;
import java.util.concurrent.atomic.AtomicInteger;

import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.gpu.TpTask;

import Jama.Matrix;

/**
 * Classic-LMA structured global refinement path.
 *
 * <p>This class keeps explicit LMA structure
 * ({@code prepareLMA/getFxDerivs/getYminusFxWeighted/getWJtJlambda/lmaStep/runLma})
 * and assembles one combined residual/Jacobian consisting of correlation, LPF and pull terms.
 */
public class IntersceneGlobalLmaRefine {

	private static final int[] SCENE_POSE_PARAM_ORDER = ErsCorrection.DP_XYZATR.clone();
	private static final int[] SCENE_RATE_ANG_PAR_INDICES = {
			ErsCorrection.DP_DSVAZ,
			ErsCorrection.DP_DSVTL,
			ErsCorrection.DP_DSVRL};
	private static final int[] SCENE_RATE_LIN_PAR_INDICES = {
			ErsCorrection.DP_DSVX,
			ErsCorrection.DP_DSVY,
			ErsCorrection.DP_DSVZ};
	private static final int[] REF_RATE_ANG_PAR_INDICES = {
			ErsCorrection.DP_DVAZ,
			ErsCorrection.DP_DVTL,
			ErsCorrection.DP_DVRL};
	private static final int[] REF_RATE_LIN_PAR_INDICES = {
			ErsCorrection.DP_DVX,
			ErsCorrection.DP_DVY,
			ErsCorrection.DP_DVZ};

	private static final double MIN_DIAG = 1.0e-12;
	private static final double DEFAULT_PULL_WEIGHT = 1.0e-6;
	private static final double DEFAULT_DT = 1.0 / 60.0;

	private static final class PairFactor {
		int sceneIndex;
		int refSceneIndex;
		double pairWeight;

		PairFactor(
				final int sceneIndex,
				final int refSceneIndex,
				final double pairWeight) {
			this.sceneIndex = sceneIndex;
			this.refSceneIndex = refSceneIndex;
			this.pairWeight = pairWeight;
		}
	}

	private static final class PairObservation {
		int sceneIndex;
		int refSceneIndex;
		double pairWeight = 1.0;
		double[][] centers = null;
		double[][] eigen = null;
		double[][] vectorXys = null;
		boolean[] reliableRef = null;
		int yBase = 0;
		int wBase = 0;
	}

	private static final class ObservationCache {
		ArrayList<PairObservation> observations = new ArrayList<PairObservation>();
		double[] yVector = null;
		double[] wVector = null;
		int tilesPerPair = 0;
		int solvedPairs = 0;
		int failedPairs = 0;
	}

	private static final class RefGpuData {
		int refSceneIndex = -1;
		double[][] pXpYD = null;
		TpTask[] tasks = null;
		boolean[] reliableMask = null;
	}

	private static final class RateTerm {
		int sceneIndex;
		double coeff;

		RateTerm(
				final int sceneIndex,
				final double coeff) {
			this.sceneIndex = sceneIndex;
			this.coeff = coeff;
		}
	}

	private static final class VectorTerm {
		int vectorIndex;
		double coeff;

		VectorTerm(
				final int vectorIndex,
				final double coeff) {
			this.vectorIndex = vectorIndex;
			this.coeff = coeff;
		}
	}

	private static final class CorrelationPairContext {
		PairObservation obs;
		int[] localParamIndices = new int[0];
		ArrayList<VectorTerm>[] vectorTerms = null;
		int[] validTiles = new int[0];
		double[][][] eigTrans = null;
		double[] yRows = new double[0];
		double[] rawWeights = new double[0];
		int rowBase = 0;
	}

	private static final class ClassicState {
		int[] activePoseParams = new int[0];
		int[] vectorSceneIndices = new int[0];
		int[] vectorParamIndices = new int[0];
		HashMap<Long, Integer> vectorIndexBySceneParam = new HashMap<Long, Integer>();
		double[] parameterVector = new double[0];
		double[] parameterInitial = new double[0];
		double[] parameterPull = new double[0];

		int correlationRows = 0;
		int pullBase = 0;
		int pullRows = 0;
		int lpfBase = 0;
		int[] lpfVar = new int[0];
		int[] lpfPrevVar = new int[0];
		int[] lpfNextVar = new int[0];
		double[] lpfPrevFixed = new double[0];
		double[] lpfNextFixed = new double[0];

		ArrayList<CorrelationPairContext> correlationPairs = new ArrayList<CorrelationPairContext>();
		double[][][] rateSourceScenes = null;
		boolean includeRateChain = true;

		double[] yVector = new double[0];
		double[] weights = new double[0];
		double[][] lastJt = null;
		double[] lastYminusFx = null;
		double[] lastRms = null;
		double[] initialRms = null;
		double lastMaxDelta = Double.NaN;
		int acceptedSteps = 0;
		int attemptedSteps = 0;

		void clearLinearization() {
			lastJt = null;
			lastYminusFx = null;
			lastRms = null;
			initialRms = null;
			lastMaxDelta = Double.NaN;
			acceptedSteps = 0;
			attemptedSteps = 0;
		}
	}

	private final CLTParameters cltParameters;
	private final QuadCLT[] quadCLTs;
	private final QuadCLT centerCLT;
	private final int centerIndex;
	private final int earliestScene;
	private final int lastScene;
	private final double[][][] scenesXyzatr;
	private final double[][][] scenesXyzatrPull;
	private final boolean[] paramSelect;
	private final double[] paramRegweights;
	private final double[] paramLpf;
	private final double[] centerDisparity;
	private final boolean[] reliableRef;
	private final boolean disableErs;
	private final double mbMaxGain;
	private final IntersceneGlobalRefine.Options options;
	private final ClassicState classicState = new ClassicState();

	private double[] refDisparity = null;
	private double[][] pXpYDCenter = null;
	private double[] sceneTimes = null;

	private IntersceneGlobalLmaRefine(
			final CLTParameters cltParameters,
			final QuadCLT[] quadCLTs,
			final QuadCLT centerCLT,
			final int centerIndex,
			final int earliestScene,
			final int lastScene,
			final double[][][] scenesXyzatr,
			final double[][][] scenesXyzatrPull,
			final boolean[] paramSelect,
			final double[] paramRegweights,
			final double[] paramLpf,
			final double[] centerDisparity,
			final boolean[] reliableRef,
			final boolean disableErs,
			final double mbMaxGain,
			final IntersceneGlobalRefine.Options options) {
		this.cltParameters = cltParameters;
		this.quadCLTs = quadCLTs;
		this.centerCLT = centerCLT;
		this.centerIndex = centerIndex;
		this.earliestScene = earliestScene;
		this.lastScene = lastScene;
		this.scenesXyzatr = scenesXyzatr;
		this.scenesXyzatrPull = scenesXyzatrPull;
		this.paramSelect = paramSelect;
		this.paramRegweights = paramRegweights;
		this.paramLpf = paramLpf;
		this.centerDisparity = centerDisparity;
		this.reliableRef = reliableRef;
		this.disableErs = disableErs;
		this.mbMaxGain = mbMaxGain;
		this.options = options;
	}

	public static IntersceneGlobalRefine.Result refineAllToReference(
			final CLTParameters cltParameters,
			final QuadCLT[] quadCLTs,
			final QuadCLT centerCLT,
			final int centerIndex,
			final int earliestScene,
			final int lastScene,
			final double[][][] scenesXyzatr,
			final double[][][] scenesXyzatrPull,
			final boolean[] paramSelect,
			final double[] paramRegweights,
			final double[] paramLpf,
			final double[] centerDisparity,
			final boolean[] reliableRef,
			final boolean disableErs,
			final double mbMaxGain,
			final IntersceneGlobalRefine.Options options,
			final int debugLevel) {
		final IntersceneGlobalLmaRefine solver = new IntersceneGlobalLmaRefine(
				cltParameters,
				quadCLTs,
				centerCLT,
				centerIndex,
				earliestScene,
				lastScene,
				scenesXyzatr,
				scenesXyzatrPull,
				paramSelect,
				paramRegweights,
				paramLpf,
				centerDisparity,
				reliableRef,
				disableErs,
				mbMaxGain,
				options);
		solver.prepareLMA(debugLevel);
		return solver.runLma(debugLevel);
	}

	private static boolean isPoseValid(final double[][] scenePose) {
		return (scenePose != null) &&
				(scenePose.length >= 2) &&
				(scenePose[0] != null) && (scenePose[0].length >= 3) &&
				(scenePose[1] != null) && (scenePose[1].length >= 3);
	}

	private static double getScenePoseParameter(
			final double[][] scenePose,
			final int dpIndex) {
		if (!isPoseValid(scenePose)) {
			return Double.NaN;
		}
		switch (dpIndex) {
		case ErsCorrection.DP_DSX:
			return scenePose[0][0];
		case ErsCorrection.DP_DSY:
			return scenePose[0][1];
		case ErsCorrection.DP_DSZ:
			return scenePose[0][2];
		case ErsCorrection.DP_DSAZ:
			return scenePose[1][0];
		case ErsCorrection.DP_DSTL:
			return scenePose[1][1];
		case ErsCorrection.DP_DSRL:
			return scenePose[1][2];
		default:
			return Double.NaN;
		}
	}

	private static void setScenePoseParameter(
			final double[][] scenePose,
			final int dpIndex,
			final double value) {
		if (!isPoseValid(scenePose)) {
			return;
		}
		switch (dpIndex) {
		case ErsCorrection.DP_DSX:
			scenePose[0][0] = value;
			break;
		case ErsCorrection.DP_DSY:
			scenePose[0][1] = value;
			break;
		case ErsCorrection.DP_DSZ:
			scenePose[0][2] = value;
			break;
		case ErsCorrection.DP_DSAZ:
			scenePose[1][0] = value;
			break;
		case ErsCorrection.DP_DSTL:
			scenePose[1][1] = value;
			break;
		case ErsCorrection.DP_DSRL:
			scenePose[1][2] = value;
			break;
		default:
			break;
		}
	}

	private static double normalizeValue(final double value) {
		return Double.isFinite(value) ? value : 0.0;
	}

	private static double getArrayValue(
			final double[] values,
			final int index) {
		if ((values == null) || (index < 0) || (index >= values.length)) {
			return Double.NaN;
		}
		return values[index];
	}

	private static long getSceneParamKey(
			final int sceneIndex,
			final int dpIndex) {
		return (((long) sceneIndex) << 32) ^ (dpIndex & 0xffffffffL);
	}

	private static int[] getParameterIndices(final boolean[] mask) {
		if (mask == null) {
			return new int[0];
		}
		int n = 0;
		for (int i = 0; i < mask.length; i++) {
			if (mask[i]) {
				n++;
			}
		}
		final int[] out = new int[n];
		int indx = 0;
		for (int i = 0; i < mask.length; i++) {
			if (mask[i]) {
				out[indx++] = i;
			}
		}
		return out;
	}

	private int[] buildActivePoseParameters() {
		final ArrayList<Integer> active = new ArrayList<Integer>();
		for (final int dpIndex : SCENE_POSE_PARAM_ORDER) {
			final boolean selected = (paramSelect == null) ||
					((dpIndex >= 0) && (dpIndex < paramSelect.length) && paramSelect[dpIndex]);
			if (selected) {
				active.add(dpIndex);
			}
		}
		final int[] out = new int[active.size()];
		for (int i = 0; i < out.length; i++) {
			out[i] = active.get(i);
		}
		return out;
	}

	private static double[] normalizeWeights(final double[] rawWeights) {
		if ((rawWeights == null) || (rawWeights.length == 0)) {
			return new double[0];
		}
		double sum = 0.0;
		for (int i = 0; i < rawWeights.length; i++) {
			final double w = rawWeights[i];
			if (Double.isFinite(w) && (w > 0.0)) {
				sum += w;
			}
		}
		final double[] out = rawWeights.clone();
		if (sum <= 0.0) {
			Arrays.fill(out, 1.0 / out.length);
			return out;
		}
		for (int i = 0; i < out.length; i++) {
			final double w = out[i];
			out[i] = (Double.isFinite(w) && (w > 0.0)) ? (w / sum) : 0.0;
		}
		return out;
	}

	private static double[][][] cloneScenePoses(final double[][][] scenes_xyzatr) {
		if (scenes_xyzatr == null) {
			return null;
		}
		final double[][][] copy = new double[scenes_xyzatr.length][][];
		for (int i = 0; i < scenes_xyzatr.length; i++) {
			if (scenes_xyzatr[i] == null) {
				continue;
			}
			copy[i] = new double[scenes_xyzatr[i].length][];
			for (int j = 0; j < scenes_xyzatr[i].length; j++) {
				if (scenes_xyzatr[i][j] != null) {
					copy[i][j] = scenes_xyzatr[i][j].clone();
				}
			}
		}
		return copy;
	}

	private void buildLpfRows() {
		final int nvars = classicState.parameterVector.length;
		final ArrayList<Integer> lpfVar = new ArrayList<Integer>();
		final ArrayList<Integer> lpfPrevVar = new ArrayList<Integer>();
		final ArrayList<Integer> lpfNextVar = new ArrayList<Integer>();
		final ArrayList<Double> lpfPrevFixed = new ArrayList<Double>();
		final ArrayList<Double> lpfNextFixed = new ArrayList<Double>();

		for (int i = 0; i < nvars; i++) {
			final int scene = classicState.vectorSceneIndices[i];
			final int dpIndex = classicState.vectorParamIndices[i];
			final double lpfControl = getArrayValue(paramLpf, dpIndex);
			if (!(lpfControl > 0.0)) {
				continue;
			}
			final int prevScene = scene - 1;
			final int nextScene = scene + 1;
			if ((prevScene < earliestScene) || (nextScene > lastScene)) {
				continue;
			}

			int prevVar = -1;
			int nextVar = -1;
			double prevFixed = 0.0;
			double nextFixed = 0.0;

			final Integer prevIdx = classicState.vectorIndexBySceneParam.get(getSceneParamKey(
					prevScene,
					dpIndex));
			if (prevIdx != null) {
				prevVar = prevIdx.intValue();
			} else if ((prevScene >= 0) && (prevScene < scenesXyzatr.length) && isPoseValid(scenesXyzatr[prevScene])) {
				prevFixed = normalizeValue(getScenePoseParameter(
						scenesXyzatr[prevScene],
						dpIndex));
			} else {
				continue;
			}

			final Integer nextIdx = classicState.vectorIndexBySceneParam.get(getSceneParamKey(
					nextScene,
					dpIndex));
			if (nextIdx != null) {
				nextVar = nextIdx.intValue();
			} else if ((nextScene >= 0) && (nextScene < scenesXyzatr.length) && isPoseValid(scenesXyzatr[nextScene])) {
				nextFixed = normalizeValue(getScenePoseParameter(
						scenesXyzatr[nextScene],
						dpIndex));
			} else {
				continue;
			}

			lpfVar.add(Integer.valueOf(i));
			lpfPrevVar.add(Integer.valueOf(prevVar));
			lpfNextVar.add(Integer.valueOf(nextVar));
			lpfPrevFixed.add(Double.valueOf(prevFixed));
			lpfNextFixed.add(Double.valueOf(nextFixed));
		}

		classicState.pullRows = nvars;
		classicState.lpfVar = new int[lpfVar.size()];
		classicState.lpfPrevVar = new int[lpfVar.size()];
		classicState.lpfNextVar = new int[lpfVar.size()];
		classicState.lpfPrevFixed = new double[lpfVar.size()];
		classicState.lpfNextFixed = new double[lpfVar.size()];
		for (int i = 0; i < lpfVar.size(); i++) {
			classicState.lpfVar[i] = lpfVar.get(i).intValue();
			classicState.lpfPrevVar[i] = lpfPrevVar.get(i).intValue();
			classicState.lpfNextVar[i] = lpfNextVar.get(i).intValue();
			classicState.lpfPrevFixed[i] = lpfPrevFixed.get(i).doubleValue();
			classicState.lpfNextFixed[i] = lpfNextFixed.get(i).doubleValue();
		}
	}

	private void buildStateVector() {
		classicState.activePoseParams = buildActivePoseParameters();
		if (scenesXyzatr == null) {
			classicState.vectorSceneIndices = new int[0];
			classicState.vectorParamIndices = new int[0];
			classicState.parameterVector = new double[0];
			classicState.parameterInitial = new double[0];
			classicState.parameterPull = new double[0];
			classicState.vectorIndexBySceneParam.clear();
			classicState.pullRows = 0;
			classicState.lpfVar = new int[0];
			classicState.lpfPrevVar = new int[0];
			classicState.lpfNextVar = new int[0];
			classicState.lpfPrevFixed = new double[0];
			classicState.lpfNextFixed = new double[0];
			classicState.correlationRows = 0;
			classicState.pullBase = 0;
			classicState.lpfBase = 0;
			classicState.correlationPairs.clear();
			classicState.yVector = new double[0];
			classicState.weights = new double[0];
			classicState.clearLinearization();
			return;
		}
		final ArrayList<Integer> vectorSceneIndices = new ArrayList<Integer>();
		final ArrayList<Integer> vectorParamIndices = new ArrayList<Integer>();

		for (int scene = earliestScene; scene <= lastScene; scene++) {
			if (scene == centerIndex) {
				continue;
			}
			if ((scene < 0) || (scene >= scenesXyzatr.length) || !isPoseValid(scenesXyzatr[scene])) {
				continue;
			}
			for (final int dpIndex : classicState.activePoseParams) {
				vectorSceneIndices.add(scene);
				vectorParamIndices.add(dpIndex);
			}
		}

		final int n = vectorSceneIndices.size();
		classicState.vectorSceneIndices = new int[n];
		classicState.vectorParamIndices = new int[n];
		classicState.parameterVector = new double[n];
		classicState.parameterInitial = new double[n];
		classicState.parameterPull = new double[n];
		classicState.vectorIndexBySceneParam.clear();

		for (int i = 0; i < n; i++) {
			final int scene = vectorSceneIndices.get(i);
			final int dpIndex = vectorParamIndices.get(i);
			classicState.vectorSceneIndices[i] = scene;
			classicState.vectorParamIndices[i] = dpIndex;
			classicState.vectorIndexBySceneParam.put(getSceneParamKey(scene, dpIndex), i);

			final double current = normalizeValue(getScenePoseParameter(scenesXyzatr[scene], dpIndex));
			double pull = current;
			if ((scenesXyzatrPull != null) &&
					(scene >= 0) && (scene < scenesXyzatrPull.length) &&
					isPoseValid(scenesXyzatrPull[scene])) {
				pull = normalizeValue(getScenePoseParameter(scenesXyzatrPull[scene], dpIndex));
			}

			classicState.parameterVector[i] = current;
			classicState.parameterInitial[i] = current;
			classicState.parameterPull[i] = pull;
		}
		buildLpfRows();
		classicState.clearLinearization();
	}

	private void captureCurrentVectorFromScenes() {
		if (scenesXyzatr == null) {
			return;
		}
		for (int i = 0; i < classicState.parameterVector.length; i++) {
			final int scene = classicState.vectorSceneIndices[i];
			final int dpIndex = classicState.vectorParamIndices[i];
			if ((scene >= 0) && (scene < scenesXyzatr.length)) {
				classicState.parameterVector[i] = normalizeValue(getScenePoseParameter(scenesXyzatr[scene], dpIndex));
			}
		}
	}

	private void applyVectorToScenes(final double[] vector) {
		if (scenesXyzatr == null) {
			return;
		}
		for (int i = 0; i < vector.length; i++) {
			final int scene = classicState.vectorSceneIndices[i];
			final int dpIndex = classicState.vectorParamIndices[i];
			if ((scene < 0) || (scene >= scenesXyzatr.length) || !isPoseValid(scenesXyzatr[scene])) {
				continue;
			}
			setScenePoseParameter(
					scenesXyzatr[scene],
					dpIndex,
					normalizeValue(vector[i]));
		}
	}

	private static double[][][] setEigenTransform(
			final double eigMaxSqrt,
			final double eigMinSqrt,
			final double[][] eigen) {
		if (eigen == null) {
			return null;
		}
		final double[][][] transform = new double[eigen.length][][];
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		final double k0 = 1.0 / eigMaxSqrt;
		for (int ithread = 0; ithread < threads.length; ithread++) {
			threads[ithread] = new Thread() {
				@Override
				public void run() {
					for (int iTile = ai.getAndIncrement(); iTile < eigen.length; iTile = ai.getAndIncrement()) {
						if (eigen[iTile] == null) {
							continue;
						}
						final double[][] am = {{eigen[iTile][0], eigen[iTile][1]}, {eigen[iTile][1], -eigen[iTile][0]}};
						if (!Double.isNaN(am[0][0]) && !Double.isNaN(am[0][1])) {
							transform[iTile] = new double[2][2];
							for (int i = 0; i < 2; i++) {
								final double k = Math.max(0, 1 / Math.max(eigMinSqrt, Math.sqrt(eigen[iTile][2 + i])) - k0);
								for (int j = 0; j < 2; j++) {
									transform[iTile][i][j] = k * am[i][j];
								}
							}
						}
					}
				}
			};
		}
		ImageDtt.startAndJoin(threads);
		return transform;
	}

	private void buildCombinedRows(
			final ObservationCache obsCache,
			final int firstScene,
			final int endScene,
			final boolean[] activePoseScene,
			final int debugLevel) {
		classicState.correlationPairs.clear();
		final ArrayList<Double> corrY = new ArrayList<Double>();
		final ArrayList<Double> corrW = new ArrayList<Double>();
		int corrRows = 0;

		for (int iobs = 0; iobs < obsCache.observations.size(); iobs++) {
			final PairObservation obs = obsCache.observations.get(iobs);
			if (!(obs.pairWeight > 0.0) || (obs.vectorXys == null) || (obs.centers == null)) {
				continue;
			}
			final int ivar = obs.sceneIndex - firstScene;
			if ((ivar < 0) || (ivar >= activePoseScene.length) || !activePoseScene[ivar]) {
				continue;
			}
			final boolean[] localMask = getLocalParamMaskForPair(
					paramSelect,
					obs.sceneIndex,
					obs.refSceneIndex,
					centerIndex,
					disableErs);
			final int[] localParamIndices = getParameterIndices(localMask);
			if (localParamIndices.length == 0) {
				continue;
			}
			@SuppressWarnings("unchecked")
			final ArrayList<VectorTerm>[] vectorTerms = new ArrayList[localParamIndices.length];
			for (int ilp = 0; ilp < localParamIndices.length; ilp++) {
				vectorTerms[ilp] = mapLocalParameterToVectorTerms(
						localParamIndices[ilp],
						obs.sceneIndex,
						obs.refSceneIndex,
						centerIndex,
						firstScene,
						endScene,
						activePoseScene,
						classicState.rateSourceScenes,
						classicState.includeRateChain);
			}
			final double[][][] eigTrans = setEigenTransform(
					cltParameters.imp.eig_max_sqrt,
					cltParameters.imp.eig_min_sqrt,
					obs.eigen);
			final ArrayList<Integer> validTileList = new ArrayList<Integer>();
			final ArrayList<Double> yRows = new ArrayList<Double>();
			final ArrayList<Double> wRows = new ArrayList<Double>();
			final int nt = Math.min(obs.vectorXys.length, obs.centers.length);
			for (int itile = 0; itile < nt; itile++) {
				final double[] v = obs.vectorXys[itile];
				final double[] center = obs.centers[itile];
				if ((v == null) || (v.length < 3) || (center == null) || (center.length < 3)) {
					continue;
				}
				final double w = v[2];
				if (!(w > 0.0) || !Double.isFinite(w)) {
					continue;
				}
				final double[][] tr = (eigTrans == null) ? null : eigTrans[itile];
				if ((eigTrans != null) && (tr == null)) {
					continue;
				}
				double y0 = v[0];
				double y1 = v[1];
				if (tr != null) {
					final double ty0 = tr[0][0] * y0 + tr[0][1] * y1;
					final double ty1 = tr[1][0] * y0 + tr[1][1] * y1;
					y0 = ty0;
					y1 = ty1;
				}
				validTileList.add(Integer.valueOf(itile));
				yRows.add(Double.valueOf(y0));
				yRows.add(Double.valueOf(y1));
				wRows.add(Double.valueOf(w));
				wRows.add(Double.valueOf(w));
			}
			if (validTileList.isEmpty()) {
				continue;
			}
			final CorrelationPairContext cpc = new CorrelationPairContext();
			cpc.obs = obs;
			cpc.localParamIndices = localParamIndices;
			cpc.vectorTerms = vectorTerms;
			cpc.validTiles = new int[validTileList.size()];
			for (int i = 0; i < cpc.validTiles.length; i++) {
				cpc.validTiles[i] = validTileList.get(i).intValue();
			}
			cpc.eigTrans = eigTrans;
			cpc.yRows = new double[yRows.size()];
			for (int i = 0; i < cpc.yRows.length; i++) {
				cpc.yRows[i] = yRows.get(i).doubleValue();
			}
			cpc.rawWeights = new double[wRows.size()];
			for (int i = 0; i < cpc.rawWeights.length; i++) {
				cpc.rawWeights[i] = wRows.get(i).doubleValue();
			}
			cpc.rowBase = corrRows;
			corrRows += cpc.yRows.length;
			classicState.correlationPairs.add(cpc);
			for (int i = 0; i < cpc.yRows.length; i++) {
				corrY.add(Double.valueOf(cpc.yRows[i]));
				corrW.add(Double.valueOf(cpc.rawWeights[i]));
			}
		}

		classicState.correlationRows = corrRows;
		classicState.pullBase = corrRows;
		classicState.lpfBase = classicState.pullBase + classicState.pullRows;
		final int rows = classicState.lpfBase + classicState.lpfVar.length;
		classicState.yVector = new double[rows];
		final double[] rawWeights = new double[rows];
		for (int i = 0; i < corrRows; i++) {
			classicState.yVector[i] = corrY.get(i).doubleValue();
			rawWeights[i] = corrW.get(i).doubleValue();
		}
		for (int i = 0; i < classicState.pullRows; i++) {
			final int row = classicState.pullBase + i;
			classicState.yVector[row] = 0.0;
			final int dpIndex = classicState.vectorParamIndices[i];
			final double regWeight = getArrayValue(paramRegweights, dpIndex);
			rawWeights[row] = (regWeight > 0.0) ? (regWeight * regWeight) : DEFAULT_PULL_WEIGHT;
		}
		for (int i = 0; i < classicState.lpfVar.length; i++) {
			final int row = classicState.lpfBase + i;
			classicState.yVector[row] = 0.0;
			final int iv = classicState.lpfVar[i];
			final int dpIndex = classicState.vectorParamIndices[iv];
			final double lpfControl = getArrayValue(paramLpf, dpIndex);
			rawWeights[row] = (lpfControl > 0.0) ? (lpfControl * lpfControl) : 0.0;
		}
		classicState.weights = normalizeWeights(rawWeights);
		if (debugLevel > -3) {
			System.out.println(
					"IntersceneGlobalLmaRefine: combined rows built, corrPairs=" + classicState.correlationPairs.size() +
					", corrRows=" + classicState.correlationRows +
					", pullRows=" + classicState.pullRows +
					", lpfRows=" + classicState.lpfVar.length +
					", totalRows=" + rows);
		}
	}

	private boolean rebuildObservationRowsForOuter(final int debugLevel) {
		if ((classicState.parameterVector == null) || (classicState.parameterVector.length == 0)) {
			classicState.correlationPairs.clear();
			classicState.correlationRows = 0;
			classicState.pullBase = 0;
			classicState.lpfBase = classicState.pullRows;
			final int rows = classicState.pullRows + classicState.lpfVar.length;
			classicState.yVector = new double[rows];
			final double[] rawWeights = new double[rows];
			for (int i = 0; i < classicState.pullRows; i++) {
				final int dpIndex = classicState.vectorParamIndices[i];
				final double regWeight = getArrayValue(paramRegweights, dpIndex);
				rawWeights[i] = (regWeight > 0.0) ? (regWeight * regWeight) : DEFAULT_PULL_WEIGHT;
			}
			for (int i = 0; i < classicState.lpfVar.length; i++) {
				final int iv = classicState.lpfVar[i];
				final int dpIndex = classicState.vectorParamIndices[iv];
				final double lpfControl = getArrayValue(paramLpf, dpIndex);
				rawWeights[classicState.pullRows + i] = (lpfControl > 0.0) ? (lpfControl * lpfControl) : 0.0;
			}
			classicState.weights = normalizeWeights(rawWeights);
			return true;
		}
		final int firstScene = Math.max(0, earliestScene);
		final int endScene = Math.min(lastScene, quadCLTs.length - 1);
		final int nvars = endScene - firstScene + 1;
		final boolean[] activePoseScene = new boolean[nvars];
		for (int nscene = firstScene; nscene <= endScene; nscene++) {
			final int ivar = nscene - firstScene;
			activePoseScene[ivar] = (nscene != centerIndex) &&
					(quadCLTs[nscene] != null) &&
					(scenesXyzatr[nscene] != null);
		}
		classicState.rateSourceScenes = options.freezeRateChainInInner ? cloneScenePoses(scenesXyzatr) : scenesXyzatr;
		classicState.includeRateChain = !options.freezeRateChainInInner;

		final ArrayList<PairFactor> pairFactors = buildPairFactors(
				cltParameters,
				quadCLTs,
				scenesXyzatr,
				activePoseScene,
				firstScene,
				endScene,
				centerIndex,
				options.minOffset,
				debugLevel);
		applyPairWeights(
				pairFactors,
				centerIndex,
				options.centerPairWeightMode,
				debugLevel);
		final RefGpuData[] refGpuCache = new RefGpuData[nvars];
		final double[] minMax = {options.minOffset, options.maxOffset, Double.NaN};
		final ObservationCache obsCache = buildObservationCache(
				cltParameters,
				quadCLTs,
				centerCLT,
				centerIndex,
				firstScene,
				endScene,
				scenesXyzatr,
				sceneTimes,
				pairFactors,
				refDisparity,
				pXpYDCenter,
				reliableRef,
				mbMaxGain,
				minMax,
				refGpuCache,
				debugLevel);
		if (obsCache.observations.isEmpty()) {
			classicState.correlationPairs.clear();
			classicState.correlationRows = 0;
			classicState.pullBase = 0;
			classicState.lpfBase = classicState.pullRows;
			final int rows = classicState.pullRows + classicState.lpfVar.length;
			classicState.yVector = new double[rows];
			final double[] rawWeights = new double[rows];
			for (int i = 0; i < classicState.pullRows; i++) {
				final int dpIndex = classicState.vectorParamIndices[i];
				final double regWeight = getArrayValue(paramRegweights, dpIndex);
				rawWeights[i] = (regWeight > 0.0) ? (regWeight * regWeight) : DEFAULT_PULL_WEIGHT;
			}
			for (int i = 0; i < classicState.lpfVar.length; i++) {
				final int iv = classicState.lpfVar[i];
				final int dpIndex = classicState.vectorParamIndices[iv];
				final double lpfControl = getArrayValue(paramLpf, dpIndex);
				rawWeights[classicState.pullRows + i] = (lpfControl > 0.0) ? (lpfControl * lpfControl) : 0.0;
			}
			classicState.weights = normalizeWeights(rawWeights);
			return false;
		}
		buildCombinedRows(
				obsCache,
				firstScene,
				endScene,
				activePoseScene,
				debugLevel);
		return true;
	}

	private void prepareLMA(final int debugLevel) {
		buildStateVector();
		applyVectorToScenes(classicState.parameterVector);
		final int firstScene = Math.max(0, earliestScene);
		final int endScene = Math.min(lastScene, quadCLTs.length - 1);
		if ((endScene >= firstScene) && (centerCLT != null)) {
			refDisparity = (centerDisparity != null) ? centerDisparity :
					centerCLT.getDLS()[cltParameters.imp.use_lma_dsi ? 1 : 0];
			pXpYDCenter = OpticalFlow.transformToScenePxPyD(
					null,
					refDisparity,
					Interscene.ZERO3,
					Interscene.ZERO3,
					centerCLT,
					centerCLT);
			sceneTimes = getSceneTimes(
					quadCLTs,
					firstScene,
					endScene);
		}
		rebuildObservationRowsForOuter(debugLevel);
		if (debugLevel > -4) {
			System.out.println(
					"IntersceneGlobalLmaRefine: prepareLMA() classic LMA active; " +
					"unknowns=" + classicState.parameterVector.length +
					", rows=" + classicState.yVector.length +
					", corrRows=" + classicState.correlationRows +
					", pullRows=" + classicState.pullRows +
					", lpfRows=" + classicState.lpfVar.length +
					", activePoseParams=" + Arrays.toString(classicState.activePoseParams) +
					", center=" + centerIndex +
					", range=[" + earliestScene + "," + lastScene + "]" +
					", includeRateChain=" + classicState.includeRateChain);
		}
	}

	private double[] getFxDerivs(
			final double[] vector,
			final double[][] jt,
			final int debugLevel) {
		applyVectorToScenes(vector);
		final int nvars = vector.length;
		final int nrows = classicState.yVector.length;
		final double[] fx = new double[nrows];
		if (jt != null) {
			for (int p = 0; p < nvars; p++) {
				if ((jt[p] == null) || (jt[p].length != nrows)) {
					jt[p] = new double[nrows];
				}
				Arrays.fill(jt[p], 0.0);
			}
		}

		// Correlation block: one multithreaded tile loop per pair.
		for (int ipair = 0; ipair < classicState.correlationPairs.size(); ipair++) {
			final CorrelationPairContext cpc = classicState.correlationPairs.get(ipair);
			final PairObservation obs = cpc.obs;
			if ((obs == null) || (obs.centers == null) || (cpc.validTiles.length == 0)) {
				continue;
			}
			final int nscene = obs.sceneIndex;
			final int refScene = obs.refSceneIndex;
			if ((nscene < 0) || (nscene >= quadCLTs.length) || (quadCLTs[nscene] == null) ||
					(refScene < 0) || (refScene >= quadCLTs.length) || (quadCLTs[refScene] == null)) {
				continue;
			}

			final double[][] poseNow = getScenePose(
					scenesXyzatr,
					nscene,
					centerIndex);
			final double[][] refPose = getScenePose(
					scenesXyzatr,
					refScene,
					centerIndex);
			final double[][] sceneRates = getSceneRates(
					nscene,
					classicState.rateSourceScenes,
					quadCLTs,
					sceneTimes,
					Math.max(0, earliestScene),
					Math.min(lastScene, quadCLTs.length - 1),
					centerIndex);
			final double[][] refRates = getSceneRates(
					refScene,
					classicState.rateSourceScenes,
					quadCLTs,
					sceneTimes,
					Math.max(0, earliestScene),
					Math.min(lastScene, quadCLTs.length - 1),
					centerIndex);

			final ErsCorrection ersRef = quadCLTs[refScene].getErsCorrection();
			final ErsCorrection ersScene = quadCLTs[nscene].getErsCorrection();
			final double[] refWatrBak = ersRef.ers_watr_center_dt.clone();
			final double[] refWxyzBak = ersRef.ers_wxyz_center_dt.clone();
			final double[] sceneWatrBak = ersScene.ers_watr_center_dt.clone();
			final double[] sceneWxyzBak = ersScene.ers_wxyz_center_dt.clone();
			try {
				ersRef.ers_watr_center_dt = refRates[1].clone();
				ersRef.ers_wxyz_center_dt = refRates[0].clone();
				ersScene.ers_watr_center_dt = sceneRates[1].clone();
				ersScene.ers_wxyz_center_dt = sceneRates[0].clone();
				ersScene.setupERS();
				ersRef.setupERS();

				final Matrix[] referenceMatricesInverse = ErsCorrection.getInterRotDeriveMatrices(
						refPose[1],
						true);
				final Matrix[] sceneMatricesInverse = ErsCorrection.getInterRotDeriveMatrices(
						poseNow[1],
						true);
				final Matrix sceneRotMatrix = ErsCorrection.getInterRotDeriveMatrices(
						poseNow[1],
						false)[0];

				final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
				final AtomicInteger ai = new AtomicInteger(0);
				for (int ithread = 0; ithread < threads.length; ithread++) {
					threads[ithread] = new Thread() {
						@Override
						public void run() {
							for (int ilocal = ai.getAndIncrement(); ilocal < cpc.validTiles.length; ilocal = ai.getAndIncrement()) {
								final int itile = cpc.validTiles[ilocal];
								if ((itile < 0) || (itile >= obs.centers.length) || (obs.centers[itile] == null) || (obs.centers[itile].length < 3)) {
									continue;
								}
								final double[] center = obs.centers[itile];
								final boolean isInfinity = center[2] < 0.0;
								final double[][] derivParams = ersRef.getDPxSceneDParameters(
										ersScene,
										true,
										isInfinity,
										center,
										refPose[0],
										poseNow[0],
										referenceMatricesInverse,
										sceneMatricesInverse,
										sceneRotMatrix,
										debugLevel);
								if ((derivParams == null) || (derivParams.length == 0) || (derivParams[0] == null) || (derivParams[0].length < 2)) {
									continue;
								}
								double fx0 = derivParams[0][0];
								double fx1 = derivParams[0][1];
								final double[][] tr = (cpc.eigTrans == null) ? null : cpc.eigTrans[itile];
								if (tr != null) {
									final double t0 = tr[0][0] * fx0 + tr[0][1] * fx1;
									final double t1 = tr[1][0] * fx0 + tr[1][1] * fx1;
									fx0 = t0;
									fx1 = t1;
								}
								final int row0 = cpc.rowBase + 2 * ilocal;
								final int row1 = row0 + 1;
								fx[row0] = fx0;
								fx[row1] = fx1;
								if (jt != null) {
									for (int ip = 0; ip < cpc.localParamIndices.length; ip++) {
										final int parIndex = cpc.localParamIndices[ip];
										final int dindx = parIndex + 1;
										if ((dindx < 0) || (dindx >= derivParams.length) || (derivParams[dindx] == null) || (derivParams[dindx].length < 2)) {
											continue;
										}
										double j0 = derivParams[dindx][0];
										double j1 = derivParams[dindx][1];
										if (tr != null) {
											final double tj0 = tr[0][0] * j0 + tr[0][1] * j1;
											final double tj1 = tr[1][0] * j0 + tr[1][1] * j1;
											j0 = tj0;
											j1 = tj1;
										}
										final ArrayList<VectorTerm> vt = cpc.vectorTerms[ip];
										for (int it = 0; it < vt.size(); it++) {
											final VectorTerm term = vt.get(it);
											jt[term.vectorIndex][row0] += term.coeff * j0;
											jt[term.vectorIndex][row1] += term.coeff * j1;
										}
									}
								}
							}
						}
					};
				}
				ImageDtt.startAndJoin(threads);
			} finally {
				ersRef.ers_watr_center_dt = refWatrBak;
				ersRef.ers_wxyz_center_dt = refWxyzBak;
				ersScene.ers_watr_center_dt = sceneWatrBak;
				ersScene.ers_wxyz_center_dt = sceneWxyzBak;
			}
		}

		// Pull block: one equation per unknown.
		for (int i = 0; i < classicState.pullRows; i++) {
			final int row = classicState.pullBase + i;
			fx[row] = vector[i] - classicState.parameterPull[i];
			if (jt != null) {
				jt[i][row] = 1.0;
			}
		}

		// LPF block: x_i - 0.5*(x_{i-1}+x_{i+1}) for each valid row.
		for (int i = 0; i < classicState.lpfVar.length; i++) {
			final int row = classicState.lpfBase + i;
			final int iv = classicState.lpfVar[i];
			double v = vector[iv];
			final int prevVar = classicState.lpfPrevVar[i];
			if (prevVar >= 0) {
				v -= 0.5 * vector[prevVar];
				if (jt != null) {
					jt[prevVar][row] -= 0.5;
				}
			} else {
				v -= 0.5 * classicState.lpfPrevFixed[i];
			}
			final int nextVar = classicState.lpfNextVar[i];
			if (nextVar >= 0) {
				v -= 0.5 * vector[nextVar];
				if (jt != null) {
					jt[nextVar][row] -= 0.5;
				}
			} else {
				v -= 0.5 * classicState.lpfNextFixed[i];
			}
			fx[row] = v;
			if (jt != null) {
				jt[iv][row] += 1.0;
			}
		}

		if ((debugLevel > 2) && (nrows > 0)) {
			System.out.println("IntersceneGlobalLmaRefine: getFxDerivs() rows=" + nrows +
					", corrRows=" + classicState.correlationRows +
					", pullRows=" + classicState.pullRows +
					", lpfRows=" + classicState.lpfVar.length +
					", unknowns=" + nvars);
		}
		return fx;
	}

	private double[] getYminusFxWeighted(
			final double[] fx,
			final double[] rms,
			final boolean noNaNs) {
		final double[] weighted = new double[fx.length];
		double sum = 0.0;
		double sumPure = 0.0;
		for (int i = 0; i < fx.length; i++) {
			double d = classicState.yVector[i] - fx[i];
			double wd = d * classicState.weights[i];
			if (Double.isNaN(wd)) {
				if (noNaNs) {
					if ((rms != null) && (rms.length >= 2)) {
						rms[0] = Double.NaN;
						rms[1] = Double.NaN;
					}
					return null;
				}
				d = 0.0;
				wd = 0.0;
			}
			weighted[i] = wd;
			final double dwd = d * wd;
			sum += dwd;
			sumPure += d * d;
		}
		if ((rms != null) && (rms.length >= 2)) {
			rms[0] = Math.sqrt(Math.max(0.0, sum));
			rms[1] = Math.sqrt(Math.max(0.0, sumPure / Math.max(1, Math.max(1, classicState.correlationRows))));
		}
		return weighted;
	}

	private double[][] getWJtJlambda(
			final double lambda,
			final double[][] jt) {
		final int n = jt.length;
		final double[][] wjtjl = new double[n][n];
		for (int i = 0; i < n; i++) {
			for (int j = i; j < n; j++) {
				double d = 0.0;
				for (int k = 0; k < classicState.weights.length; k++) {
					d += classicState.weights[k] * jt[i][k] * jt[j][k];
				}
				wjtjl[i][j] = d;
				if (i == j) {
					wjtjl[i][i] += Math.max(MIN_DIAG, d) * lambda;
				} else {
					wjtjl[j][i] = d;
				}
			}
		}
		return wjtjl;
	}

	private static double getMaxAbs(final double[] data) {
		double max = 0.0;
		for (int i = 0; i < data.length; i++) {
			final double a = Math.abs(data[i]);
			if (a > max) {
				max = a;
			}
		}
		return max;
	}

	private boolean[] lmaStep(
			final double lambda,
			final double rmsDiffStop,
			final double deltaStop,
			final int debugLevel) {
		final int n = classicState.parameterVector.length;
		if (n == 0) {
			return new boolean[] {true, true};
		}
		classicState.attemptedSteps++;
		if ((classicState.lastJt == null) || (classicState.lastJt.length != n)) {
			classicState.lastJt = new double[n][];
		}
		if (classicState.lastRms == null) {
			classicState.lastRms = new double[2];
			final double[] fx0 = getFxDerivs(
					classicState.parameterVector,
					classicState.lastJt,
					debugLevel);
			classicState.lastYminusFx = getYminusFxWeighted(
					fx0,
					classicState.lastRms,
					true);
			if (classicState.lastYminusFx == null) {
				return null;
			}
			classicState.initialRms = classicState.lastRms.clone();
		}

		final Matrix yMinusFxWeighted = new Matrix(classicState.lastYminusFx, classicState.lastYminusFx.length);
		final Matrix wjtjLambda = new Matrix(getWJtJlambda(lambda, classicState.lastJt));
		final Matrix jty = (new Matrix(classicState.lastJt)).times(yMinusFxWeighted);
		final Matrix deltaVector;
		try {
			deltaVector = wjtjLambda.inverse().times(jty);
		} catch (RuntimeException ex) {
			return new boolean[] {false, true};
		}

		final double[] delta = deltaVector.getColumnPackedCopy();
		final double maxDelta = getMaxAbs(delta);
		classicState.lastMaxDelta = maxDelta;
		final double[] oldVector = classicState.parameterVector.clone();
		final double[] oldRms = classicState.lastRms.clone();
		final double[] newVector = oldVector.clone();
		for (int i = 0; i < n; i++) {
			newVector[i] += delta[i];
		}

		final double[] fx = getFxDerivs(
				newVector,
				classicState.lastJt,
				debugLevel);
		final double[] rms = new double[2];
		final double[] yMinusFxNew = getYminusFxWeighted(
				fx,
				rms,
				true);
		if ((yMinusFxNew != null) && (rms[0] < oldRms[0])) {
			classicState.parameterVector = newVector;
			classicState.lastRms = rms;
			classicState.lastYminusFx = yMinusFxNew;
			classicState.acceptedSteps++;
			applyVectorToScenes(newVector);
			final boolean convergedByRms = rms[0] >= (oldRms[0] * (1.0 - rmsDiffStop));
			final boolean convergedByDelta = maxDelta <= deltaStop;
			return new boolean[] {true, convergedByRms || convergedByDelta};
		}

		final double[] fxRestore = getFxDerivs(
				oldVector,
				classicState.lastJt,
				debugLevel);
		classicState.lastYminusFx = getYminusFxWeighted(
				fxRestore,
				classicState.lastRms,
				true);
		classicState.parameterVector = oldVector;
		applyVectorToScenes(oldVector);
		return new boolean[] {false, false};
	}

	private IntersceneGlobalRefine.Result runLma(final int debugLevel) {
		final IntersceneGlobalRefine.Result result = new IntersceneGlobalRefine.Result();
		final int unknowns = classicState.parameterVector.length;
		if (unknowns == 0) {
			return result;
		}
		double lambda = (options != null) ? options.lambdaDiag : 0.1;
		if (!(lambda > 0.0)) {
			lambda = 0.1;
		}
		final double lambdaScaleGood = ((options != null) && (options.lambdaScaleGood > 0.0)) ? options.lambdaScaleGood : 0.5;
		final double lambdaScaleBad = ((options != null) && (options.lambdaScaleBad > 1.0)) ? options.lambdaScaleBad : 8.0;
		final double lambdaMax = ((options != null) && (options.lambdaMax > 0.0)) ? options.lambdaMax : 100.0;
		final double rmsDiffStop = ((options != null) && (options.rmsDiffStop >= 0.0)) ? options.rmsDiffStop : 0.001;
		final double deltaStop = ((options != null) && (options.deltaStop > 0.0)) ? options.deltaStop : 1.0e-5;
		final int outerIterations = ((options != null) && (options.outerIterations > 0)) ? options.outerIterations : 1;
		final int innerIterations = ((options != null) && (options.innerIterations > 0)) ? options.innerIterations : 1;

		boolean converged = false;
		int outerDone = 0;
		int innerDone = 0;
		for (int outer = 0; outer < outerIterations; outer++) {
			final boolean haveObs = rebuildObservationRowsForOuter(debugLevel);
			if (!haveObs && (classicState.correlationRows == 0)) {
				if (debugLevel > -2) {
					System.out.println("IntersceneGlobalLmaRefine: no valid correlation rows for outer=" + outer + ", stopping.");
				}
				break;
			}
			classicState.lastJt = null;
			classicState.lastYminusFx = null;
			classicState.lastRms = null;
			classicState.initialRms = null;
			for (int inner = 0; inner < innerIterations; inner++) {
				final boolean[] step = lmaStep(
						lambda,
						rmsDiffStop,
						deltaStop,
						debugLevel);
				if (step == null) {
					converged = true;
					break;
				}
				final boolean improved = step[0];
				final boolean stop = step[1];
				if (improved) {
					lambda *= lambdaScaleGood;
				} else {
					lambda *= lambdaScaleBad;
				}
				if (lambda > lambdaMax) {
					converged = true;
				}
				innerDone = inner + 1;
				outerDone = outer + 1;
				if (debugLevel > -3 && (classicState.lastRms != null)) {
					System.out.println(
							"IntersceneGlobalLmaRefine: outer=" + outer +
							" inner=" + inner +
							" improved=" + improved +
							" stop=" + stop +
							" avgPairRms=" + classicState.lastRms[0] +
							" avgPairRmsPure=" + classicState.lastRms[1] +
							" maxDelta=" + classicState.lastMaxDelta +
							" lambda=" + lambda);
				}
				if (stop || converged) {
					converged = true;
					break;
				}
			}
			if (converged) {
				break;
			}
		}
		captureCurrentVectorFromScenes();
		final HashSet<Integer> solved = new HashSet<Integer>();
		for (final int scene : classicState.vectorSceneIndices) {
			solved.add(Integer.valueOf(scene));
		}
		result.solvedScenes = solved.size();
		result.failedScenes = 0;
		result.outerDone = outerDone;
		result.maxDelta = classicState.lastMaxDelta;
		result.pcgIterationsLast = Math.max(innerDone, classicState.attemptedSteps);
		if (classicState.lastRms != null) {
			result.avgPairRms = classicState.lastRms[0];
			result.avgPairRmsPure = classicState.lastRms[1];
		}
		if (debugLevel > -4) {
			System.out.println(
					"IntersceneGlobalLmaRefine: finished, outerDone=" + result.outerDone +
					", solvedScenes=" + result.solvedScenes +
					", avgPairRms=" + result.avgPairRms +
					", avgPairRmsPure=" + result.avgPairRmsPure +
					", acceptedSteps=" + classicState.acceptedSteps +
					", attemptedSteps=" + classicState.attemptedSteps);
		}
		return result;
	}

	private static ArrayList<PairFactor> buildPairFactors(
			final CLTParameters cltParameters,
			final QuadCLT[] quadCLTs,
			final double[][][] scenes_xyzatr,
			final boolean[] activePoseScene,
			final int firstScene,
			final int lastScene,
			final int centerIndex,
			final double minOffset,
			final int debugLevel) {
		final ArrayList<PairFactor> factors = new ArrayList<PairFactor>();
		final HashSet<Long> used = new HashSet<Long>();

		for (int nscene = firstScene; nscene <= lastScene; nscene++) {
			final int ivar = nscene - firstScene;
			if ((ivar >= 0) && (ivar < activePoseScene.length) && activePoseScene[ivar]) {
				final long key = (((long) nscene) << 32) | (centerIndex & 0xffffffffL);
				if (used.add(key)) {
					factors.add(new PairFactor(
							nscene,
							centerIndex,
							1.0));
				}
			}
		}
		if (!cltParameters.imp.fmg_initial_en) {
			return factors;
		}
		final double avgZ = getAverageAbsZ(
				scenes_xyzatr,
				activePoseScene,
				firstScene);
		final ArrayList<Integer> fpnList = new ArrayList<Integer>();
		for (int nscene = firstScene; nscene <= lastScene; nscene++) {
			final int ivar = nscene - firstScene;
			if ((ivar < 0) || (ivar >= activePoseScene.length) || !activePoseScene[ivar]) {
				continue;
			}
			double estShift = quadCLTs[centerIndex].estimateAverageShift(
					getScenePose(scenes_xyzatr, centerIndex, centerIndex),
					getScenePose(scenes_xyzatr, nscene, centerIndex),
					avgZ,
					false,
					cltParameters.imp.fmg_rectilinear);
			if (estShift < minOffset) {
				estShift = quadCLTs[centerIndex].estimateAverageShift(
						getScenePose(scenes_xyzatr, centerIndex, centerIndex),
						getScenePose(scenes_xyzatr, nscene, centerIndex),
						avgZ,
						true,
						cltParameters.imp.fmg_rectilinear);
			}
			if (estShift < minOffset) {
				fpnList.add(nscene);
			}
		}
		if (fpnList.isEmpty()) {
			return factors;
		}
		double fmgDistance = cltParameters.imp.fmg_distance;
		if (fmgDistance < (minOffset + 2.0)) {
			fmgDistance = minOffset + 2.0;
		}
		final int[][] fpnPairs = Interscene.getFPNPairs(
				fpnList,
				fmgDistance,
				cltParameters.imp.fmg_rectilinear,
				quadCLTs,
				scenes_xyzatr,
				avgZ,
				centerIndex,
				firstScene);
		for (int i = 0; i < fpnPairs.length; i++) {
			final int scene = fpnPairs[i][0];
			final int ref = fpnPairs[i][1];
			if ((scene < firstScene) || (scene > lastScene) || (ref < firstScene) || (ref > lastScene)) {
				continue;
			}
			if ((scene == ref) || (ref == centerIndex)) {
				continue;
			}
			final int ivarScene = scene - firstScene;
			final int ivarRef = ref - firstScene;
			if ((ivarScene < 0) || (ivarScene >= activePoseScene.length) || !activePoseScene[ivarScene]) {
				continue;
			}
			if ((ivarRef < 0) || (ivarRef >= activePoseScene.length) || !activePoseScene[ivarRef]) {
				continue;
			}
			if ((quadCLTs[ref] == null) || (scenes_xyzatr[ref] == null)) {
				continue;
			}
			final long key = (((long) scene) << 32) | (ref & 0xffffffffL);
			if (used.add(key)) {
				factors.add(new PairFactor(
						scene,
						ref,
						1.0));
			}
		}
		if (debugLevel > -3) {
			System.out.println("IntersceneGlobalLmaRefine: pair factors total=" + factors.size());
		}
		return factors;
	}

	private static void applyPairWeights(
			final ArrayList<PairFactor> pairFactors,
			final int centerIndex,
			final int centerPairWeightMode,
			final int debugLevel) {
		if ((pairFactors == null) || pairFactors.isEmpty()) {
			return;
		}
		final int mode = Math.max(0, centerPairWeightMode);
		int numZero = 0;
		for (PairFactor pf : pairFactors) {
			double w = 1.0;
			if ((mode > 0) && (pf.refSceneIndex == centerIndex)) {
				final int d = Math.abs(pf.sceneIndex - centerIndex);
				if ((d > 0) && (d <= mode)) {
					w = 0.0;
				}
			}
			pf.pairWeight = w;
			if (!(w > 0.0)) {
				numZero++;
			}
		}
		if (debugLevel > -3) {
			System.out.println(
					"IntersceneGlobalLmaRefine: pair weights applied, mode=" + mode +
					", zeroWeightedPairs=" + numZero + "/" + pairFactors.size());
		}
	}

	private static double getAverageAbsZ(
			final double[][][] scenes_xyzatr,
			final boolean[] activePoseScene,
			final int firstScene) {
		double sum = 0.0;
		int n = 0;
		for (int ivar = 0; ivar < activePoseScene.length; ivar++) {
			if (!activePoseScene[ivar]) {
				continue;
			}
			final int nscene = firstScene + ivar;
			if ((nscene >= 0) && (nscene < scenes_xyzatr.length) && (scenes_xyzatr[nscene] != null)) {
				sum += Math.abs(scenes_xyzatr[nscene][0][2]);
				n++;
			}
		}
		return (n > 0) ? Math.max(1.0, sum / n) : 1.0;
	}

	private static RefGpuData getReferenceGpuData(
			final CLTParameters clt_parameters,
			final QuadCLT[] quadCLTs,
			final double[][][] scenes_xyzatr,
			final double[] sceneTimes,
			final int firstScene,
			final int lastScene,
			final int centerIndex,
			final int refScene,
			final double[] refDisparity,
			final double[][] pXpYDCenter,
			final boolean[] reliableRefCenter,
			final double mb_max_gain,
			final RefGpuData[] cache,
			final int debugLevel) {
		final int ivar = refScene - firstScene;
		if ((ivar >= 0) && (ivar < cache.length) && (cache[ivar] != null) &&
				(cache[ivar].refSceneIndex == refScene) && (cache[ivar].tasks != null) &&
				(cache[ivar].pXpYD != null)) {
			return cache[ivar];
		}
		if ((refScene < 0) || (refScene >= quadCLTs.length) || (quadCLTs[refScene] == null)) {
			return null;
		}
		final RefGpuData rslt = new RefGpuData();
		rslt.refSceneIndex = refScene;
		final double[][] refPose = getScenePose(
				scenes_xyzatr,
				refScene,
				centerIndex);
		final double[][] refRates = getSceneRates(
				refScene,
				scenes_xyzatr,
				quadCLTs,
				sceneTimes,
				firstScene,
				lastScene,
				centerIndex);
		rslt.pXpYD = (refScene == centerIndex) ? pXpYDCenter : OpticalFlow.transformToScenePxPyD(
				null,
				refDisparity,
				refPose[0],
				refPose[1],
				quadCLTs[refScene],
				quadCLTs[centerIndex]);
		rslt.reliableMask = (refScene == centerIndex) ? reliableRefCenter : buildPairReliableMask(
				clt_parameters,
				quadCLTs,
				centerIndex,
				refScene,
				centerIndex,
				scenes_xyzatr,
				reliableRefCenter,
				debugLevel);
		double[][] mbVectorsRef = null;
		if (clt_parameters.imp.mb_en) {
			final double[][] refDt = QuadCLTCPU.scaleDtFromErs(
					clt_parameters,
					new double[][] {
						refRates[0].clone(),
						refRates[1].clone()});
			mbVectorsRef = OpticalFlow.getMotionBlur(
					quadCLTs[centerIndex],
					quadCLTs[refScene],
					pXpYDCenter,
					refPose[0],
					refPose[1],
					refDt[0],
					refDt[1],
					0,
					debugLevel);
		}
		final TpTask[][] tasks = Interscene.setReferenceGPU(
				clt_parameters,
				quadCLTs[refScene],
				refDisparity,
				rslt.pXpYD,
				null,
				rslt.reliableMask,
				0,
				clt_parameters.imp.mb_tau,
				mb_max_gain,
				mbVectorsRef,
				debugLevel);
		if ((tasks == null) || (tasks.length == 0) || (tasks[0] == null)) {
			return null;
		}
		rslt.tasks = tasks[0];
		if ((ivar >= 0) && (ivar < cache.length)) {
			cache[ivar] = rslt;
		}
		return rslt;
	}

	private static ObservationCache buildObservationCache(
			final CLTParameters clt_parameters,
			final QuadCLT[] quadCLTs,
			final QuadCLT centerCLT,
			final int centerIndex,
			final int firstScene,
			final int lastScene,
			final double[][][] scenes_xyzatr,
			final double[] sceneTimes,
			final ArrayList<PairFactor> pairFactors,
			final double[] refDisparity,
			final double[][] pXpYDCenter,
			final boolean[] reliableRef,
			final double mb_max_gain,
			final double[] minMax,
			final RefGpuData[] refGpuCache,
			final int debugLevel) {
		final ObservationCache cache = new ObservationCache();
		if ((pairFactors == null) || pairFactors.isEmpty()) {
			return cache;
		}
		final int tilesX = centerCLT.getTileProcessor().getTilesX();
		final int tilesY = centerCLT.getTileProcessor().getTilesY();
		cache.tilesPerPair = tilesX * tilesY;
		cache.yVector = new double[2 * cache.tilesPerPair * pairFactors.size()];
		cache.wVector = new double[cache.tilesPerPair * pairFactors.size()];
		for (int ifactor = 0; ifactor < pairFactors.size(); ifactor++) {
			final PairFactor pf = pairFactors.get(ifactor);
			final RefGpuData refGpu = getReferenceGpuData(
					clt_parameters,
					quadCLTs,
					scenes_xyzatr,
					sceneTimes,
					firstScene,
					lastScene,
					centerIndex,
					pf.refSceneIndex,
					refDisparity,
					pXpYDCenter,
					reliableRef,
					mb_max_gain,
					refGpuCache,
					debugLevel);
			if (refGpu == null) {
				cache.failedPairs++;
				continue;
			}
			final PairObservation obs = buildPairObservation(
					clt_parameters,
					quadCLTs,
					centerCLT,
					centerIndex,
					pf.sceneIndex,
					pf.refSceneIndex,
					firstScene,
					lastScene,
					scenes_xyzatr,
					sceneTimes,
					refGpu.pXpYD,
					refGpu.tasks,
					refGpu.reliableMask,
					refDisparity,
					minMax,
					mb_max_gain,
					pf.pairWeight,
					ifactor,
					cache,
					debugLevel);
			if (obs == null) {
				cache.failedPairs++;
				continue;
			}
			cache.observations.add(obs);
			cache.solvedPairs++;
		}
		return cache;
	}

	private static PairObservation buildPairObservation(
			final CLTParameters clt_parameters,
			final QuadCLT[] quadCLTs,
			final QuadCLT centerCLT,
			final int centerIndex,
			final int nscene,
			final int refScene,
			final int earliestScene,
			final int lastScene,
			final double[][][] scenes_xyzatr,
			final double[] sceneTimes,
			final double[][] pXpYDRef,
			final TpTask[] tpTaskRef,
			final boolean[] reliableRef,
			final double[] refDisparity,
			final double[] minMax,
			final double mb_max_gain,
			final double pairWeight,
			final int pairOrdinal,
			final ObservationCache cache,
			final int debugLevel) {
		if ((quadCLTs[nscene] == null) || (scenes_xyzatr[nscene] == null)) {
			return null;
		}
		final boolean[] reliablePair = buildPairReliableMask(
				clt_parameters,
				quadCLTs,
				centerIndex,
				nscene,
				refScene,
				scenes_xyzatr,
				reliableRef,
				debugLevel);
		final double[][] poseNow = getScenePose(
				scenes_xyzatr,
				nscene,
				centerIndex);
		final double[][] sceneRates = getSceneRates(
				nscene,
				scenes_xyzatr,
				quadCLTs,
				sceneTimes,
				earliestScene,
				lastScene,
				centerIndex);
		double[][] mbVectorsScene = null;
		if (clt_parameters.imp.mb_en) {
			final double[][] sceneDt = QuadCLTCPU.scaleDtFromErs(
					clt_parameters,
					new double[][] {
						sceneRates[0].clone(),
						sceneRates[1].clone()});
			mbVectorsScene = OpticalFlow.getMotionBlur(
					centerCLT,
					quadCLTs[nscene],
					pXpYDRef,
					poseNow[0],
					poseNow[1],
					sceneDt[0],
					sceneDt[1],
					0,
					debugLevel);
		}
		final int[] failReason = {0};
		final double[][][] coordMotion = Interscene.interCorrPair(
				clt_parameters,
				false,
				false,
				mb_max_gain,
				minMax,
				failReason,
				centerCLT,
				refDisparity,
				quadCLTs[refScene],
				pXpYDRef,
				tpTaskRef,
				quadCLTs[nscene],
				poseNow[0],
				poseNow[1],
				reliablePair,
				clt_parameters.imp.margin,
				clt_parameters.imp.sensor_mask_inter,
				null,
				null,
				false,
				false,
				true,
				mbVectorsScene,
				-1,
				false,
				clt_parameters.imp.debug_level,
				debugLevel);
		if ((coordMotion == null) || (coordMotion.length < 2) || (coordMotion[0] == null) || (coordMotion[1] == null)) {
			return null;
		}
		final PairObservation obs = new PairObservation();
		obs.sceneIndex = nscene;
		obs.refSceneIndex = refScene;
		obs.pairWeight = pairWeight;
		obs.centers = coordMotion[0];
		obs.eigen = (coordMotion.length > 2) ? coordMotion[2] : null;
		obs.reliableRef = reliablePair;
		obs.yBase = pairOrdinal * 2 * cache.tilesPerPair;
		obs.wBase = pairOrdinal * cache.tilesPerPair;
		final int pairTiles = obs.centers.length;
		final int ntiles = Math.min(
				Math.min(cache.tilesPerPair, pairTiles),
				coordMotion[1].length);
		for (int itile = 0; itile < ntiles; itile++) {
			final double[] v = coordMotion[1][itile];
			if ((v == null) || (v.length < 3)) {
				continue;
			}
			final boolean reliable = (reliablePair == null) || ((itile < reliablePair.length) && reliablePair[itile]);
			final double w = (pairWeight > 0.0) ? ((reliable ? v[2] : 0.0) * pairWeight) : 0.0;
			cache.yVector[obs.yBase + 2 * itile + 0] = v[0];
			cache.yVector[obs.yBase + 2 * itile + 1] = v[1];
			cache.wVector[obs.wBase + itile] = w;
		}
		obs.vectorXys = new double[pairTiles][];
		for (int itile = 0; itile < pairTiles; itile++) {
			if (itile >= cache.tilesPerPair) {
				obs.vectorXys[itile] = null;
				continue;
			}
			final double w = cache.wVector[obs.wBase + itile];
			if (w <= 0.0) {
				obs.vectorXys[itile] = null;
			} else {
				obs.vectorXys[itile] = new double[] {
						cache.yVector[obs.yBase + 2 * itile + 0],
						cache.yVector[obs.yBase + 2 * itile + 1],
						w};
			}
		}
		return obs;
	}

	private static boolean[] buildPairReliableMask(
			final CLTParameters clt_parameters,
			final QuadCLT[] quadCLTs,
			final int centerIndex,
			final int sceneIndex,
			final int refSceneIndex,
			final double[][][] scenes_xyzatr,
			final boolean[] reliableBase,
			final int debugLevel) {
		if ((centerIndex < 0) || (centerIndex >= quadCLTs.length) || (quadCLTs[centerIndex] == null)) {
			return reliableBase;
		}
		final QuadCLT centerCLT = quadCLTs[centerIndex];
		final int tilesX = centerCLT.getTileProcessor().getTilesX();
		final int tilesY = centerCLT.getTileProcessor().getTilesY();
		boolean[] reliable = reliableBase;
		if (reliable == null) {
			reliable = new boolean[tilesX * tilesY];
			Arrays.fill(reliable, true);
		}
		final double[][] centerPose = getScenePose(
				scenes_xyzatr,
				centerIndex,
				centerIndex);
		final double[][] scenePose = getScenePose(
				scenes_xyzatr,
				sceneIndex,
				centerIndex);
		final double avgZ = centerCLT.getAverageZ(true);
		final boolean fmgRectilinear = clt_parameters.imp.fmg_rectilinear;
		final double[] centerToSceneOffset = centerCLT.estimateCenterShiftXY(
				centerPose,
				scenePose,
				avgZ,
				fmgRectilinear);
		if (centerToSceneOffset != null) {
			reliable = centerCLT.maskByOverlap(
					reliable,
					centerToSceneOffset);
		}
		if (refSceneIndex != centerIndex) {
			final double[][] refPose = getScenePose(
					scenes_xyzatr,
					refSceneIndex,
					centerIndex);
			final double[] centerToRefOffset = centerCLT.estimateCenterShiftXY(
					centerPose,
					refPose,
					avgZ,
					fmgRectilinear);
			if (centerToRefOffset != null) {
				reliable = centerCLT.maskByOverlap(
						reliable,
						centerToRefOffset);
			}
		}
		if (debugLevel > 1) {
			int selected = 0;
			for (int i = 0; i < reliable.length; i++) {
				if (reliable[i]) {
					selected++;
				}
			}
			System.out.println(
					"IntersceneGlobalLmaRefine: overlap mask scene=" + sceneIndex +
					", ref=" + refSceneIndex +
					", selected=" + selected + "/" + reliable.length);
		}
		return reliable;
	}

	private static double parseSceneTimestamp(final String ts) {
		if (ts == null) {
			return Double.NaN;
		}
		final String[] parts = ts.trim().split("_");
		if (parts.length != 2) {
			return Double.NaN;
		}
		try {
			final long sec = Long.parseLong(parts[0]);
			final long usec = Long.parseLong(parts[1]);
			return sec + (usec * 1.0e-6);
		} catch (NumberFormatException nfe) {
			return Double.NaN;
		}
	}

	private static double[] getSceneTimes(
			final QuadCLT[] quadCLTs,
			final int firstScene,
			final int lastScene) {
		final double[] out = new double[quadCLTs.length];
		Arrays.fill(out, Double.NaN);
		for (int nscene = firstScene; nscene <= lastScene; nscene++) {
			if (quadCLTs[nscene] == null) {
				continue;
			}
			out[nscene] = parseSceneTimestamp(quadCLTs[nscene].getImageName());
		}
		double t = 0.0;
		for (int nscene = firstScene; nscene <= lastScene; nscene++) {
			if (Double.isNaN(out[nscene])) {
				out[nscene] = t;
			}
			t = out[nscene] + DEFAULT_DT;
		}
		return out;
	}

	private static int findPrevScene(
			final int sceneIndex,
			final int firstScene,
			final QuadCLT[] quadCLTs,
			final double[][][] scenes_xyzatr) {
		for (int i = sceneIndex - 1; i >= firstScene; i--) {
			if ((quadCLTs[i] != null) && (scenes_xyzatr[i] != null)) {
				return i;
			}
		}
		return -1;
	}

	private static int findNextScene(
			final int sceneIndex,
			final int lastScene,
			final QuadCLT[] quadCLTs,
			final double[][][] scenes_xyzatr) {
		for (int i = sceneIndex + 1; i <= lastScene; i++) {
			if ((quadCLTs[i] != null) && (scenes_xyzatr[i] != null)) {
				return i;
			}
		}
		return -1;
	}

	private static double safeDt(
			final int i0,
			final int i1,
			final double[] sceneTimes) {
		if ((sceneTimes == null) ||
				(i0 < 0) || (i1 < 0) ||
				(i0 >= sceneTimes.length) || (i1 >= sceneTimes.length)) {
			return DEFAULT_DT;
		}
		final double dt = Math.abs(sceneTimes[i1] - sceneTimes[i0]);
		return (dt > MIN_DIAG) ? dt : DEFAULT_DT;
	}

	private static double wrapPi(double a) {
		while (a > Math.PI) {
			a -= (2.0 * Math.PI);
		}
		while (a < -Math.PI) {
			a += (2.0 * Math.PI);
		}
		return a;
	}

	private static double getPoseComponent(
			final double[][][] scenes_xyzatr,
			final int sceneIndex,
			final int centerIndex,
			final boolean angular,
			final int comp) {
		if (sceneIndex == centerIndex) {
			return 0.0;
		}
		if ((sceneIndex < 0) || (sceneIndex >= scenes_xyzatr.length) || (scenes_xyzatr[sceneIndex] == null)) {
			return 0.0;
		}
		return angular ? scenes_xyzatr[sceneIndex][1][comp] : scenes_xyzatr[sceneIndex][0][comp];
	}

	private static ArrayList<RateTerm> getRateTerms(
			final int sceneIndex,
			final int firstScene,
			final int lastScene,
			final QuadCLT[] quadCLTs,
			final double[][][] scenes_xyzatr,
			final double[] sceneTimes) {
		final int prev = findPrevScene(
				sceneIndex,
				firstScene,
				quadCLTs,
				scenes_xyzatr);
		final int next = findNextScene(
				sceneIndex,
				lastScene,
				quadCLTs,
				scenes_xyzatr);
		final ArrayList<RateTerm> terms = new ArrayList<RateTerm>(2);
		if ((prev < 0) && (next < 0)) {
			return terms;
		}
		if (prev < 0) {
			final double dt = safeDt(sceneIndex, next, sceneTimes);
			terms.add(new RateTerm(sceneIndex, -1.0 / dt));
			terms.add(new RateTerm(next, 1.0 / dt));
			return terms;
		}
		if (next < 0) {
			final double dt = safeDt(prev, sceneIndex, sceneTimes);
			terms.add(new RateTerm(prev, -1.0 / dt));
			terms.add(new RateTerm(sceneIndex, 1.0 / dt));
			return terms;
		}
		final double dt = safeDt(prev, next, sceneTimes);
		terms.add(new RateTerm(prev, -1.0 / dt));
		terms.add(new RateTerm(next, 1.0 / dt));
		return terms;
	}

	private static double getRateValue(
			final int sceneIndex,
			final boolean angular,
			final int comp,
			final int firstScene,
			final int lastScene,
			final int centerIndex,
			final double[][][] scenes_xyzatr,
			final QuadCLT[] quadCLTs,
			final double[] sceneTimes) {
		final int prev = findPrevScene(
				sceneIndex,
				firstScene,
				quadCLTs,
				scenes_xyzatr);
		final int next = findNextScene(
				sceneIndex,
				lastScene,
				quadCLTs,
				scenes_xyzatr);
		if ((prev < 0) && (next < 0)) {
			return 0.0;
		}
		if (prev < 0) {
			final double dt = safeDt(sceneIndex, next, sceneTimes);
			final double d = getPoseComponent(scenes_xyzatr, next, centerIndex, angular, comp) -
					getPoseComponent(scenes_xyzatr, sceneIndex, centerIndex, angular, comp);
			return (angular ? wrapPi(d) : d) / dt;
		}
		if (next < 0) {
			final double dt = safeDt(prev, sceneIndex, sceneTimes);
			final double d = getPoseComponent(scenes_xyzatr, sceneIndex, centerIndex, angular, comp) -
					getPoseComponent(scenes_xyzatr, prev, centerIndex, angular, comp);
			return (angular ? wrapPi(d) : d) / dt;
		}
		final double dt = safeDt(prev, next, sceneTimes);
		final double d = getPoseComponent(scenes_xyzatr, next, centerIndex, angular, comp) -
				getPoseComponent(scenes_xyzatr, prev, centerIndex, angular, comp);
		return (angular ? wrapPi(d) : d) / dt;
	}

	private static double[][] getSceneRates(
			final int sceneIndex,
			final double[][][] scenes_xyzatr,
			final QuadCLT[] quadCLTs,
			final double[] sceneTimes,
			final int firstScene,
			final int lastScene,
			final int centerIndex) {
		final double[][] rates = new double[2][3];
		for (int i = 0; i < 3; i++) {
			rates[0][i] = getRateValue(
					sceneIndex,
					false,
					i,
					firstScene,
					lastScene,
					centerIndex,
					scenes_xyzatr,
					quadCLTs,
					sceneTimes);
			rates[1][i] = getRateValue(
					sceneIndex,
					true,
					i,
					firstScene,
					lastScene,
					centerIndex,
					scenes_xyzatr,
					quadCLTs,
					sceneTimes);
		}
		return rates;
	}

	private static double[][] getScenePose(
			final double[][][] scenes_xyzatr,
			final int sceneIndex,
			final int centerIndex) {
		if (sceneIndex == centerIndex) {
			return new double[][] {
				Interscene.ZERO3.clone(),
				Interscene.ZERO3.clone()};
		}
		if ((sceneIndex < 0) || (sceneIndex >= scenes_xyzatr.length) || (scenes_xyzatr[sceneIndex] == null)) {
			return new double[][] {
				Interscene.ZERO3.clone(),
				Interscene.ZERO3.clone()};
		}
		return new double[][] {
			scenes_xyzatr[sceneIndex][0].clone(),
			scenes_xyzatr[sceneIndex][1].clone()};
	}

	private static boolean[] getLocalParamMaskForPair(
			final boolean[] paramSelect,
			final int sceneIndex,
			final int refSceneIndex,
			final int centerIndex,
			final boolean disableErs) {
		final boolean[] mask = new boolean[ErsCorrection.DP_NUM_PARS];
		if (paramSelect != null) {
			for (int i = 0; i < SCENE_POSE_PARAM_ORDER.length; i++) {
				final int pi = SCENE_POSE_PARAM_ORDER[i];
				if (pi < paramSelect.length) {
					mask[pi] = paramSelect[pi];
				}
			}
		} else {
			for (int i = 0; i < SCENE_POSE_PARAM_ORDER.length; i++) {
				mask[SCENE_POSE_PARAM_ORDER[i]] = true;
			}
		}
		if (sceneIndex == centerIndex) {
			for (int i = 0; i < SCENE_POSE_PARAM_ORDER.length; i++) {
				mask[SCENE_POSE_PARAM_ORDER[i]] = false;
			}
		}
		for (int i = 0; i < REF_RATE_ANG_PAR_INDICES.length; i++) {
			mask[REF_RATE_ANG_PAR_INDICES[i]] = !disableErs;
			mask[SCENE_RATE_ANG_PAR_INDICES[i]] = !disableErs;
			mask[REF_RATE_LIN_PAR_INDICES[i]] = true;
			mask[SCENE_RATE_LIN_PAR_INDICES[i]] = true;
		}
		return mask;
	}

	private ArrayList<VectorTerm> mapLocalParameterToVectorTerms(
			final int parIndex,
			final int sceneIndex,
			final int refSceneIndex,
			final int centerIndex,
			final int firstScene,
			final int lastScene,
			final boolean[] activePoseScene,
			final double[][][] scenesRatesSource,
			final boolean includeRateChain) {
		final ArrayList<VectorTerm> rslt = new ArrayList<VectorTerm>(2);
		switch (parIndex) {
		case ErsCorrection.DP_DSAZ:
		case ErsCorrection.DP_DSTL:
		case ErsCorrection.DP_DSRL:
		case ErsCorrection.DP_DSX:
		case ErsCorrection.DP_DSY:
		case ErsCorrection.DP_DSZ:
		{
			final Integer vidx = classicState.vectorIndexBySceneParam.get(getSceneParamKey(sceneIndex, parIndex));
			if (vidx != null) {
				rslt.add(new VectorTerm(vidx.intValue(), 1.0));
			}
			break;
		}
		case ErsCorrection.DP_DSVAZ:
		case ErsCorrection.DP_DSVTL:
		case ErsCorrection.DP_DSVRL:
		{
			if (!includeRateChain) {
				break;
			}
			final int comp = parIndex - ErsCorrection.DP_DSVAZ;
			final int posePar = ErsCorrection.DP_DSAZ + comp;
			final ArrayList<RateTerm> rts = getRateTerms(
					sceneIndex,
					firstScene,
					lastScene,
					quadCLTs,
					scenesRatesSource,
					sceneTimes);
			for (int i = 0; i < rts.size(); i++) {
				final RateTerm rt = rts.get(i);
				if (rt.sceneIndex == centerIndex) {
					continue;
				}
				final int ivar = rt.sceneIndex - firstScene;
				if ((ivar < 0) || (ivar >= activePoseScene.length) || !activePoseScene[ivar]) {
					continue;
				}
				final Integer vidx = classicState.vectorIndexBySceneParam.get(getSceneParamKey(rt.sceneIndex, posePar));
				if (vidx != null) {
					rslt.add(new VectorTerm(vidx.intValue(), rt.coeff));
				}
			}
			break;
		}
		case ErsCorrection.DP_DSVX:
		case ErsCorrection.DP_DSVY:
		case ErsCorrection.DP_DSVZ:
		{
			if (!includeRateChain) {
				break;
			}
			final int comp = parIndex - ErsCorrection.DP_DSVX;
			final int posePar = ErsCorrection.DP_DSX + comp;
			final ArrayList<RateTerm> rts = getRateTerms(
					sceneIndex,
					firstScene,
					lastScene,
					quadCLTs,
					scenesRatesSource,
					sceneTimes);
			for (int i = 0; i < rts.size(); i++) {
				final RateTerm rt = rts.get(i);
				if (rt.sceneIndex == centerIndex) {
					continue;
				}
				final int ivar = rt.sceneIndex - firstScene;
				if ((ivar < 0) || (ivar >= activePoseScene.length) || !activePoseScene[ivar]) {
					continue;
				}
				final Integer vidx = classicState.vectorIndexBySceneParam.get(getSceneParamKey(rt.sceneIndex, posePar));
				if (vidx != null) {
					rslt.add(new VectorTerm(vidx.intValue(), rt.coeff));
				}
			}
			break;
		}
		case ErsCorrection.DP_DVAZ:
		case ErsCorrection.DP_DVTL:
		case ErsCorrection.DP_DVRL:
		{
			if (!includeRateChain) {
				break;
			}
			final int comp = parIndex - ErsCorrection.DP_DVAZ;
			final int posePar = ErsCorrection.DP_DSAZ + comp;
			final ArrayList<RateTerm> rts = getRateTerms(
					refSceneIndex,
					firstScene,
					lastScene,
					quadCLTs,
					scenesRatesSource,
					sceneTimes);
			for (int i = 0; i < rts.size(); i++) {
				final RateTerm rt = rts.get(i);
				if (rt.sceneIndex == centerIndex) {
					continue;
				}
				final int ivar = rt.sceneIndex - firstScene;
				if ((ivar < 0) || (ivar >= activePoseScene.length) || !activePoseScene[ivar]) {
					continue;
				}
				final Integer vidx = classicState.vectorIndexBySceneParam.get(getSceneParamKey(rt.sceneIndex, posePar));
				if (vidx != null) {
					rslt.add(new VectorTerm(vidx.intValue(), rt.coeff));
				}
			}
			break;
		}
		case ErsCorrection.DP_DVX:
		case ErsCorrection.DP_DVY:
		case ErsCorrection.DP_DVZ:
		{
			if (!includeRateChain) {
				break;
			}
			final int comp = parIndex - ErsCorrection.DP_DVX;
			final int posePar = ErsCorrection.DP_DSX + comp;
			final ArrayList<RateTerm> rts = getRateTerms(
					refSceneIndex,
					firstScene,
					lastScene,
					quadCLTs,
					scenesRatesSource,
					sceneTimes);
			for (int i = 0; i < rts.size(); i++) {
				final RateTerm rt = rts.get(i);
				if (rt.sceneIndex == centerIndex) {
					continue;
				}
				final int ivar = rt.sceneIndex - firstScene;
				if ((ivar < 0) || (ivar >= activePoseScene.length) || !activePoseScene[ivar]) {
					continue;
				}
				final Integer vidx = classicState.vectorIndexBySceneParam.get(getSceneParamKey(rt.sceneIndex, posePar));
				if (vidx != null) {
					rslt.add(new VectorTerm(vidx.intValue(), rt.coeff));
				}
			}
			break;
		}
		default:
		}
		return rslt;
	}
}
