/**
 **
 ** OrthoMultiLMA - Fit multiple scenes orthographic using pair-wise affine
 **                tr4ansform matrices already calculated.
 **
 ** Copyright (C) 2024 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  OrthoMultiLMA.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.awt.Point;
import java.util.ArrayList;
import java.util.Arrays;

import com.elphel.imagej.cameras.CLTParameters;

import Jama.Matrix;

public class OrthoMultiLMA {
	private double []         last_rms =        null; // {rms, rms_pure}, matching this.vector
	private double []         good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
	private double []         initial_rms =     null; // {rms, rms_pure}, first-calcualted rms
	private double []         parameters_vector = null;
//	private double []         x_vector =        null; // not used. Save total weight
	private double []         y_vector =        null;
	private double [][]       tile_centers =    null;
	private double            weight = 0; // total weight
	private double            pure_weight; // weight of samples only as a fraction of total weight
	private double []         weights; // normalized so sum is 1.0 for all - samples and extra regularization
//	private double            pure_weight; // weight of samples only	
	private double []         last_ymfx =       null;
	private double [][]       last_jt =         null;
	private PairwiseOrthoMatch [][] matches =   null;
	private ArrayList<Point>  pairs =           null;
	private int []            indices =         null;
	private int               num_scenes =      0;
	private int               num_pairs =       0;
	public static boolean testMultiLMA(
			CLTParameters       clt_parameters,
			OrthoMapsCollection maps_collection,
			int []              indices) {
		int scene0 = indices[0], scene1 = indices[1];
		PairwiseOrthoMatch direct = maps_collection.ortho_maps[scene0].getMatch(maps_collection.ortho_maps[scene1].getName());
		double [] enuOffset = maps_collection.ortho_maps[scene1].enuOffsetTo(maps_collection.ortho_maps[scene0]);
		double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
		PairwiseOrthoMatch inv_match = direct.getInverse(rd);
		double [][] affine0 = new double [2][3]; // inv_match.affine()
		double [][] affine1 = new double [2][3]; // inv_match.affine()
		affine0[0][0] = 0.5 * inv_match.getAffine()[0][0];
		double [][] E = {{1,0,0},{0,1,0}};
		for (int i = 0; i < 2; i++) {
			for (int j = 0; j < 3; j++) {
				affine0[i][j] = 0.5 * (inv_match.getAffine()[i][j]+E[i][j]);
				affine1[i][j] = 0.5 * (direct.getAffine()[i][j]+E[i][j]);
			}
		}
		double [][][] derivs =       new double [12][][];
		double [][][] derivs_delta = new double [12][][];
		double delta = 1E-7;
		getAffineAndDerivetives_delta(
				delta,   // double        delta,
				affine0, // double [][]   affine00,
				affine1, // double [][]   affine10,
				rd,      // double []     rd,
				derivs_delta); // double [][][] derivs)
		double [][] affine = 		getAffineAndDerivetives(
				affine0, // double [][]   affine00,
				affine1, // double [][]   affine10,
				rd,      // double []     rd,
				derivs); // double [][][] derivs)
		double [][][] err = new double [12][2][3];
		for (int n = 0; n < err.length; n++) if ((derivs[n] != null) && (derivs_delta[n] != null)) {
			for (int i = 0; i < err[n].length; i++) {
				for (int j = 0; j < err[n][i].length; j++) {
					err[n][i][j] = derivs[n][i][j] - derivs_delta[n][i][j];
				}
			}			
		}
		System.out.println();
		return true;
	}
	public int prepareLMA (
			CLTParameters       clt_parameters,
			OrthoMapsCollection maps_collection,
			int []              indices,
			double []           val_coord, // 1 - valid, 0 - invalid, minimize coordinates errors
			double              position_pull,
            boolean             use_inv,
            double              overlap_pow, // match weight as overlap fraction to this power
			int                 debugLevel) {
		this.indices = indices;
		num_scenes = indices.length;
		matches = new PairwiseOrthoMatch[indices.length][indices.length];
		pairs = new ArrayList<Point>();
		for (int i = 0; i < num_scenes-1; i++) {
			int scene0 = indices[i]; 
			for (int j = i+1; j < num_scenes; j++){
				int scene1 = indices[j];
				PairwiseOrthoMatch match = maps_collection.ortho_maps[scene0].getMatch(maps_collection.ortho_maps[scene1].getName());
				PairwiseOrthoMatch inv_match = use_inv?
						maps_collection.ortho_maps[scene1].getMatch(maps_collection.ortho_maps[scene0].getName()):null;
				if ((match != null) || (inv_match != null)){
					if (match == null) {
						double [] enuOffset = maps_collection.ortho_maps[scene0].enuOffsetTo(maps_collection.ortho_maps[scene1]);
						double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
						match = inv_match.getInverse(rd);
					}
					if (inv_match == null) {
						double [] enuOffset = maps_collection.ortho_maps[scene1].enuOffsetTo(maps_collection.ortho_maps[scene0]);
						double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
						inv_match = match.getInverse(rd);
					}
				}
				matches[i][j] = match;
				matches[j][i] = inv_match;
				pairs.add(new Point(i,j)); // only once?
			}
		}
		num_pairs = pairs.size();
		if (val_coord == null) { // maybe not needed - use weights
			val_coord = new double [indices.length];
			Arrays.fill(val_coord, 1.0);
		}
		
		y_vector =          new double [6*num_pairs + 2 * num_scenes]; // last 2 * num_scenes will stay 0
		weights =           new double [6*num_pairs + 2 * num_scenes];
		parameters_vector = new double [6*num_scenes]; // maybe will need move-only mode?
		for (int n = 0; n < num_scenes; n++) {
			double [][] affine = maps_collection.ortho_maps[indices[n]].getAffine();
			System.arraycopy(affine[0], 0, parameters_vector, 6*n, 3);
			System.arraycopy(affine[1], 0, parameters_vector, 6*n + 3, 3);
			y_vector[6*num_pairs+2*n + 0] = affine[0][2]; 
			y_vector[6*num_pairs+2*n + 1] = affine[1][2]; 
		}
		for (int np = 0; np < num_pairs; np++) {
			Point pair_ind = pairs.get(np);
			PairwiseOrthoMatch match = matches[pair_ind.x][pair_ind.y];
			double [][] affine_pair = match.getAffine();
			System.arraycopy(affine_pair[0], 0, y_vector, 6*np, 3);
			System.arraycopy(affine_pair[1], 0, y_vector, 6*np + 3, 3);
			double overlap = match.overlap;
			double [][] jtj = match.jtj;
			double [] w = new double [6];
			w[0] = jtj[0][0];w[1]=jtj[1][1]; w[2] = jtj[4][4]; // a00, a01, b0
			w[3] = jtj[2][2];w[4]=jtj[3][3]; w[5] = jtj[5][5]; // a10, a11, b1
			double w_overlap = Math.pow(overlap, overlap_pow);
			for (int i = 0; i < w.length; i++) {
				w[i] = Math.sqrt(w[i]) * w_overlap;
			}
			System.arraycopy(w, 0, weights, 6*np, 6);
		}
		int wi = 6*num_pairs;
		for (int n = 0; n < num_scenes; n++) {
			double w = val_coord[n]*position_pull;
			weights[wi++] = w;
			weights[wi++] = w;
		}		
		
		return weights.length;
	}

	public static double [][] getAffineAndDerivetives_delta(
			double        delta,
			double [][]   affine00,
			double [][]   affine10,
			double []     rd,
			double [][][] derivs) {
		double [][] affine = getAffineAndDerivetives(
				affine00, // double [][]   affine0,
				affine10, // double [][]   affine1,
				rd,      // double []     rd,
				null); // double [][][] derivs)
		double [][][] affines0 = {affine00, affine10};
		for (int naff = 0; naff < affines0.length; naff++) {
			for (int row = 0; row < affines0[naff].length; row++) {
				for (int col = 0; col <  affines0[naff][row].length; col++) {
					double [][][] affines = {
							{affines0[0][0].clone(), affines0[0][1].clone()},
							{affines0[1][0].clone(), affines0[1][1].clone()}};
					affines[naff][row][col]= affines0[naff][row][col] - 0.5*delta;
					double [][] affine_minus = getAffineAndDerivetives(
							affines[0], // double [][]   affine0,
							affines[1], // double [][]   affine1,
							rd,      // double []     rd,
							null); // double [][][] derivs)
					affines[naff][row][col]= affines0[naff][row][col] + 0.5*delta;
					double [][] affine_plus = getAffineAndDerivetives(
							affines[0], // double [][]   affine0,
							affines[1], // double [][]   affine1,
							rd,      // double []     rd,
							null); // double [][][] derivs)
					double [][] daffine = new double [2][3];
					for (int i = 0; i < daffine.length; i++) {
						for (int j = 0; j < daffine[i].length; j++) {
							daffine[i][j] = (affine_plus[i][j] - affine_minus[i][j])/delta; 
						}
					}
					derivs[naff*6+row*3+col] = daffine;
				}
			}
		}
		return affine;
	}
	
	public static double [][] getAffineAndDerivetives(
			double [][]   affine0,
			double [][]   affine1,
			double []     rd,
			double [][][] derivs) { // null or double[12] - will return affine derivatives by each of affine0, affine1
		Matrix A0 = new Matrix (
				new double [][] {{affine0[0][0],affine0[0][1]},{affine0[1][0],affine0[1][1]}});
		Matrix B0 = new Matrix(new double [][] {{affine0[0][2]},{affine0[1][2]}});
		Matrix A1 = new Matrix (
				new double [][] {{affine1[0][0],affine1[0][1]},{affine1[1][0],affine1[1][1]}});
		Matrix B1 = new Matrix(new double [][] {{affine1[0][2]},{affine1[1][2]}});
		Matrix V =  new Matrix(new double [][] {{-rd[0]},{-rd[1]}});
		Matrix A =  A1.times(A0.inverse());
		Matrix B = A.times(V.minus(B0)).minus(A1.times(V)).plus(B1);
		double [][] affine = new double[][] {
			{A.get(0,0),A.get(0,1), B.get(0,0)},
			{A.get(1,0),A.get(1,1), B.get(1,0)}};
		if (derivs != null) {
			double [][] a = A.getArray();
			double [][] a0 = A0.getArray();
			double []   b0 = B0.getColumnPackedCopy();
			double [][] a1 = A1.getArray();
			double idet = 1/(a0[0][0]*a0[1][1]-a0[1][0]*a0[0][1]);
			double [] v = V.getColumnPackedCopy();
			double [] vb0= {v[0]-b0[0], v[1]-b0[1]};
			double d2 = -idet; // *idet; One idet is already included in a[][]
			double [][] didet_da = {{d2*a0[1][1], -d2*a0[1][0]}, {-d2*a0[0][1], d2*a0[0][0]}};
			/*
			 1/det0 * | a100, a101 | * | a011, -a001 |
			          | a110, a111 |   |-a010,  a000 |
			 * 
			 1/det0 * |a100*a011 - a101*a010, -a100*a001 + a101*a000 |
			          |a110*a011 - a111*a010, -a110*a001 + a111*a000 |
			 */
			double [][] da_da000 = {{0,  idet*a1[0][1]}, {0, idet*a1[1][1]}};
			double [][] da_da001 = {{0, -idet*a1[0][0]}, {0,-idet*a1[1][0]}};
			double [][] da_da010 = {{-idet*a1[0][1], 0}, {-idet*a1[1][1],0}};
			double [][] da_da011 = {{ idet*a1[0][0], 0}, { idet*a1[1][0],0}};
			double [][][][] da_da0 = {{da_da000, da_da001}, {da_da010, da_da011}};
			for (int i = 0; i < da_da0.length; i++) {
				for (int j = 0; j < da_da0[i].length; j++) {
					for (int k = 0; k < da_da0[i][j].length; k++) {
						for (int l = 0; l < da_da0[i][j][k].length; l++) {
							da_da0[i][j][k][l] += a[k][l]*didet_da[i][j]; // a[][] already has idet
						}
					}
				}
			}
			// some symmetry below - use to reduce calcs?
			double [][] da_da100 = {{ idet*a0[1][1],-idet*a0[0][1]},{0,0}};
			double [][] da_da101 = {{-idet*a0[1][0], idet*a0[0][0]},{0,0}};
			double [][] da_da110 = {{ 0,        0       },{ idet*a0[1][1],-idet*a0[0][1]}};   
			double [][] da_da111 = {{ 0,        0       },{-idet*a0[1][0], idet*a0[0][0]}}; 
			
			double [][][][] da_da1 = {{da_da100, da_da101}, {da_da110, da_da111}};
			double [][][][][] da_da = {da_da0, da_da1};
			double [] db_da000 = {
					da_da000[0][0]*vb0[0]+da_da000[0][1]*vb0[1],
					da_da000[1][0]*vb0[0]+da_da000[1][1]*vb0[1]};
			double [] db_da001 = {
					da_da001[0][0]*vb0[0]+da_da001[0][1]*vb0[1],
					da_da001[1][0]*vb0[0]+da_da001[1][1]*vb0[1]};
			double [] db_da010 = {
					da_da010[0][0]*vb0[0]+da_da010[0][1]*vb0[1],
					da_da010[1][0]*vb0[0]+da_da010[1][1]*vb0[1]};
			double [] db_da011 = {
					da_da011[0][0]*vb0[0]+da_da011[0][1]*vb0[1],
					da_da011[1][0]*vb0[0]+da_da011[1][1]*vb0[1]};
			double [][][] db_da0 = {{db_da000,db_da001},{db_da010,db_da011}};
			double [] db_da100 = {
					da_da100[0][0]*vb0[0]+da_da100[0][1]*vb0[1] - v[0],
					da_da100[1][0]*vb0[0]+da_da100[1][1]*vb0[1]};
			double [] db_da101 = {
					da_da101[0][0]*vb0[0]+da_da101[0][1]*vb0[1] - v[1],
					da_da101[1][0]*vb0[0]+da_da101[1][1]*vb0[1]};
			double [] db_da110 = {
					da_da110[0][0]*vb0[0]+da_da110[0][1]*vb0[1],
					da_da110[1][0]*vb0[0]+da_da110[1][1]*vb0[1] - v[0]};
			double [] db_da111 = {
					da_da111[0][0]*vb0[0]+da_da111[0][1]*vb0[1],
					da_da111[1][0]*vb0[0]+da_da111[1][1]*vb0[1] - v[1]};
			double [][][] db_da1 = {{db_da100,db_da101},{db_da110,db_da111}};
			double [][][][] db_da = {db_da0,db_da1};
			
			double [] db_db00 = {-a[0][0],-a[1][0]};
			double [] db_db01 = {-a[0][1],-a[1][1]};
			double [] db_db10 = {1,0};
			double [] db_db11 = {0,1};
			double [][][] db_db = {{db_db00,db_db01},{db_db10,db_db11}};
			// fill in  derivatives, skipping B - only 2x2 A
			for (int naff = 0; naff < da_da.length; naff++) {
				for (int row = 0; row < da_da[naff].length; row++) {
					for (int col = 0; col <  3; col++) {
						int indx = naff*6+row*3+col;
						derivs[indx] = new double[2][3];
						if ((col <  da_da[naff][row].length) ) {
							for (int i = 0; i < 2; i++) {
								for (int j = 0; j< 2; j++) {
									derivs[indx][i][j] = da_da[naff][row][col][i][j];
								}
								derivs[indx][i][2] = db_da[naff][row][col][i];
							}
						} else { // d/db
							for (int i = 0; i < 2; i++) {
								derivs[indx][i][2] = db_db[naff][row][i];
							}							
						}
					}
				}
			}
		}
		return affine; // _alt;
	}


}
