package com.elphel.imagej.cuas.rt;
/**
 **
 ** CuasConditioning.java - lean per-sensor image conditioning for the CUAS RT / tile-processor path.
 **
 ** Copyright (C) 2026 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  CuasConditioning.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/>.
 ** -----------------------------------------------------------------------------**
 **
 */

import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.cuas.CuasMotion;
import com.elphel.imagej.gpu.GpuQuad;
import com.elphel.imagej.tileprocessor.QuadCLT;

/**
 * Lean, self-contained per-sensor conditioning for the CUAS real-time / tile-processor path.
 * By Claude on 06/30/2026.
 *
 * Deliberately bypasses the heavy QuadCLT "conditioning over conditioning" instance-array path:
 * it operates directly on raw jp4-domain per-sensor pixel arrays and the (borrowed) calibration
 * values pulled from a QuadCLT (lwir_scales / lwir_offsets / lwir_scales2 and the per-pixel FPN).
 *
 * Pipeline (per sensor), applied in place:
 *   1) Row/Col denoise (optional): estimate a per-row and a per-column average, optionally
 *      high-pass the 1-D profile (preserve the low frequencies for the downstream LoG), subtract.
 *      The HPF is applied to the length-H / length-W average vectors, NOT the full image
 *      (mathematically ~equivalent, far fewer operations).
 *   2) Photometric + fixed-pattern (2-map model, per-pixel-ready):
 *         corrected = scales2*(raw-offset)^2 + scale*(raw-offset)    - FPN(x,y)
 *      matching the existing model (QuadCLT.calibratePhotometric2 forward form) so that with
 *      scales2==0 (the abandoned 2nd-order term) it is bit-identical to today's additive path.
 *      scale = global per-sensor (uniform) for now; the per-pixel *scale* map (multiplicative
 *      dust) is the future improvement, hence the 2-image (scale, offset) framing.
 */
public class CuasConditioning {

	public static class Config {
		public boolean rowcol_enable        = true;   // subtract per-row and per-col noise (runs AFTER photometric+FPN since 07/07/2026 - see condition())
		public boolean rowcol_hpf           = true;   // high-pass the 1-D avg profile before subtracting - applies ONLY when rowcol_um_sigma==0 (the UM measurement already band-limits; stacking both under-removed mid-band lines x2, see subtractRowColSensor) // By Claude on 07/07/2026
		public double  rowcol_hpf_sigma     = 8.0;    // px; ~ the LoG scale (KR3-ish) - noise band the LoG would pass
		public double  rowcol_um_sigma      = 10.0;   // UM sigma for the profile MEASUREMENT (content suppression borrowed from the oracle CorrectionFPN path: gate works on residual counts); 0 = measure on the data as-is (pre-07/07 behavior) // By Claude on 07/07/2026
		public double  rowcol_max_abs       = 50.0;   // only average pixels with |UM residual| within this (outlier reject, calibrated counts - meaningful now that rowcol runs post-photometric on UM residuals); +INF = no gate (the old default: on raw absolute counts the gate was unusable) // By Claude on 07/07/2026
		public double  rowcol_weight_outlier= 0.01;   // small weight for out-of-range pixels
		public double  rowcol_min_inlier    = 0.33;   // minimal INLIER (|UM residual| <= max_abs) pixel fraction of a line to trust its estimate; below it (content-dominated line - e.g. terrain rows, where the outlier-weighted mean degenerates to the CONTENT mean and stamps bias bands, measured +10..22 counts in ROWCOL-04 terrain) the line gets ZERO correction. Also |estimate| > max_abs -> 0 (physical cap: line noise cannot exceed the gate scale). // By Claude on 07/07/2026
		public boolean photometric          = true;   // apply scale*(raw-offset) [+ scales2 quadratic]
		public boolean apply_fpn            = true;    // subtract per-pixel FPN (the offset map's per-pixel part)
		// Zero-DC (Andrey 2026-07-05, restoring a lost pre-CUAS invariant): the MCLT has no
		// clean DC/AC separation - a DC constant spreads over many TD points, costing float32
		// mantissa bits under the small AC signal (LWIR: ~100 counts/K, typical scene AC
		// ~300 counts = ~3K, current DC ~ -3500). Subtract ONE constant from ALL sensors
		// (pure shift - never rescale, counts stay physically meaningful; a common constant
		// preserves the photometric equalization). dc_level = the constant to subtract;
		// NaN = AUTO (the global mean over all sensors of this scene). A calibration-
		// maintained fixed level (no scene-to-scene DC flicker) is the production target.
		public boolean zero_dc              = true;   // By Claude on 07/05/2026
		public double  dc_level             = Double.NaN; // the PER-SEQUENCE constant (CuasRT.sequenceDcLevel - RT-STATE/appropriateCenter). NaN = per-scene sky median FALLBACK ONLY: it oscillates with rotation (terrain sweeps the sky half; 234-count pk-pk @ 1 rev period, POSE-10) and downstream temporal averages turn that into content ghosts. // By Claude on 07/05/2026
		public Config() {}
	}

