/* 
 * PROJECT: NyARToolkitCS
 * --------------------------------------------------------------------------------
 * 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>
 * 
 */
namespace jp.nyatla.nyartoolkit.cs.core
{
    /**
     * 平行移動量を計算するクラス
     * 
     * NyARPerspectiveProjectionMatrixに直接アクセスしてる場所があるけど、
     * この辺の計算はNyARPerspectiveProjectionMatrixクラスの関数にして押し込む予定。
     *
     */
    public class NyARFitVecCalculator
    {
        private NyARMat _mat_b = new NyARMat(3, 8);//3,NUMBER_OF_VERTEX*2
        private NyARMat _mat_a = new NyARMat(8, 3);/*NUMBER_OF_VERTEX,3*/
        private NyARMat _mat_d = new NyARMat(3, 3);
        private NyARPerspectiveProjectionMatrix _projection_mat;
        private NyARCameraDistortionFactor _distortionfactor;


        //	private NyARDoublePoint2d[] _vertex_2d_ref;
        public NyARFitVecCalculator(NyARPerspectiveProjectionMatrix i_projection_mat_ref, NyARCameraDistortionFactor i_distortion_ref)
        {
            // 変換マトリクスdとbの準備(arGetTransMatSubの一部)
            double[][] a_array = this._mat_a.getArray();
            double[][] b_array = this._mat_b.getArray();

            //変換用行列のcpara固定値の部分を先に初期化してしまう。
            for (int i = 0; i < 4; i++)
            {
                int x2 = i * 2;
                a_array[x2][0] = b_array[0][x2] = i_projection_mat_ref.m00;// mat_a->m[j*6+0]=mat_b->m[num*0+j*2] =cpara[0][0];
                a_array[x2][1] = b_array[1][x2] = i_projection_mat_ref.m01;// 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] - o_marker_vertex_2d[i].x;// 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] = i_projection_mat_ref.m11;// 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] - o_marker_vertex_2d[i].y;// mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1];
            }
            this._projection_mat = i_projection_mat_ref;
            this._distortionfactor = i_distortion_ref;
            return;
        }
        private NyARDoublePoint2d[] _fitsquare_vertex = NyARDoublePoint2d.createArray(4);
        private NyARTransOffset _offset_square;
        public void setOffsetSquare(NyARTransOffset i_offset)
        {
            this._offset_square = i_offset;
            return;
        }
        public NyARDoublePoint2d[] getFitSquare()
        {
            return this._fitsquare_vertex;
        }
        public NyARTransOffset getOffsetVertex()
        {
            return this._offset_square;
        }

        /**
         * 適合させる矩形座標を指定します。
         * @param i_square_vertex
         * @throws NyARException
         */
        public void setFittedSquare(NyARDoublePoint2d[] i_square_vertex)
        {
            NyARDoublePoint2d[] vertex = _fitsquare_vertex;
            //		int i;
            //		if (arFittingMode == AR_FITTING_TO_INPUT) {
            //			// arParamIdeal2Observをバッチ処理
            this._distortionfactor.ideal2ObservBatch(i_square_vertex, vertex, 4);
            //		} else {
            //			for (i = 0; i < NUMBER_OF_VERTEX; i++) {
            //				o_marker_vertex_2d[i].x = i_square_vertex[i].x;
            //				o_marker_vertex_2d[i].y = i_square_vertex[i].y;
            //			}
            //		}		


            double cpara02 = this._projection_mat.m02;
            double cpara12 = this._projection_mat.m12;
            NyARMat mat_d = _mat_d;
            NyARMat mat_a = this._mat_a;
            NyARMat mat_b = this._mat_b;
            double[][] a_array = mat_a.getArray();
            double[][] b_array = mat_b.getArray();
            int x2;
            for (int i = 0; i < 4; i++)
            {
                x2 = i * 2;
                a_array[x2][2] = b_array[2][x2] = cpara02 - vertex[i].x;// mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0];
                a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara12 - vertex[i].y;// mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1];
            }
            // mat_d
            mat_d.matrixMul(mat_b, mat_a);
            mat_d.matrixSelfInv();
            return;
        }
        private NyARMat _mat_e = new NyARMat(3, 1);
        private NyARMat _mat_f = new NyARMat(3, 1);
        private NyARMat __calculateTransferVec_mat_c = new NyARMat(8, 1);//NUMBER_OF_VERTEX * 2, 1
        private NyARDoublePoint3d[] __calculateTransfer_point3d = NyARDoublePoint3d.createArray(4);

        /**
         * 現在のオフセット矩形、適合先矩形と、回転行列から、平行移動量を計算します。
         * @param i_rotation
         * @param o_transfer
         * @throws NyARException
         */
        public void calculateTransfer(NyARRotMatrix i_rotation, NyARDoublePoint3d o_transfer)
        {
            //assert(this._offset_square != null);
            double cpara00 = this._projection_mat.m00;
            double cpara01 = this._projection_mat.m01;
            double cpara02 = this._projection_mat.m02;
            double cpara11 = this._projection_mat.m11;
            double cpara12 = this._projection_mat.m12;

            NyARDoublePoint3d[] point3d = this.__calculateTransfer_point3d;
            NyARDoublePoint3d[] vertex3d = this._offset_square.vertex;
            NyARDoublePoint2d[] vertex2d = this._fitsquare_vertex;
            NyARMat mat_c = this.__calculateTransferVec_mat_c;// 次処理で値をもらうので、初期化の必要は無い。

            double[][] f_array = this._mat_f.getArray();
            double[][] c_array = mat_c.getArray();


            //（3D座標？）を一括請求
            int x2;
            i_rotation.getPoint3dBatch(vertex3d, point3d, 4);
            for (int i = 0; i < 4; i++)
            {
                x2 = i + i;
                NyARDoublePoint3d point3d_ptr = point3d[i];
                //			i_rotation.getPoint3d(vertex3d[i],point3d);
                //透視変換？
                c_array[x2][0] = point3d_ptr.z * vertex2d[i].x - cpara00 * point3d_ptr.x - cpara01 * point3d_ptr.y - cpara02 * point3d_ptr.z;// 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] = point3d_ptr.z * vertex2d[i].y - cpara11 * point3d_ptr.y - cpara12 * point3d_ptr.z;// mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz;
            }
            this._mat_e.matrixMul(this._mat_b, mat_c);
            this._mat_f.matrixMul(this._mat_d, this._mat_e);

            // double[] trans=wk_arGetTransMatSub_trans;//double trans[3];
            o_transfer.x = f_array[0][0];// trans[0] = mat_f->m[0];
            o_transfer.y = f_array[1][0];
            o_transfer.z = f_array[2][0];// trans[2] = mat_f->m[2];
            return;
        }
    }
}