/**
 ** PairwiseOrthoMatch - Represent pairwise match between scenes 
 **
 ** Copyright (C) 2024 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  PairwiseOrthoMatch.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.io.IOException;
import java.io.ObjectInputStream;
import java.io.ObjectOutputStream;
import java.io.Serializable;

import Jama.Matrix;

public class PairwiseOrthoMatch implements Serializable {
	private static final long serialVersionUID = 1L;
//	public static boolean  READ_NO_ALT = false; // to read old format w/o alt data ( not final!)
	private          double [][] affine = new double[2][3];
	public           int         zoom_lev;
	public           double      rms = Double.NaN;
	public           double      overlap = 0.0;
	public           double []   alt_data = null;
	public           double []   equalize1to0 = {1,0}; // value1 = equalize2to1[0]*value2+equalize2to1[1]
	public           double []   quat = null; // relative orientation of the second scene (ERS affine removed). Will be eventually saved 
	public           double [][] qaffine = null; // affine relative to rotated scenes 
	// real transient
	public transient double [][] jtj =    new double [6][6];
	// below - not saved/restored
	public transient boolean     ok = false; // not saved/restored
	public transient int []      nxy = null; // not saved, just to communicate for logging
	
	private void writeObject(ObjectOutputStream oos) throws IOException {
		oos.defaultWriteObject();
		if (jtj == null) {
			oos.writeObject(Double.NaN);
		} else {
			for (int i = 0; i < jtj.length; i++) {
				for (int j = i; j < jtj[i].length; j++) {
					oos.writeObject(jtj[i][j]);
				}
			}
		}
//		oos.writeObject(overlap);
//		oos.writeObject(equalize1to0);
//		oos.writeObject(alt_data);
//		oos.writeObject(quat);
//		oos.writeObject(qaffine);
		
	}
	private void readObject(ObjectInputStream ois) throws ClassNotFoundException, IOException {
		ois.defaultReadObject();
		jtj = new double [6][6]; // readObject does not use constructor!
		readjtj:
			for (int i = 0; i < jtj.length; i++) {
				for (int j = i; j < jtj[i].length; j++) {
					jtj[i][j] = (Double) ois.readObject();
					if (Double.isNaN(jtj[0][0]) && (i==0) && (j==0)) {
						jtj=null;
						break readjtj;
					}
					if (j > i) {
						jtj[j][i] = jtj[i][j];
					}
				}
			}
//		overlap = (Double) ois.readObject();
//		equalize1to0 = new double[] {1,0};
//		equalize1to0 = (double[]) ois.readObject();
//		if (!READ_NO_ALT) {
//		alt_data = (double[]) ois.readObject();
//		}
//		if (OrthoMapsCollection.CURRENT_VERSION >= OrthoMapsCollection.VERSION_POST_ORIENT) {
//		quat= (double[]) ois.readObject();
//		qaffine = (double[][]) ois.readObject();
//		}		
		
	}
	
	
	
	public double [] getQuaternion() {
		return quat;
	}
	public void setQuaternion(double [] quat) { // clone() by caller
		this.quat = quat;
	}
	
	public double [][] getQAffine() {
		return qaffine;
	}
	public void setQAffine(double [][] qaffine) {
		this.qaffine = qaffine;
	}
	
//	public PairwiseOrthoMatch() {}
	public double getOverlap() {
		return overlap;
	}
	public void setOverlap(double overlap) {
		this.overlap =overlap;
	}
	
	public double [] getAltData() {
		return alt_data;
	}
	
	public void setAltData(double [] data) {
		alt_data = data;
	}
	
	public PairwiseOrthoMatch(
			double [][] affine,
			double [][] jtj,
			double rms,
			int zoom_lev,
			double overlap) {
		this.affine =  affine;
		this.jtj =     jtj;
		this.zoom_lev= zoom_lev;
		this.rms =     rms;
		this.overlap = overlap;
	}
	public double getRMS() {
		return this.rms;
	}
	
	public boolean isDefined() {
		return affine != null;
	}
	
	public boolean isAffineNonTrivial() {
		return OrthoMap.isAffineNonTrivial(this.affine);
	}

	public double [] getEqualize2to1() {
		return equalize1to0;
	}
	public boolean isSetEqualize2to1() {
		if (equalize1to0 == null) {
			return false;
		}
		return (equalize1to0[0] != 1.0) || (equalize1to0[1] != 0.0);
	}
	
	
	public void setEqualize2to1 (double [] equalize2to1) {
		this.equalize1to0 = equalize2to1;
	}
	
	public PairwiseOrthoMatch clone() {
		double [][] affine = (this.affine==null) ? null: (new double [][] {this.affine[0].clone(),this.affine[1].clone()});
		double [][] jtj = new double [this.jtj.length][];
		for (int i = 0; i < this.jtj.length; i++) {
			jtj[i] = this.jtj[i].clone();
		}
		PairwiseOrthoMatch pom = new PairwiseOrthoMatch(
				affine,
				jtj,
				this.rms,
				this.zoom_lev,
				this.overlap);
		if (nxy != null) {
			pom.nxy = nxy.clone();
		}
		pom.equalize1to0 = this.equalize1to0.clone();
		pom.ok = this.ok;
		pom.alt_data = this.alt_data.clone();
		pom.quat = (quat!=null)? quat.clone() : null;
		pom.qaffine = (qaffine!=null) ? (new double [][] {qaffine[0].clone(),qaffine[1].clone()}):null;
		return pom;
	}
	/**
	 * If this match is calculated from scene0 to scene1, inverse one is from scene1 to scene0  
	 * @param rd displacement from scene0 reference (vertical) point to scene1 reference point
	 * (GNSS-derived), referenced as -V in the notes below. (it is calculated as 0 from 1)
	 * @return inverted 3x2 matrix
	 * 
		Xs0, Xs1 - metric coordinates (from top-left) in each of the source images
		V - image center offset of the second scene from the first in rectified space (GPS-derived)
		S0, S1 - vertical point offset from the top-left in image coordinates
		A0,A1 - affine transform matrices (sensor coordinates from rectified coordinates)
		B0, B1 centers offsets in image coordinates
		Xr - rectified coordinates. Xr1, Xr2 - just different points
		
		Xsi = Ai*(Xr - V) + Bi + Si
		
		1) A00 = E, A1, B1 - second image match when the first is unity:
		
		Xs0 = Xr1 + S0
		Xs1 = A1 * (Xr1 - V) + B1 + S1;
		
		2) calculate inverted A1,B1 of the first scene relative to the second one. In this case Xs0
		of the first scene should still correspond to Xs1
		Xs0 = A0 * (Xr2 + V) + B0 + S0;
		Xs1 = Xr2 + S1
		
		----
		Xs1 =  A1 * (Xs0 - S0 - V) + B1 + S1;
		Xs1 - B1 - S1 =  A1 * (Xs0 - S0 - V);
		A1inv*(Xs1 - B1 - S1) =  Xs0 - S0 - V;
		Xs0 = A1inv*(Xs1 - B1 - S1) + S0 + V
		---
		Xr2 = Xs1 - S1
		Xs0 = A0 * (Xs1 - S1 + V) + B0 + S0;
		
		A1inv*(Xs1 - B1 - S1) + S0 + V = A0 * (Xs1 - S1 + V) + B0 + S0
		=> A0=A1inv
		A1inv*(Xs1 - B1 - S1) + S0 + V = A1inv * (Xs1 - S1 + V) + B0 + S0
		-A1inv*( B1)  + V = A1inv * ( V) + B0
		A1inv * ( V) + B0 + A1inv*(B1) - V = 0
		B0 = -((A1inv-E)*V + A1inv*B1)
	 */
	
	public PairwiseOrthoMatch getInverse(double [] rd) {
		double [][] affine = OrthoMap.invertAffine(getAffine());
		PairwiseOrthoMatch inverted_match = new PairwiseOrthoMatch(
				affine, // double [][] affine,
				jtj, // double [][] jtj,
				rms, // double rms,
				zoom_lev, // int zoom_lev)
				overlap); // 
		double [] corr = {
				rd[0] * (affine[0][0]-1.0)+ rd[1]*affine[0][1],
				rd[0] * affine[1][0]+       rd[1]*(affine[1][1]-1.0)};
		affine[0][2] += corr[0];
		affine[1][2] += corr[1];
		inverted_match.setEqualize2to1(new double [] {1/equalize1to0[0], -equalize1to0[1]/equalize1to0[0]}); 
		return inverted_match;	
	}
	
	/**
	 * Create differential PairwiseOrthoMatch instance from 2 affines of the scenes and an offset vector
	 * @param affine0
	 * @param affine1
	 * @param rd
	 * Continued from getInverse notes:
	 * =========================== V = -rd
		Xsi = Ai*(Xr - V) + Bi + Si
		
		Xs0 = A0*(Xr - 0) + B0 + S0
		Xs1 = A1*(Xr - V) + B1 + S1
		
		--- difference Ad, Bd 
		Xs0 = Xr1 + S0
		Xs1 = Ad * (Xr1 - V) + Bd + S1;
		
		
		Xs0 = A0*(Xr - 0) + B0 + S0
		Xs0 - (B0 + S0) = A0*Xr
		Xr = A0inv*(Xs0 - (B0 + S0))
		Xs1 = A1*((A0inv*(Xs0 - (B0 + S0))) - V) + B1 + S1
		
		Xr1 = Xs0-S0
		
		Xs1 = Ad * ((Xs0-S0) - V) + Bd + S1;
		
		A1*((A0inv*(Xs0 - (B0 + S0))) - V) + B1 + S1 = Ad * ((Xs0-S0) - V) + Bd + S1
		
		A1*((A0inv*(Xs0 - (B0 + S0))) - V) + B1 = Ad * ((Xs0-S0) - V) + Bd
		
		A1*((A0inv*(Xs0 - (B0 + S0))) - V) + B1 = Ad * ((Xs0-S0) - V) + Bd
		A1 * A0inv * Xs0 - A1 * A0inv * (B0 + S0) - A1*V + B1 - Ad *Xs0 + Ad * S0 +Ad*V -Bd
		=> Ad = A1 * A0inv:
		 - A1 * A0inv * (B0 + S0) - A1*V + B1  + Ad * S0 +Ad*V -Bd =
		 - Ad * (B0 + S0) - A1*V + B1  + Ad * S0 + Ad*V - Bd =
		 Ad * (S0 + V - B0 - S0) - A1*V + B1- Bd = 
		  Ad * (V - B0) - A1 * V + B1 - Bd
		Bd =  Ad * (V - B0) - A1 * V + B1
		
		--- check if A0 = E, B0 = 0
		Ad = A1inv
		Bd = A1 * V - A1 *V + B1 = B1
		--- check if A1 = E, B1 = 0, V1= -V
		Bd =  A0inv * (V1 - B0) - V1 =
		(A0inv-E)*V1 -A0inv*B0 = 
		-(A0inv-E)*V -A0inv*B0 = 
	 */
	public PairwiseOrthoMatch (
			double [][] affine0,
			double [][] affine1,
			double [] rd) {
		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);
		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)}};
	}
	
	public void combineEqualize(
			double [] equalize0,
			double [] equalize1	) {
		setEqualize2to1(new double[] {
				equalize1[0]/equalize0[0],
				equalize1[1]-equalize1[0]/equalize0[0]*equalize0[1]});
	}
	
	public static double [][] combineAffines( 
			double [][] affine0,
			double [][] affine, // differential
			double [] rd) {
		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 A = new Matrix (
				new double [][] {{affine[0][0],affine[0][1]},{affine[1][0],affine[1][1]}});
		Matrix B = new Matrix(new double [][] {{affine[0][2]},{affine[1][2]}});
		Matrix V =  new Matrix(new double [][] {{-rd[0]},{-rd[1]}});
		Matrix A1 = A.times(A0);
		Matrix B1 = B.minus(A.times(V.minus(B0).minus(A0.times(V))));
		double[][] affine1 = new double[][] {
			{A1.get(0,0),A1.get(0,1), B1.get(0,0)},
			{A1.get(1,0),A1.get(1,1), B1.get(1,0)}};
		return affine1;
	}	
	
	public int getZoomLevel() {
		return zoom_lev;
	}
	
	public double [][] getAffine(){
		if (affine == null) {
			return new double [][] {{1,0,0},{0,1,0}};
		}
		return affine;
	}
	
	public void setAffine(double [][] affine) {
		this.affine= affine;
	}
	
	//private void readObjectNoData() throws ObjectStreamException; // used to modify default values
}