	/**
	 * Build a Config from the curt dialog parameters (roadmap RT step 2a: Row/Col optional/
	 * tunable - bypass when the FLIR internal correction is verified sufficient). The ONE
	 * place mapping curt.rowcol_* to Config so all RT ingest call sites stay consistent;
	 * defaults (rowcol_en=true, max_abs=0->INFINITY) reproduce the previous hardcoded
	 * Config bit-exactly. By Claude on 07/07/2026, from Andrey's plan.
	 */
	public static Config configFromCurt(com.elphel.imagej.tileprocessor.CuasRtParameters curt) { // By Claude on 07/07/2026
		final Config cfg = new Config();
		cfg.rowcol_enable         = curt.rowcol_en;
		cfg.rowcol_hpf            = curt.rowcol_hpf;
		cfg.rowcol_hpf_sigma      = curt.rowcol_hpf_sigma;
		cfg.rowcol_um_sigma       = curt.rowcol_um_sigma; // By Claude on 07/07/2026
		cfg.rowcol_max_abs        = (curt.rowcol_max_abs > 0) ? curt.rowcol_max_abs : Double.POSITIVE_INFINITY;
		cfg.rowcol_weight_outlier = curt.rowcol_outlier;
		return cfg;
	}

	/**
	 * Condition all sensors in place.
	 * @param image_data [sensor][0][pixel] mono LWIR, row-major (idx = y*width + x). Modified in place.
	 * @param width      image width in pixels (height = pixels/width).
	 * @param scales     per-sensor global scale  (lwir_scales;  null -> 1.0).
	 * @param offsets    per-sensor global offset (lwir_offsets; null -> 0.0), subtracted before scaling.
	 * @param scales2    per-sensor 2nd-order term (lwir_scales2; null -> 0.0; normally unused).
	 * @param fpn        [sensor][0][pixel] per-pixel additive FPN, subtracted after photometric (null -> none).
	 * @param cfg        configuration (null -> defaults).
	 */
	public static void condition(
			double [][][] image_data,
			int           width,
			double []     scales,
			double []     offsets,
			double []     scales2,
			double [][][] fpn,
			Config        cfg) {
		if (cfg == null) cfg = new Config();
		final int num_sens = image_data.length;
		for (int s = 0; s < num_sens; s++) if (image_data[s] != null && image_data[s][0] != null) {
			final double [] px = image_data[s][0];
			if (cfg.photometric || cfg.apply_fpn) {
				final double B0 = (scales  != null) ? scales [s] : 1.0;
				final double C0 = (offsets != null) ? offsets[s] : 0.0;
				final double A0 = (scales2 != null) ? scales2[s] : 0.0;
				final double [] fp = (cfg.apply_fpn && fpn != null && fpn[s] != null) ? fpn[s][0] : null;
				for (int i = 0; i < px.length; i++) {
					double p = px[i];
					if (cfg.photometric) {
						final double d = p - C0;
						p = A0 * d * d + B0 * d;   // == B0*(raw-C0) when A0==0 (linear, matches current additive path)
					}
					if (fp != null) p -= fp[i];
					px[i] = p;
				}
			}
			// Row/Col LAST (reordered 07/07/2026, oracle semantics "on top of the FPN"): the
			// FPN carries strong STATIC line structure (measured rms ~6/12 counts row/col);
			// measuring row/col on RAW pixels captured it (scaled exactly through photometric)
			// and the later FPN subtraction removed it AGAIN - negative FPN line structure was
			// injected into the output. Post-FPN measurement sees only the per-frame varying
			// line noise, in calibrated counts (the max_abs gate units). By Claude on 07/07/2026.
			if (cfg.rowcol_enable) {
				subtractRowColSensor(px, width, cfg);
			}
		}
		// Zero-DC: one common constant subtracted from ALL sensors (see Config docs).
		// AUTO level = MEDIAN OF THE TOP HALF of the frame (y < height/2), pooled over all
		// sensors: for CUAS the sky background is what matters, and the median is robust to
		// both the (snowy March Latvia vs summer-calibration) ground and to targets. The
		// same level must be subtracted from the BORROWED averaged center image at borrow
		// time (firewall: borrowed data zeroed once at the boundary, never re-compensated).
		// By Claude on 07/05/2026, from Andrey's design.
		if (cfg.zero_dc) {
			double level = cfg.dc_level;
			if (Double.isNaN(level)) { // auto: median of the top (sky) half, all sensors pooled
				final int height = ((image_data[0] != null) && (image_data[0][0] != null)) ?
						image_data[0][0].length / width : 0;
				final int top_pix = (height / 2) * width;
				double [] samples = new double [num_sens * top_pix];
				int n = 0;
				for (int s = 0; s < num_sens; s++) if (image_data[s] != null && image_data[s][0] != null) {
					final double [] px = image_data[s][0];
					for (int i = 0; i < top_pix; i++) if (!Double.isNaN(px[i])) samples[n++] = px[i];
				}
				if (n == 0) return;
				java.util.Arrays.sort(samples, 0, n);
				level = samples[n/2];
			}
			for (int s = 0; s < num_sens; s++) if (image_data[s] != null && image_data[s][0] != null) {
				final double [] px = image_data[s][0];
				for (int i = 0; i < px.length; i++) px[i] -= level;
			}
		}
	}

