/* 
 * PROJECT: NyARToolkit
 * --------------------------------------------------------------------------------
 * 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 version ARToolkit class library.
 * Copyright (C)2008 R.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 2
 * 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 framework; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 * 
 * For further information please contact.
 *	http://nyatla.jp/nyatoolkit/
 *	<airmail(at)ebony.plala.or.jp>
 * 
 */
package jp.nyatla.nyartoolkit.core;

import jp.nyatla.nyartoolkit.NyARException;
import jp.nyatla.util.DoubleValue;

public class NyARTransMat_O1 implements NyARTransMat{
    private final static int AR_FITTING_TO_IDEAL=0;//#define  AR_FITTING_TO_IDEAL          0
    private final static int AR_FITTING_TO_INPUT=1;//#define  AR_FITTING_TO_INPUT          1
    private final static int	arFittingMode	=AR_FITTING_TO_INPUT;

    private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT=5;//#define   AR_GET_TRANS_MAT_MAX_LOOP_COUNT         5
    private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR=1.0;//#define   AR_GET_TRANS_MAT_MAX_FIT_ERROR          1.0
    private final static double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR=1.0;
    private final static int P_MAX=10;//頂点の数(4で十分だけどなんとなく10)//#define P_MAX       500
    private final static int NUMBER_OF_VERTEX=4;//処理対象の頂点数
    private final NyARTransRot transrot;
    private final double[] center={0.0,0.0};
    private final NyARParam param;
//    private final NyARMat result_mat=new NyARMat(3,4);
    public NyARTransMat_O1(NyARParam i_param)throws NyARException
    {
	param=i_param;
	transrot=new NyARTransRot_O3(i_param,NUMBER_OF_VERTEX);

    }
    public void setCenter(double i_x,double i_y)
    {
	center[0]=i_x;
	center[1]=i_x;
    }
//    public NyARMat getTransformationMatrix()
//    {
//	return result_mat;
//    }

    private final double[][] wk_transMat_pos3d=new double[P_MAX][3];//pos3d[P_MAX][3];
    private final double[][] wk_transMat_ppos2d=new double[4][2];
    private final double[][] wk_transMat_ppos3d=new double[4][2];
    private final double[] wk_transMat_off=new double[3];
    /**
     * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )
     * 関数の置き換え。
     * 保持している変換行列を更新する。
     * @param square
     * 計算対象のNyARSquareオブジェクト
     * @param i_direction
     * マーカーの方向
     * @param i_width
     * マーカーのサイズ(mm)
     * @param o_result_conv
     * 変換行列を受け取るオブジェクトを指定します。
     * @return
     * @throws NyARException
     */
    public double transMat(NyARSquare square,int i_direction,double i_width,NyARTransMatResult o_result_conv)throws NyARException
    {
	double[][]  ppos2d=wk_transMat_ppos2d;
	double[][]  ppos3d=wk_transMat_ppos3d;
	double[] off=wk_transMat_off;
	double[][] pos3d=wk_transMat_pos3d;
	int     dir;
	double  err=-1;



	transrot.initRot(square,i_direction);

	dir = i_direction;
	ppos2d[0][0] = square.sqvertex[(4-dir)%4][0];
	ppos2d[0][1] = square.sqvertex[(4-dir)%4][1];
	ppos2d[1][0] = square.sqvertex[(5-dir)%4][0];
	ppos2d[1][1] = square.sqvertex[(5-dir)%4][1];
	ppos2d[2][0] = square.sqvertex[(6-dir)%4][0];
	ppos2d[2][1] = square.sqvertex[(6-dir)%4][1];
	ppos2d[3][0] = square.sqvertex[(7-dir)%4][0];
	ppos2d[3][1] = square.sqvertex[(7-dir)%4][1];
	ppos3d[0][0] = center[0] - i_width/2.0;
	ppos3d[0][1] = center[1] + i_width/2.0;
	ppos3d[1][0] = center[0] + i_width/2.0;
	ppos3d[1][1] = center[1] + i_width/2.0;
	ppos3d[2][0] = center[0] + i_width/2.0;
	ppos3d[2][1] = center[1] - i_width/2.0;
	ppos3d[3][0] = center[0] - i_width/2.0;
	ppos3d[3][1] = center[1] - i_width/2.0;

	//arGetTransMat3の前段処理(pos3dとoffを初期化)
	arGetTransMat3_initPos3d(ppos3d,pos3d,off);



	for(int i=0;i<AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) {
	    err = arGetTransMat3(ppos2d, pos3d,off,o_result_conv);
	    if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){
		break;
	    }
	}

