/* 
 * PROJECT: NyARToolkit (Extension)
 * --------------------------------------------------------------------------------
 * This work is based on the original ARToolKit developed by
 *   Hirokazu Kato
 *   Mark Billinghurst
 *   HITLab, University of Washington, Seattle
 * http://www.hitl.washington.edu/artoolkit/
 *
 * The NyARToolkit is Java edition ARToolKit class library.
 * Copyright (C)2008-2009 Ryo Iizuka
 *
 * This program 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/>.
 * 
 * For further information please contact.
 *	http://nyatla.jp/nyatoolkit/
 *	<airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>
 * 
 */
package jp.nyatla.nyartoolkit.core.transmat.optimize;

import jp.nyatla.nyartoolkit.core.NyARException;
import jp.nyatla.nyartoolkit.core.param.*;

import jp.nyatla.nyartoolkit.core.types.*;
import jp.nyatla.nyartoolkit.core.types.matrix.*;
import jp.nyatla.nyartoolkit.core.utils.*;



/**
 * このクラスは、NyARToolkit方式の姿勢行列Optimizerです。
 * <p>アルゴリズム -
 * 姿勢行列をX,Y,Zの回転方向について偏微分して、それぞれ誤差が最小になる点を求めます。
 * 下位が２点ある場合は、前回の結果に近い値を採用することで、ジッタを減らします。
 * </p>
 */
public class NyARPartialDifferentiationOptimize
{
	private final NyARPerspectiveProjectionMatrix _projection_mat_ref;

	/**
	 * コンストラクタです。
	 * 射影変換オブジェクトの参照値を設定して、インスタンスを生成します。
	 * @param i_projection_mat_ref
	 * 射影変換オブジェクトの参照値。
	 */
	public NyARPartialDifferentiationOptimize(NyARPerspectiveProjectionMatrix i_projection_mat_ref)
	{
		this._projection_mat_ref = i_projection_mat_ref;
		return;
	}
	/*
	 * 射影変換式 基本式 ox=(cosc * cosb - sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb + sinc * sina * cosb)*iz+i_trans.x; oy=(sinc * cosb + cosc * sina *
	 * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb - cosc * sina * cosb)*iz+i_trans.y; oz=(-cosa * sinb)*ix+(sina)*iy+(cosb * cosa)*iz+i_trans.z;
	 * 
	 * double ox=(cosc * cosb)*ix+(-sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb)*iz + (sinc * sina * cosb)*iz+i_trans.x; double oy=(sinc * cosb)*ix
	 * +(cosc * sina * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb)*iz+(- cosc * sina * cosb)*iz+i_trans.y; double oz=(-cosa * sinb)*ix+(sina)*iy+(cosb *
	 * cosa)*iz+i_trans.z;
	 * 
	 * sina,cosaについて解く cx=(cp00*(-sinc*sinb*ix+sinc*cosb*iz)+cp01*(cosc*sinb*ix-cosc*cosb*iz)+cp02*(iy))*sina
	 * +(cp00*(-sinc*iy)+cp01*((cosc*iy))+cp02*(-sinb*ix+cosb*iz))*cosa
	 * +(cp00*(i_trans.x+cosc*cosb*ix+cosc*sinb*iz)+cp01*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp02*(i_trans.z));
	 * cy=(cp11*(cosc*sinb*ix-cosc*cosb*iz)+cp12*(iy))*sina +(cp11*((cosc*iy))+cp12*(-sinb*ix+cosb*iz))*cosa
	 * +(cp11*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp12*(i_trans.z)); ch=(iy)*sina +(-sinb*ix+cosb*iz)*cosa +i_trans.z; sinb,cosb hx=(cp00*(-sinc *
	 * sina*ix+cosc*iz)+cp01*(cosc * sina*ix+sinc*iz)+cp02*(-cosa*ix))*sinb +(cp01*(sinc*ix-cosc * sina*iz)+cp00*(cosc*ix+sinc * sina*iz)+cp02*(cosa*iz))*cosb
	 * +(cp00*(i_trans.x+(-sinc*cosa)*iy)+cp01*(i_trans.y+(cosc * cosa)*iy)+cp02*(i_trans.z+(sina)*iy)); double hy=(cp11*(cosc *
	 * sina*ix+sinc*iz)+cp12*(-cosa*ix))*sinb +(cp11*(sinc*ix-cosc * sina*iz)+cp12*(cosa*iz))*cosb +(cp11*(i_trans.y+(cosc *
	 * cosa)*iy)+cp12*(i_trans.z+(sina)*iy)); double h =((-cosa*ix)*sinb +(cosa*iz)*cosb +i_trans.z+(sina)*iy); パラメータ返還式 L=2*Σ(d[n]*e[n]+a[n]*b[n])
	 * J=2*Σ(d[n]*f[n]+a[n]*c[n])/L K=2*Σ(-e[n]*f[n]+b[n]*c[n])/L M=Σ(-e[n]^2+d[n]^2-b[n]^2+a[n]^2)/L 偏微分式 +J*cos(x) +K*sin(x) -sin(x)^2 +cos(x)^2
	 * +2*M*cos(x)*sin(x)
	 */
	private double optimizeParamX(double sinb,double cosb,double sinc,double cosc,NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException
	{
		NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;
		double L, J, K, M, N, O;
		L = J = K = M = N = O = 0;
		final double cp00 = cp.m00;
		final double cp01 = cp.m01;
		final double cp02 = cp.m02;
		final double cp11 = cp.m11;
		final double cp12 = cp.m12;

		for (int i = 0; i < i_number_of_vertex; i++) {
			double ix, iy, iz;
			ix = i_vertex3d[i].x;
			iy = i_vertex3d[i].y;
			iz = i_vertex3d[i].z;

			double X0 = (cp00 * (-sinc * sinb * ix + sinc * cosb * iz) + cp01 * (cosc * sinb * ix - cosc * cosb * iz) + cp02 * (iy));
			double X1 = (cp00 * (-sinc * iy) + cp01 * ((cosc * iy)) + cp02 * (-sinb * ix + cosb * iz));
			double X2 = (cp00 * (i_trans.x + cosc * cosb * ix + cosc * sinb * iz) + cp01 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp02 * (i_trans.z));
			double Y0 = (cp11 * (cosc * sinb * ix - cosc * cosb * iz) + cp12 * (iy));
			double Y1 = (cp11 * ((cosc * iy)) + cp12 * (-sinb * ix + cosb * iz));
			double Y2 = (cp11 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp12 * (i_trans.z));
			double H0 = (iy);
			double H1 = (-sinb * ix + cosb * iz);
			double H2 = i_trans.z;

			double VX = i_vertex2d[i].x;
			double VY = i_vertex2d[i].y;

			double a, b, c, d, e, f;
			a = (VX * H0 - X0);
			b = (VX * H1 - X1);
			c = (VX * H2 - X2);
			d = (VY * H0 - Y0);
			e = (VY * H1 - Y1);
			f = (VY * H2 - Y2);

			L += d * e + a * b;
			N += d * d + a * a;
			J += d * f + a * c;
			M += e * e + b * b;
			K += e * f + b * c;
			O += f * f + c * c;

		}
		L *=2;
		J *=2;
		K *=2;

		return getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);


	}