	/**
	 * Pure data-domain appropriation kernel for the BORROWED center CLT: subtract
	 * level*pattern per TD point, NaN-aware (only where both terms are defined). Knows
	 * NOTHING of QuadCLT/GPU - the caller (CuasRT initialization, the borrow boundary)
	 * extracts the arrays and stores the result back. Returns a NEW array; the input is
	 * not modified. By Claude on 07/05/2026, per Andrey's clean-method ruling.
	 * @param fclt    borrowed center CLT (single merged-sensor slice, tile-major TD points)
	 * @param pattern CLT of a uniform 1.0 image over the same task tiles (NaN elsewhere)
	 * @param level   DC level to subtract (counts)
	 * @return zeroed copy of fclt (fclt - level*pattern where both defined)
	 */
	public static float [] zeroDcClt(
			final float [] fclt,
			final float [] pattern,
			final double   level) {
		final float [] zeroed = fclt.clone();
		final int len = Math.min(zeroed.length, pattern.length);
		int n_off = 0;
		for (int i = 0; i < len; i++) {
			final float p = pattern[i];
			if (!Float.isNaN(p) && !Float.isNaN(zeroed[i])) {
				zeroed[i] -= (float) (level * p);
				n_off++;
			}
		}
		System.out.println("CuasConditioning.zeroDcClt(): CLT offset by "+level+
				" over "+n_off+" TD points");
		return zeroed;
	}

