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

import java.util.Arrays;

import com.elphel.imagej.tileprocessor.TileNeibs;

import Jama.EigenvalueDecomposition;
import Jama.Matrix;

public class CorrelationPeakStats {
	public double best_d;
	public double eff_rad;
	public double elong;
	public double dist;
	public double [] cent_offs;
	public double max_other;
	public CorrelationPeakStats(
			double [] data, // square data
			double [] cent_xy, // if null, use center of the square
			double    radius, // search for maximum within this radius
			double    frac_max,
			double    other_radius,
			int       debugLevel) {
		double [] stats = getStatsNaN(
				data,        // square data
				cent_xy,     // if null, use center of the square
				radius,      // search for maximum within this radius
				frac_max,
				other_radius,
				debugLevel);
		best_d =  stats[0];
		eff_rad = stats[1];
		elong =   stats[2];
		dist =    stats[3];
		cent_offs = new double [] {stats[4], stats[5]};
		max_other = stats[6];
	}
	
	int [] getIntOffset() {
		return new int[] {(int) Math.round(cent_offs[0]), (int) Math.round(cent_offs[1])};
	}
	/**
	 * Search for maximum within specified radius from the center or specified offset from the center
	 * @param data square data
	 * @param cent_xy offset from the square center to search
	 * @param radius search for maximum within this radius
	 * @param frac_max fraction of maximum to measure area
	 * @param other_radius measure maximal disconnected pixel fraction of the maximum within this radius
	 * @return {maximum, area, dist, x, y}. No interpolation yet, each returned value is integer. 
	 * Returns null if no local maximum within area. x,y are offsets from the (provided) center 
	 */
	public static double [] getStatsNaN(
			double [] data, // square data
			double [] cent_xy, // if null, use center of the square
			double    radius, // search for maximum within this radius
			double    frac_max,
			double    other_radius,
			int       debugLevel) {
		double [] rslt = getStats(
				data, // square data
				cent_xy, // if null, use center of the square
				radius, // search for maximum within this radius
				frac_max,
				other_radius,
				debugLevel);
		if (rslt != null) {
			return rslt;
		} else {
			return new double[] {Double.NaN,Double.NaN,Double.NaN,Double.NaN,Double.NaN,Double.NaN,Double.NaN};
		}
	}