	private double optimizeParamY(double sina,double cosa,double sinc,double cosc, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException
	{
		NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;
		double L, J, K, M, N, O;
		L = J = K = M = N = O = 0;
		final double cp00 = cp.m00;
		final double cp01 = cp.m01;
		final double cp02 = cp.m02;
		final double cp11 = cp.m11;
		final double cp12 = cp.m12;

		for (int i = 0; i < i_number_of_vertex; i++) {
			double ix, iy, iz;
			ix = i_vertex3d[i].x;
			iy = i_vertex3d[i].y;
			iz = i_vertex3d[i].z;

			double X0 = (cp00 * (-sinc * sina * ix + cosc * iz) + cp01 * (cosc * sina * ix + sinc * iz) + cp02 * (-cosa * ix));
			double X1 = (cp01 * (sinc * ix - cosc * sina * iz) + cp00 * (cosc * ix + sinc * sina * iz) + cp02 * (cosa * iz));
			double X2 = (cp00 * (i_trans.x + (-sinc * cosa) * iy) + cp01 * (i_trans.y + (cosc * cosa) * iy) + cp02 * (i_trans.z + (sina) * iy));
			double Y0 = (cp11 * (cosc * sina * ix + sinc * iz) + cp12 * (-cosa * ix));
			double Y1 = (cp11 * (sinc * ix - cosc * sina * iz) + cp12 * (cosa * iz));
			double Y2 = (cp11 * (i_trans.y + (cosc * cosa) * iy) + cp12 * (i_trans.z + (sina) * iy));
			double H0 = (-cosa * ix);
			double H1 = (cosa * iz);
			double H2 = i_trans.z + (sina) * iy;

			double VX = i_vertex2d[i].x;
			double VY = i_vertex2d[i].y;

			double a, b, c, d, e, f;
			a = (VX * H0 - X0);
			b = (VX * H1 - X1);
			c = (VX * H2 - X2);
			d = (VY * H0 - Y0);
			e = (VY * H1 - Y1);
			f = (VY * H2 - Y2);

			L += d * e + a * b;
			N += d * d + a * a;
			J += d * f + a * c;
			M += e * e + b * b;
			K += e * f + b * c;
			O += f * f + c * c;

		}
		L *= 2;
		J *= 2;
		K *= 2;
		return getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);

	}