	/**
	 * The A2/RT ingest: read a scene's RAW /jp4/ source, condition it with the CURRENT
	 * calibration (curt_calib-updated lwir scales/offsets/scales2 + per-pixel FPN from the
	 * scene QuadCLT), and feed the GPU DIRECTLY - completely bypassing the QuadCLT
	 * image_data mechanics (per Andrey: this is the permanent RT shape, especially as more
	 * data becomes GPU-memory-resident; QuadCLT stays for geometry/poses only).
	 *
	 * GPU handoff contract (validated by the bayer-guard fix, ebef0b23): bind the scene
	 * FIRST (saveQuadClt - conditional, resets the guard only on instance switch), clear
	 * hasNewImageData, then FORCE-upload - setBayerImages(data,true) sets bayer_set, so the
	 * pipeline's later pull-variant setBayerImages(false) skips its re-pull and the
	 * conditioned pixels survive through convert_direct. TELL if this ever breaks: the
	 * downstream results become identical to the prepared-data path (silent re-pull).
	 *
	 * By Claude on 07/05/2026, from Andrey's design.
	 * @param scene      physical scene: /jp4/ source, calibration globals, FPN, geometry
	 * @param cfg        conditioning configuration (null - defaults: rowcol+photometric+FPN)
	 * @param threadsMax max read threads
	 * @param debugLevel debug verbosity
	 * @return true on success; false if raw source missing (GPU state untouched)
	 */
	public static boolean conditionSceneToGpu(
			final QuadCLT scene,
			final Config  cfg,
			final int     threadsMax,
			final int     debugLevel) {
		return conditionSceneToGpuCuas(scene, cfg, null, null, threadsMax, debugLevel);
	}

	/**
	 * CUAS wrapper: the ONE place where per-scene MOTION enters the RT ingest (Andrey's
	 * design 2026-07-05). Sets the scene's ERS rates (when given) before the ingest so the
	 * downstream grid transform (transformToScenePxPyD -> ErsCorrection, the fully-preserved
	 * RGB-proven path) applies the rolling-shutter correction; omegas==null &&
	 * velocities==null leaves the scene's current ERS state untouched (legacy behavior).
	 * For CUAS: omegas from the wobble radius + rotation RPM (analytic, per scene phase);
	 * velocities - PROVISION only, pass {0,0,0} (or null) for the rotation-only camera.
	 * FUTURE (GPU migration): the same omegas/velocities feed the GPU corr_vector
	 * imu_rot/imu_move (get_tiles_offsets has per-sensor ERS support, currently dormant) -
	 * to be enabled after verifying no double-application against the CPU pXpYD ERS.
	 * By Claude on 07/05/2026, from Andrey's design.
	 * @param omegas     angular rates {az, tilt, roll} rad/s, scene-vs-virtual-center
	 *                   relative (wobble-scale; the constant-rotation part cancels against
	 *                   the co-rotating center) - or null (with velocities null: keep state)
	 * @param velocities linear rates {vx, vy, vz} m/s (provision; null/{0,0,0} for CUAS)
	 */
	public static boolean conditionSceneToGpuCuas(
			final QuadCLT   scene,
			final Config    cfg,
			final double [] omegas,
			final double [] velocities,
			final int       threadsMax,
			final int       debugLevel) {
		if ((omegas != null) || (velocities != null)) {
			scene.getErsCorrection().setErsDt(
					(velocities != null) ? velocities : new double [3],
					(omegas != null)     ? omegas     : new double [3]);
		}
		return conditionSceneToGpuInner(scene, cfg, threadsMax, debugLevel);
	}