	return err;
    }
    /**
     * transMatContinue用のワーク
     */
    private final NyARTransMatResult wk_transMatContinue_result=new NyARTransMatResult();
    /**
     * double arGetTransMatCont( ARMarkerInfo *marker_info, double prev_conv[3][4],double center[2], double width, double conv[3][4] )
     *     
     * @param i_square
     * @param i_direction
     * マーカーの方位を指定する。
     * @param i_width
     * @param io_result_conv
     * 計算履歴を持つNyARTransMatResultオブジェクトを指定する。
     * 履歴を持たない場合は、transMatと同じ処理を行う。
     * @return
     * @throws NyARException
     */
    public double transMatContinue(NyARSquare i_square,int i_direction, double i_width,NyARTransMatResult io_result_conv)throws NyARException
    {
	//io_result_convが初期値なら、transMatで計算する。
	if(!io_result_conv.hasValue()){
	    return this.transMat(i_square, i_direction, i_width, io_result_conv);
	}
	
	
	
	double  err1,err2;
	int     i,dir;
	double[][]  ppos2d=wk_transMat_ppos2d;
	double[][]  ppos3d=wk_transMat_ppos3d;
	double[]    off   =wk_transMat_off;
	double[][]  pos3d=wk_transMat_pos3d;

	//arGetTransMatContSub計算部分
	transrot.initRotByPrevResult(io_result_conv);

	dir = i_direction;
	ppos2d[0][0] = i_square.sqvertex[(4-dir)%4][0];
	ppos2d[0][1] = i_square.sqvertex[(4-dir)%4][1];
	ppos2d[1][0] = i_square.sqvertex[(5-dir)%4][0];
	ppos2d[1][1] = i_square.sqvertex[(5-dir)%4][1];
	ppos2d[2][0] = i_square.sqvertex[(6-dir)%4][0];
	ppos2d[2][1] = i_square.sqvertex[(6-dir)%4][1];
	ppos2d[3][0] = i_square.sqvertex[(7-dir)%4][0];
	ppos2d[3][1] = i_square.sqvertex[(7-dir)%4][1];
	ppos3d[0][0] = center[0] - i_width/2.0;
	ppos3d[0][1] = center[1] + i_width/2.0;
	ppos3d[1][0] = center[0] + i_width/2.0;
	ppos3d[1][1] = center[1] + i_width/2.0;
	ppos3d[2][0] = center[0] + i_width/2.0;
	ppos3d[2][1] = center[1] - i_width/2.0;
	ppos3d[3][0] = center[0] - i_width/2.0;
	ppos3d[3][1] = center[1] - i_width/2.0;

	//arGetTransMat3の前段処理(pos3dとoffを初期化)
	arGetTransMat3_initPos3d(ppos3d,pos3d,off);

	err1=err2=-1;
	for( i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) {
	    err1 = arGetTransMat3(ppos2d, pos3d,off,io_result_conv);
	    if( err1 < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){
		//十分な精度を達成できたらブレーク
		break;
	    }
	}

	//エラー値が許容範囲でなければTransMatをやり直し
	if(err1>AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR ) {
	    NyARTransMatResult result2=this.wk_transMatContinue_result;
	    //transMatを実行
	    transrot.initRot(i_square,i_direction);
            err2 = transMat(i_square,i_direction,i_width,result2);
	    //transmMatここまで
	    if(err2<err1){
		io_result_conv.copyFrom(result2);
		err1 = err2;
	    }
	}
	return err1;
    }


 

    private final double[] wk_arGetTransMat3_initPos3d_pmax=new double[3];
    private final double[] wk_arGetTransMat3_initPos3d_pmin=new double[3];
    /**
     * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。
     * 計算結果から再帰的に変更される可能性が無いので、切り離し。
     * @param i_ppos3d
     * 入力配列[num][3]
     * @param o_pos3d
     * 出力配列[P_MAX][3]
     * @param o_off
     * [3]
     * @throws NyARException
     */
    private final void arGetTransMat3_initPos3d(double i_ppos3d[][],double[][] o_pos3d,double[] o_off)throws NyARException
    {
	double[] pmax=wk_arGetTransMat3_initPos3d_pmax;//new double[3];
	double[] pmin=wk_arGetTransMat3_initPos3d_pmin;//new double[3];//double  off[3], pmax[3], pmin[3];
	int i;
	pmax[0]=pmax[1]=pmax[2] = -10000000000.0;
	pmin[0]=pmin[1]=pmin[2] =  10000000000.0;
	for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {
	    if( i_ppos3d[i][0] > pmax[0] ){
		pmax[0] = i_ppos3d[i][0];
	    }
	    if( i_ppos3d[i][0] < pmin[0] ){
		pmin[0] = i_ppos3d[i][0];
	    }
	    if( i_ppos3d[i][1] > pmax[1] ){
		pmax[1] = i_ppos3d[i][1];
	    }
	    if( i_ppos3d[i][1] < pmin[1] ){
		pmin[1] = i_ppos3d[i][1];
	    }
	    /*	オリジナルでもコメントアウト
	        if( ppos3d[i][2] > pmax[2] ) pmax[2] = ppos3d[i][2];
	        if( ppos3d[i][2] < pmin[2] ) pmin[2] = ppos3d[i][2];
	     */	    
	}
	o_off[0] = -(pmax[0] + pmin[0]) / 2.0;
	o_off[1] = -(pmax[1] + pmin[1]) / 2.0;
	o_off[2] = -(pmax[2] + pmin[2]) / 2.0;

	
	double[] o_pos3d_pt;
        double[] i_pos_pd_pt;	
	for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {
            o_pos3d_pt =o_pos3d[i];
            i_pos_pd_pt=i_ppos3d[i];
            o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0];
            o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1];
            o_pos3d_pt[2] = 0.0;
	}
    }    
    
     
    /**
     * double arGetTransMat3( double rot[3][3], double ppos2d[][2],double ppos3d[][2], int num, double conv[3][4],double *dist_factor, double cpara[3][4] )
     * STEP 414->
     * @param ppos2d
     * @param i_pos3d
     * @param i_off
     * @param num
     * @return
     * @throws NyARException
     */
    private final double arGetTransMat3(
	    double ppos2d[][],
	    final double i_pos3d[][],
	    final double i_off[],
	    NyARTransMatResult o_result_conv)throws NyARException{

	double  ret;
	ret = arGetTransMatSub(ppos2d,i_pos3d,i_off,o_result_conv);
	
//        double[][] conv=o_result_conv.getArray();
//        double[] rot=transrot.getArray();
//        for(int i=0;i<3;i++){
//            conv[i][0] = rot[i*3+0];
//            conv[i][1] = rot[i*3+1];
//            conv[i][2] = rot[i*3+2];
//        }
//	conv[0][3] = conv[0][0]*i_off[0] + conv[0][1]*i_off[1] + conv[0][2]*i_off[2] + conv[0][3];
//	conv[1][3] = conv[1][0]*i_off[0] + conv[1][1]*i_off[1] + conv[1][2]*i_off[2] + conv[1][3];
//	conv[2][3] = conv[2][0]*i_off[0] + conv[2][1]*i_off[1] + conv[2][2]*i_off[2] + conv[2][3];
	return ret;
    }
    private final NyARMat wk_arGetTransMatSub_mat_a=new NyARMat(NUMBER_OF_VERTEX*2,3);
    private final NyARMat wk_arGetTransMatSub_mat_b=new NyARMat(3,NUMBER_OF_VERTEX*2);
    private final NyARMat wk_arGetTransMatSub_mat_c=new NyARMat(NUMBER_OF_VERTEX*2,1);
    private final NyARMat wk_arGetTransMatSub_mat_d=new NyARMat( 3, 3 );
    private final NyARMat wk_arGetTransMatSub_mat_e=new NyARMat( 3, 1 );
    private final NyARMat wk_arGetTransMatSub_mat_f=new NyARMat( 3, 1 );
    private final double[] wk_arGetTransMatSub_trans=new double[3];
    private final double[][] wk_arGetTransMatSub_pos2d=new double[P_MAX][2];//pos2d[P_MAX][2];
    private final DoubleValue wk_arGetTransMatSub_a1=new DoubleValue();
    private final DoubleValue wk_arGetTransMatSub_a2=new DoubleValue();
    

    /**
     * static double arGetTransMatSub( double rot[3][3], double ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double *dist_factor, double cpara[3][4] )
     * Optimize:2008.04.20:STEP[1033→1004]
     * @param i_ppos2d
     * @param i_pos3d
     * @return
     * @throws NyARException
     */
    private final double arGetTransMatSub(double i_ppos2d[][],double i_pos3d[][],double[] i_off,NyARTransMatResult o_result_conv) throws NyARException
    {
	double[][] pos2d=wk_arGetTransMatSub_pos2d;
	double cpara[]=param.get34Array();
	NyARMat mat_a,mat_b,mat_c,mat_d,mat_e,mat_f;//ARMat   *mat_a, *mat_b, *mat_c, *mat_d, *mat_e, *mat_f;

	double  wx, wy, wz;
	double  ret;
	int     i;
	double[] po2d_pt;

	mat_a =this.wk_arGetTransMatSub_mat_a;
//	mat_a.realloc(NUMBER_OF_VERTEX*2,3);
	double[][] a_array=mat_a.getArray();

	mat_b =this.wk_arGetTransMatSub_mat_b;
//	mat_b.realloc(3,NUMBER_OF_VERTEX*2);
	double[][] b_array=mat_b.getArray();


        DoubleValue a1=wk_arGetTransMatSub_a1;
        DoubleValue a2=wk_arGetTransMatSub_a2;
	if(arFittingMode == AR_FITTING_TO_INPUT ){
	    for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {
		param.ideal2Observ(i_ppos2d[i][0], i_ppos2d[i][1],a1,a2);//arParamIdeal2Observ(dist_factor, ppos2d[i][0], ppos2d[i][1],&pos2d[i][0], &pos2d[i][1]);
		po2d_pt=pos2d[i];
		po2d_pt[0]=a1.value;
		po2d_pt[1]=a2.value;
	    }
	}else{
	    for( i = 0; i < NUMBER_OF_VERTEX; i++ ){
		po2d_pt=pos2d[i];
		pos2d[i][0] = i_ppos2d[i][0];
		pos2d[i][1] = i_ppos2d[i][1];
	    }
	}
	mat_c =this.wk_arGetTransMatSub_mat_c;//次処理で値をもらうので、初期化の必要は無い。
//	mat_c.realloc(NUMBER_OF_VERTEX*2,1);
	double[][] c_array=mat_c.getArray();
	double[] rot=transrot.getArray();
	double[] i_pos3d_pt;
	int x2;
	for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {
	    x2=i*2;
	    i_pos3d_pt=i_pos3d[i];
	    po2d_pt=pos2d[i];
	    wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];
	    wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];
	    wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];
	    //</Optimize>
	    a_array[x2  ][0]=b_array[0][x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0];
	    a_array[x2  ][1]=b_array[1][x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];
	    a_array[x2  ][2]=b_array[2][x2]=cpara[0*4+2] - po2d_pt[0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];
	    a_array[x2+1][0]=b_array[0][x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;
	    a_array[x2+1][1]=b_array[1][x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1];
	    a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - po2d_pt[1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];
	    c_array[x2][0]  =wz * po2d_pt[0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;
	    c_array[x2+1][0]=wz * po2d_pt[1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;
	}
	mat_d = this.wk_arGetTransMatSub_mat_d;//次処理で値をもらうので、初期化の必要は無い。
	mat_e = this.wk_arGetTransMatSub_mat_e;//次処理で値をもらうので、初期化の必要は無い。
	mat_f = this.wk_arGetTransMatSub_mat_f;//次処理で値をもらうので、初期化の必要は無い。
	double[][] f_array=mat_f.getArray();

	mat_d.matrixMul(mat_b, mat_a );
	mat_e.matrixMul(mat_b, mat_c );
	mat_d.matrixSelfInv();
	mat_f.matrixMul(mat_d, mat_e );

	double[] trans=wk_arGetTransMatSub_trans;//double  trans[3];	
	trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];
	trans[1] = f_array[1][0];
	trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];

	ret =transrot.modifyMatrix(trans, i_pos3d, pos2d);
	for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {
	    x2=i*2;
	    i_pos3d_pt=i_pos3d[i];
	    po2d_pt=pos2d[i];
	    //<Optimize>    
	    //wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] * pos3d[j][2];
	    //wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] * pos3d[j][2];
	    //wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] * pos3d[j][2];
	    wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];
	    wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];
	    wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];
	    //</Optimize>
	    a_array[x2  ][0]=b_array[0][x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0];
	    a_array[x2  ][1]=b_array[1][x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];
	    a_array[x2  ][2]=b_array[2][x2]=cpara[0*4+2] - po2d_pt[0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];
	    a_array[x2+1][0]=b_array[0][x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;
	    a_array[x2+1][1]=b_array[1][x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1];
	    a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - po2d_pt[1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];
	    c_array[x2][0]  =wz * po2d_pt[0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;
	    c_array[x2+1][0]=wz * po2d_pt[1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;
	}

	mat_d.matrixMul(mat_b, mat_a );
	mat_e.matrixMul(mat_b, mat_c );
	mat_d.matrixSelfInv();
	mat_f.matrixMul(mat_d, mat_e );

	trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];
	trans[1] = f_array[1][0];
	trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];


	ret = transrot.modifyMatrix(trans, i_pos3d, pos2d);
	//変換行列を保存
	o_result_conv.updateMatrixValue(this.transrot,i_off,trans);
	
//	double[][] conv=o_result_conv.getArray();
//	for( i = 2; i >=0; i-- ) {//<Optimize/>for( j = 0; j < 3; j++ ) {
//	    //<Optimize>
//	    //for( i = 0; i < 3; i++ ){
//	    //	conv[j][i] = rot[j][i];
//	    //}
//	    conv[i][0] = rot[i*3+0];
//	    conv[i][1] = rot[i*3+1];
//	    conv[i][2] = rot[i*3+2];
//	    //</Optimize>
//	    conv[i][3] = trans[i];
//	}
	return ret;
    }
}