	private double optimizeParamZ(double sina,double cosa,double sinb,double cosb, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException
	{
		NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;
		double L, J, K, M, N, O;
		L = J = K = M = N = O = 0;
		final double cp00 = cp.m00;
		final double cp01 = cp.m01;
		final double cp02 = cp.m02;
		final double cp11 = cp.m11;
		final double cp12 = cp.m12;

		for (int i = 0; i < i_number_of_vertex; i++) {
			double ix, iy, iz;
			ix = i_vertex3d[i].x;
			iy = i_vertex3d[i].y;
			iz = i_vertex3d[i].z;

			double X0 = (cp00 * (-sina * sinb * ix - cosa * iy + sina * cosb * iz) + cp01 * (ix * cosb + sinb * iz));
			double X1 = (cp01 * (sina * ix * sinb + cosa * iy - sina * iz * cosb) + cp00 * (cosb * ix + sinb * iz));
			double X2 = cp00 * i_trans.x + cp01 * (i_trans.y) + cp02 * (-cosa * sinb) * ix + cp02 * (sina) * iy + cp02 * ((cosb * cosa) * iz + i_trans.z);
			double Y0 = cp11 * (ix * cosb + sinb * iz);
			double Y1 = cp11 * (sina * ix * sinb + cosa * iy - sina * iz * cosb);
			double Y2 = (cp11 * i_trans.y + cp12 * (-cosa * sinb) * ix + cp12 * ((sina) * iy + (cosb * cosa) * iz + i_trans.z));
			double H0 = 0;
			double H1 = 0;
			double H2 = ((-cosa * sinb) * ix + (sina) * iy + (cosb * cosa) * iz + i_trans.z);

			double VX = i_vertex2d[i].x;
			double VY = i_vertex2d[i].y;

			double a, b, c, d, e, f;
			a = (VX * H0 - X0);
			b = (VX * H1 - X1);
			c = (VX * H2 - X2);
			d = (VY * H0 - Y0);
			e = (VY * H1 - Y1);
			f = (VY * H2 - Y2);

			L += d * e + a * b;
			N += d * d + a * a;
			J += d * f + a * c;
			M += e * e + b * b;
			K += e * f + b * c;
			O += f * f + c * c;

		}
		L *=2;
		J *=2;
		K *=2;
		
		return getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);
	}
	private NyARDoublePoint3d __ang=new NyARDoublePoint3d();
	/**
	 * この関数は、回転行列を最適化します。
	 * i_vertex3dのオフセット値を、io_rotとi_transで座標変換後に射影変換した２次元座標と、i_vertex2dが最も近くなるように、io_rotを調整します。
	 * io_rot,i_transの値は、ある程度の精度で求められている必要があります。
	 * @param io_rot
	 * 調整する回転行列
	 * @param i_trans
	 * 平行移動量
	 * @param i_vertex3d
	 * 三次元オフセット座標
	 * @param i_vertex2d
	 * 理想座標系の頂点座標
	 * @param i_number_of_vertex
	 * 頂点数
	 * @throws NyARException
	 */
	public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex) throws NyARException
	{
		NyARDoublePoint3d ang = this.__ang;		
		// ZXY系のsin/cos値を抽出
		io_rot.getZXYAngle(ang);
		modifyMatrix(ang,i_trans,i_vertex3d,i_vertex2d,i_number_of_vertex,ang);
		io_rot.setZXYAngle(ang.x, ang.y, ang.z);
		return;
	}
	/**
	 * この関数は、回転角を最適化します。
	 * i_vertex3dのオフセット値を、i_angleとi_transで座標変換後に射影変換した２次元座標と、i_vertex2dが最も近くなる値を、o_angleへ返します。
	 * io_rot,i_transの値は、ある程度の精度で求められている必要があります。
	 * @param i_angle
	 * 回転角
	 * @param i_trans
	 * 平行移動量
	 * @param i_vertex3d
	 * 三次元オフセット座標
	 * @param i_vertex2d
	 * 理想座標系の頂点座標
	 * @param i_number_of_vertex
	 * 頂点数
	 * @param o_angle
	 * 調整した回転角を受け取る配列
	 * @throws NyARException
	 */	
	public void modifyMatrix(NyARDoublePoint3d i_angle,NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex,NyARDoublePoint3d o_angle) throws NyARException
	{

		// ZXY系のsin/cos値を抽出
		double sinx = Math.sin(i_angle.x);
		double cosx = Math.cos(i_angle.x);
		double siny = Math.sin(i_angle.y);
		double cosy = Math.cos(i_angle.y);
		double sinz = Math.sin(i_angle.z);
		double cosz = Math.cos(i_angle.z);		
		o_angle.x = i_angle.x+optimizeParamX(siny,cosy,sinz,cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.x);
		o_angle.y = i_angle.y+optimizeParamY(sinx,cosx,sinz,cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.y);
		o_angle.z = i_angle.z+optimizeParamZ(sinx,cosx,siny,cosy, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.z);
		return;	
	}
	
	private double[] __sin_table= new double[4];
	/**
	 * エラーレートが最小になる点を得る。
	 */
	private double getMinimumErrorAngleFromParam(double iL,double iJ, double iK, double iM, double iN, double iO, double i_hint_angle) throws NyARException
	{
		//iLが0の時は誤差修正しない。
		if (iL == 0) {
			return 0;
		}		
		double[] sin_table = this.__sin_table;

		double M = (iN - iM)/iL;
		double J = iJ/iL;
		double K = -iK/iL;

		// パラメータからsinテーブルを作成
		// (- 4*M^2-4)*x^4 + (4*K- 4*J*M)*x^3 + (4*M^2 -(K^2- 4)- J^2)*x^2 +(4*J*M- 2*K)*x + J^2-1 = 0
		int number_of_sin = NyAREquationSolver.solve4Equation(-4 * M * M - 4, 4 * K - 4 * J * M, 4 * M * M - (K * K - 4) - J * J, 4 * J * M - 2 * K, J * J - 1, sin_table);


		// 最小値２個を得ておく。
		double min_ang_0 = Double.MAX_VALUE;
		double min_ang_1 = Double.MAX_VALUE;
		double min_err_0 = Double.MAX_VALUE;
		double min_err_1 = Double.MAX_VALUE;
		for (int i = 0; i < number_of_sin; i++) {
			// +-cos_v[i]が頂点候補
			double sin_rt = sin_table[i];
			double cos_rt = Math.sqrt(1 - (sin_rt * sin_rt));
			// cosを修復。微分式で0に近い方が正解
			// 0 = 2*cos(x)*sin(x)*M - sin(x)^2 + cos(x)^2 + sin(x)*K + cos(x)*J
			double a1 = 2 * cos_rt * sin_rt * M + sin_rt * (K - sin_rt) + cos_rt * (cos_rt + J);
			double a2 = 2 * (-cos_rt) * sin_rt * M + sin_rt * (K - sin_rt) + (-cos_rt) * ((-cos_rt) + J);
			// 絶対値になおして、真のcos値を得ておく。
			a1 = a1 < 0 ? -a1 : a1;
			a2 = a2 < 0 ? -a2 : a2;
			cos_rt = (a1 < a2) ? cos_rt : -cos_rt;
			double ang = Math.atan2(sin_rt, cos_rt);
			// エラー値を計算
			double err = iN * sin_rt * sin_rt + (iL*cos_rt + iJ) * sin_rt + iM * cos_rt * cos_rt + iK * cos_rt + iO;
			// 最小の２個を獲得する。
			if (min_err_0 > err) {
				min_err_1 = min_err_0;
				min_ang_1 = min_ang_0;
				min_err_0 = err;
				min_ang_0 = ang;
			} else if (min_err_1 > err) {
				min_err_1 = err;
				min_ang_1 = ang;
			}
		}
		// [0]をテスト
		double gap_0;
		gap_0 = min_ang_0 - i_hint_angle;
		if (gap_0 > Math.PI) {
			gap_0 = (min_ang_0 - Math.PI * 2) - i_hint_angle;
		} else if (gap_0 < -Math.PI) {
			gap_0 = (min_ang_0 + Math.PI * 2) - i_hint_angle;
		}
		// [1]をテスト
		double gap_1;
		gap_1 = min_ang_1 - i_hint_angle;
		if (gap_1 > Math.PI) {
			gap_1 = (min_ang_1 - Math.PI * 2) - i_hint_angle;
		} else if (gap_1 < -Math.PI) {
			gap_1 = (min_ang_1 + Math.PI * 2) - i_hint_angle;
		}
		return Math.abs(gap_1) < Math.abs(gap_0) ? gap_1 : gap_0;
	}
}