	private static boolean conditionSceneToGpuInner(
			final QuadCLT scene,
			final Config  cfg,
			final int     threadsMax,
			final int     debugLevel) {
		final double [][][] data = CuasMotion.readRawImageData(scene, threadsMax, debugLevel);
		if (data == null) {
			return false;
		}
		final int width = scene.getGeometryCorrection().getSensorWH()[0];
		condition(
				data,                    // double [][][] image_data (in place)
				width,                   // int           width
				scene.getLwirScales(),   // double []     scales
				scene.getLwirOffsets(),  // double []     offsets
				scene.getLwirScales2(),  // double []     scales2
				scene.getFPN(),          // double [][][] fpn (null - none)
				cfg);                    // Config        (null - defaults)
		final GpuQuad gpuQuad = scene.getGPUQuad();
		scene.saveQuadClt();                 // bind this scene (conditional; resets bayer guard on switch)
		scene.setHasNewImageData(false);     // the guard's other re-pull trigger - must be clear
		gpuQuad.setBayerImages(data, true);  // force H2D; sets bayer_set -> pipeline skips its pull
		return true;
	}

	/** Row/Col denoise for one sensor's flat pixel array (in place): subtract per-row, then per-col.
	 *  Profiles are MEASURED on an unsharp-masked copy when rowcol_um_sigma > 0 (content
	 *  suppression borrowed from the oracle CorrectionFPN path - the max_abs gate then works
	 *  on residual counts instead of full-scale values), and SUBTRACTED from the original.
	 *  By Claude on 07/07/2026 (was: measured on the data as-is). */
	public static void subtractRowColSensor(final double [] px, final int width, final Config cfg) {
		final int height = px.length / width;
		double [] meas = px; // measurement copy (UM'd); profiles subtract from px
		if (cfg.rowcol_um_sigma > 0) {
			meas = px.clone();
			final double [] blurred = px.clone();
			new DoubleGaussianBlur().blurDouble(blurred, width, height,
					cfg.rowcol_um_sigma, cfg.rowcol_um_sigma, 0.01);
			for (int i = 0; i < meas.length; i++) meas[i] -= blurred[i];
		}
		// Profile HPF only WITHOUT the UM (measured, v013-ROWCOL-03 vs -02): the UM already
		// high-passes the measured profile by (1-G_um); stacking the profile HPF multiplies
		// in (1-G_hpf) - at period ~20 rows the removed fraction dropped 96% -> 91%, i.e.
		// residual mid-band line noise x2 (rows metric 10.6 -> 18.7). A single high-pass at
		// the UM sigma removes strictly more noise at every period AND protects the LoG's
		// low frequencies at least as well as the sigma-8 profile HPF did. By Claude on 07/07/2026.
		final boolean prof_hpf = cfg.rowcol_hpf && (cfg.rowcol_um_sigma <= 0);
		// --- per-row profile (average along x for each row y) ---
		double [] rowProf = lineAverage(meas, width, height, true, cfg.rowcol_max_abs, cfg.rowcol_weight_outlier,
				cfg.rowcol_min_inlier);
		if (prof_hpf) rowProf = highpass(rowProf, cfg.rowcol_hpf_sigma);
		for (int y = 0; y < height; y++) {
			final double sub = rowProf[y];
			if (sub != 0.0) {
				final int row0 = y * width;
				for (int x = 0; x < width; x++) px[row0 + x] -= sub;
				if (meas != px) for (int x = 0; x < width; x++) meas[row0 + x] -= sub; // keep the measurement copy row-corrected
			}
		}
		// --- per-col profile (average along y for each col x), recomputed on row-corrected data ---
		double [] colProf = lineAverage(meas, width, height, false, cfg.rowcol_max_abs, cfg.rowcol_weight_outlier,
				cfg.rowcol_min_inlier);
		if (prof_hpf) colProf = highpass(colProf, cfg.rowcol_hpf_sigma);
		for (int y = 0; y < height; y++) {
			final int row0 = y * width;
			for (int x = 0; x < width; x++) px[row0 + x] -= colProf[x];
		}
	}