	public static double [] getStats(
			double [] data, // square data
			double [] cent_xy, // if null, use center of the square
			double    radius, // search for maximum within this radius
			double    frac_max,
			double    other_radius, // may be 0 if not needed
			int       debugLevel) {
		boolean   debug = debugLevel > 1;
		int size = (int) Math.sqrt(data.length);
		if (cent_xy == null) {
			cent_xy = new double [] {size/2, size/2};
		}
		double maxr2 = radius * radius;
		double best_d = 0;
		int best_indx = -1;
		int min_x = Math.max(1, (int) Math.floor(cent_xy[0]-radius));
		int min_y = Math.max(1, (int) Math.floor(cent_xy[1]-radius));
		int max_x = Math.min(size-2, (int) Math.ceil(cent_xy[0]+radius));
		int max_y = Math.min(size-2, (int) Math.ceil(cent_xy[1]+radius));
		for (int y = min_y; y <= max_y; y++) { // do not search on very edges
			double dy = (y-cent_xy[1]);
			double y2 = dy*dy;
			if (y2 < maxr2) {
				for (int x = min_x; x <= max_x; x++) {
					double dx = x - cent_xy[0];
					double r2 = y2 + dx*dx;
					if (r2 < maxr2) {
						int indx = y * size + x;
						double d = data[indx];
						if (d > best_d) {
							best_indx = indx;
							best_d = d;
						}
					}
				}
			}
		}
		if (best_indx < 0) {
			return null;
		}
		// is it local max?
		if     ((data[best_indx - 1] >    best_d) || (data[best_indx + 1] >    best_d) ||
				(data[best_indx - size] > best_d) || (data[best_indx + size] > best_d)) {
			return null; // on the edge, not a local max
		}
		
		boolean [] above_thresh = new boolean [data.length];
		double thresh = best_d * frac_max;
		for (int i = 0; i < data.length; i++) {
			above_thresh[i] = data[i] > thresh;
		}
		
		int [] clusters = (new TileNeibs(size,size)).enumerateClusters(
				above_thresh, // boolean [] tiles,
				null,   // int []     num_clusters,
				false); // boolean ordered)
		int center_cluster = clusters[best_indx];

		int best_x = best_indx % size;
		int best_y = best_indx / size;
		double xc = best_x - cent_xy[0];
		double yc = best_y - cent_xy[1];
		

		double s0=0, sx=0, sy=0, sx2 = 0, sy2=0, sxy = 0;
		for (int i = 0; i < clusters.length; i++) {
			if (clusters[i] == center_cluster) {
				double y = i / size - (yc + cent_xy[1]); //(yc + cent_xy[1]) - absolute, from (0,0)
				double x = i % size - (xc + cent_xy[0]);
				double w = data[i]-thresh;
				s0 += w;
				sx += w * x;
				sy += w * y;
				sx2 += w * x * x;
				sy2 += w * y * y;
				sxy += w * x * y;
			}
		}
		double cxx = sx2 - sx * sx / s0, cyy= sy2 - sy * sy / s0, cxy = sxy - sx * sy / s0; 
		/*
		 * sum(Mi*(Xi-avg(X))^2) =          SX2 - SX^2/S0		
		 * sum(Mi*(Yi-avg(Y))^2) =          SY2 - SY^2/S0
		 * sum(Mi*(Xi-avg(X)*(Yi-avg(Y))) =	SXY - SX*SY / S0	
		 */
		Matrix covar = new Matrix(new double[][] {{cxx, cxy},{cxy,cyy}});
		double [] cent_offs = {sx/s0 + xc,sy/s0 + yc};
		EigenvalueDecomposition eig = covar.eig(); 
		
		double [] eigval = {eig.getD().get(0, 0),eig.getD().get(1, 1)};
		Arrays.sort(eigval); // ascending
		double elong = Math.sqrt(eigval[1]/eigval[0]);
		double eff_rad = Math.sqrt(Math.sqrt(eigval[1]*eigval[0]));
		double dist = Math.sqrt(cent_offs[0]*cent_offs[0] + cent_offs[1]*cent_offs[1]);
		double max_other = 0.0;
		if (other_radius > 0) { // only calculate if asked for
			double or2 = other_radius*other_radius;
			min_x = Math.max(1, (int) Math.floor(best_x-other_radius));
			min_y = Math.max(1, (int) Math.floor(best_y-other_radius));
			max_x = Math.min(size-2, (int) Math.ceil(best_x+other_radius));
			max_y = Math.min(size-2, (int) Math.ceil(best_y+other_radius));
			for (int y = min_y; y <= max_y; y++) { // do not search on very edges
				double dy = (y-best_y);
				double y2 = dy*dy;
				if (y2 < or2) {
					for (int x = min_x; x <= max_x; x++) {
						double dx = x - best_x;
						double r2 = y2 + dx*dx;
						if (r2 < or2) {
							int indx = y * size + x;
							if (clusters[indx] != center_cluster) { // only disconnected from the central area
								double d = data[indx];
								if (d > max_other) {
									max_other = d;
								}
							}
						}
					}
				}
			}
			max_other /= best_d;
		}
		
		if (debug) {
			System.out.println("\ncenter offset ["+(sx/s0)+","+(sy/s0)+"] , from center: ["+cent_offs[0]+","+cent_offs[1]+"]");
			System.out.println("Covariance matrix:");
			covar.print(8, 6);
			System.out.println("eig.getV()");
			eig.getV().print(8, 6);
			System.out.println("eig.getD()");
			eig.getD().print(8, 6);
			System.out.println(String.format("best_d=%7.5f, rad=%6.3f, elong=%6.3f, dist=%6.3f, dx=%6.3f, dy=%6.3f, mo=%5.3f",
					best_d, eff_rad, elong, dist, cent_offs[0], cent_offs[1], max_other));
		}
		
		return new double [] {best_d, eff_rad, elong, dist, cent_offs[0], cent_offs[1], max_other} ;
	}
	
}