	/**
	 * Outlier-weighted average producing a 1-D profile, with the untrusted-line guard
	 * (By Claude on 07/07/2026, ROWCOL-04 finding): a CONTENT-DOMINATED line (inlier
	 * fraction below min_inlier - e.g. terrain rows where nearly every |UM residual|
	 * exceeds max_abs) makes the outlier-weighted mean degenerate to the content mean
	 * (the small weights cancel) and stamps bias bands into the image; such lines get 0
	 * (no correction). Also 0 when |estimate| > max_abs (line noise cannot physically
	 * exceed the gate scale). Guards inactive when max_abs is INFINITY (no gate - every
	 * pixel is an inlier).
	 * @param perRow true  -> length-height profile, averaging along x (per-row offset);
	 *               false -> length-width  profile, averaging along y (per-col offset).
	 */
	private static double [] lineAverage(
			final double [] px, final int width, final int height,
			final boolean perRow, final double max_abs, final double weight_outlier,
			final double min_inlier) {
		final int n = perRow ? height : width;
		final double [] sum = new double [n];
		final double [] wgt = new double [n];
		final int    [] num = new int    [n]; // finite samples
		final int    [] inl = new int    [n]; // inliers (|d| <= max_abs)
		int idx = 0;
		for (int y = 0; y < height; y++) {
			for (int x = 0; x < width; x++) {
				final double d = px[idx++];
				if (Double.isNaN(d)) continue;
				final int k = perRow ? y : x;
				final boolean inlier = (Math.abs(d) <= max_abs);
				final double w = inlier ? 1.0 : weight_outlier;
				sum[k] += w * d;
				wgt[k] += w;
				num[k]++;
				if (inlier) inl[k]++;
			}
		}
		for (int k = 0; k < n; k++) {
			double v = (wgt[k] > 0.0) ? (sum[k] / wgt[k]) : 0.0;
			if ((num[k] > 0) && (inl[k] < min_inlier * num[k])) v = 0.0; // content-dominated line
			else if (Math.abs(v) > max_abs)                     v = 0.0; // physical cap (no-op when max_abs is INF)
			sum[k] = v;
		}
		return sum;
	}

	/** 1-D Gaussian high-pass of a profile: x - smooth(x, sigma), reflect-padded. Returns a new array. */
	public static double [] highpass(final double [] x, final double sigma) {
		if (sigma <= 0.0) return x.clone();
		final int r = Math.max(1, (int) Math.ceil(3.0 * sigma));
		final double [] k = new double [2 * r + 1];
		double ksum = 0.0;
		for (int i = -r; i <= r; i++) { final double v = Math.exp(-0.5 * (i * i) / (sigma * sigma)); k[i + r] = v; ksum += v; }
		for (int i = 0; i < k.length; i++) k[i] /= ksum;
		final int n = x.length;
		final double [] hp = new double [n];
		for (int i = 0; i < n; i++) {
			double sm = 0.0;
			for (int j = -r; j <= r; j++) {
				int ii = i + j;
				if (ii < 0)      ii = -ii - 1;          // reflect
				else if (ii >= n) ii = 2 * n - ii - 1;  // reflect
				if (ii < 0) ii = 0; else if (ii >= n) ii = n - 1;
				sm += k[j + r] * x[ii];
			}
			hp[i] = x[i] - sm;
		}
		return hp;
	}

	/** Build the per-sensor per-pixel OFFSET map (post-scale, subtractive) = scale*offset + FPN,
	 *  so that (scale*raw - offset_map) == scale*(raw-offset) - FPN. For the future 2-map API /
	 *  visualization; not required by {@link #condition}. */
	public static double [][] buildOffsetMaps(
			final double [][][] fpn, final double [] scales, final double [] offsets, final int width) {
		final int num_sens = fpn.length;
		final double [][] out = new double [num_sens][];
		for (int s = 0; s < num_sens; s++) if (fpn[s] != null && fpn[s][0] != null) {
			final double B0 = (scales  != null) ? scales [s] : 1.0;
			final double C0 = (offsets != null) ? offsets[s] : 0.0;
			final double [] fp = fpn[s][0];
			final double [] m = new double [fp.length];
			final double base = B0 * C0;
			for (int i = 0; i < fp.length; i++) m[i] = base + fp[i];
			out[s] = m;
		}
		return out;
	}
}
