/* 
 * 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 NyARToolkitCS is C# version NyARToolkit 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>
 * 
 */
using System;
using System.Collections.Generic;
using System.IO;
using jp.nyatla.nyartoolkit.cs;
using jp.nyatla.nyartoolkit.cs.utils;

namespace jp.nyatla.nyartoolkit.cs.core
{
    /*typedef struct {
        int      xsize, ysize;
        double   mat[3][4];
        double   dist_factor[4];
    } ARParam;*/
    public class NyARParam{
        private const int SIZE_OF_PARAM_SET=4+4+(3*4*8)+(4*8);
        private const int PD_LOOP = 3;
        protected int		xsize, ysize;
        private double[]	array34=new double[3*4];//Double2dArray	mat=new Double2dArray(3,4);
        private double[]  dist_factor=new double[4];
        public int getX()
        {
	        return xsize;
        }
        public int getY()
        {
	        return ysize;
        }
        public double[] getDistFactor()
        {
	        return dist_factor;
        }
        /**
         * パラメタを格納した[4x3]配列を返します。
         * @return
         */
        public double[] get34Array()
        {
	        return array34;
        }
        /**
         * ARToolKit標準ファイルから1個目の設定をロードする。
         * @param i_filename
         * @throws NyARException
         */
        public void loadFromARFile(String i_filename)
        {
            try {
                StreamReader s = new StreamReader(i_filename);
                loadFromARFile(s.BaseStream);
            } catch (Exception e) {
                throw new NyARException(e);
            }
        }
        public void loadFromARFile(Stream i_stream)
        {
            try {
                NyARParam[] new_inst= arParamLoad(i_stream, 1);
                i_stream.Close();
                xsize = new_inst[0].xsize;
                ysize = new_inst[0].ysize;
                array34 = new_inst[0].array34;
                dist_factor = new_inst[0].dist_factor;
            } catch (Exception e) {
                throw new NyARException(e);
            }
        }
        /*static double dot( double a1, double a2, double a3,double b1, double b2, double b3 )*/
        private static double dot( double a1, double a2, double a3,double b1, double b2, double b3 )
        {
            return( a1 * b1 + a2 * b2 + a3 * b3 );
        }
        /*	static double norm( double a, double b, double c )*/
        private static double norm( double a, double b, double c )
        {
            return Math.Sqrt( a*a + b*b + c*c );
        }
        /**
         * int  arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] );
         * 関数の置き換え
         * Optimize STEP[754->665]
         * @param o_cpara
         * 戻り引数。3x4のマトリクスを指定すること。
         * @param o_trans
         * 戻り引数。3x4のマトリクスを指定すること。
         * @return
         */
        public void decompMat(NyARMat o_cpara, NyARMat o_trans)
        {
	        double[] source=array34;
            double[] Cpara=new double[3*4];//double    Cpara[3][4];
            double    rem1, rem2, rem3;
            int i;
            if(source[2*4+3]>= 0 ){//if( source[2][3] >= 0 ) {
                //<Optimize>
                //for(int r = 0; r < 3; r++ ){
                //    for(int c = 0; c < 4; c++ ){
                //        Cpara[r][c]=source[r][c];//Cpara[r][c] = source[r][c];
                //    }
                //}
                for(i=0;i<12;i++){
                    Cpara[i]=source[i];//Cpara[r][c] = source[r][c];
                }
                //</Optimize>
            }else {
                //<Optimize>
                //for(int r = 0; r < 3; r++ ){
                //	for(int c = 0; c < 4; c++ ){
                //        Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]);
                //    }
                //}
                for(i=0;i<12;i++){
                    Cpara[i]=source[i];//Cpara[r][c] = source[r][c];
                }
                //</Optimize>
            }
        
            double[][] cpara=o_cpara.getArray();
            double[][] trans=o_trans.getArray();
            for(int r = 0; r < 3; r++ ){
                for(int c = 0; c < 4; c++ ){
                    cpara[r][c]=0.0;//cpara[r][c] = 0.0;
                }
            }
            cpara[2][2]=norm(Cpara[2*4+0],Cpara[2*4+1], Cpara[2*4+2]);//cpara[2][2] = norm( Cpara[2][0], Cpara[2][1], Cpara[2][2] );
            trans[2][0]=Cpara[2*4+0] / cpara[2][2];//trans[2][0] = Cpara[2][0] / cpara[2][2];
            trans[2][1]=Cpara[2*4+1]/ cpara[2][2];//trans[2][1] = Cpara[2][1] / cpara[2][2];
            trans[2][2]=Cpara[2*4+2]/ cpara[2][2];//trans[2][2] = Cpara[2][2] / cpara[2][2];
            trans[2][3]=Cpara[2*4+3] / cpara[2][2];//trans[2][3] = Cpara[2][3] / cpara[2][2];
        	
            cpara[1][2]=dot(trans[2][0], trans[2][1], trans[2][2],Cpara[1*4+0], Cpara[1*4+1], Cpara[1*4+2]);//cpara[1][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[1][0], Cpara[1][1], Cpara[1][2] );
            rem1=Cpara[1*4+0]- cpara[1][2] * trans[2][0];//rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0];
            rem2=Cpara[1*4+1] - cpara[1][2] * trans[2][1];//rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1];
            rem3=Cpara[1*4+2] - cpara[1][2] * trans[2][2];//rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2];
            cpara[1][1]=norm(rem1, rem2, rem3 );//cpara[1][1] = norm( rem1, rem2, rem3 );
            trans[1][0]= rem1/cpara[1][1];//trans[1][0] = rem1 / cpara[1][1];
            trans[1][1]=rem2/cpara[1][1];//trans[1][1] = rem2 / cpara[1][1];
            trans[1][2]=rem3 / cpara[1][1];//trans[1][2] = rem3 / cpara[1][1];
        
            cpara[0][2]=dot(trans[2][0],trans[2][1], trans[2][2],Cpara[0*4+0], Cpara[0*4+1], Cpara[0*4+2] );//cpara[0][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] );
            cpara[0][1]=dot(trans[1][0],trans[1][1], trans[1][2],Cpara[0*4+0], Cpara[0*4+1], Cpara[0*4+2]);//cpara[0][1] = dot( trans[1][0], trans[1][1], trans[1][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] );
            rem1=Cpara[0*4+0]- cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0];//rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0];
            rem2 = Cpara[0*4+1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1];//rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1];
            rem3 = Cpara[0*4+2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];//rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];
            cpara[0][0]=norm(rem1, rem2, rem3);//cpara[0][0] = norm( rem1, rem2, rem3 );
            trans[0][0]=rem1 / cpara[0][0];//trans[0][0] = rem1 / cpara[0][0];
            trans[0][1]= rem2 / cpara[0][0];//trans[0][1] = rem2 / cpara[0][0];
            trans[0][2]= rem3 / cpara[0][0];//trans[0][2] = rem3 / cpara[0][0];
        
            trans[1][3]=(Cpara[1*4+3] - cpara[1][2]*trans[2][3]) / cpara[1][1];//trans[1][3] = (Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1];
            trans[0][3]=(Cpara[0*4+3] - cpara[0][1]*trans[1][3]- cpara[0][2]*trans[2][3]) / cpara[0][0];//trans[0][3] = (Cpara[0][3] - cpara[0][1]*trans[1][3]- cpara[0][2]*trans[2][3]) / cpara[0][0];
        
            for(int  r = 0; r < 3; r++ ){
                for(int c = 0; c < 3; c++ ){
            	    cpara[r][c]/=cpara[2][2];//cpara[r][c] /= cpara[2][2];
                }
            }
        }


        /*int    arParamDisp( ARParam *param );*/
        public int paramDisp()
        {
    	    Console.WriteLine("--------------------------------------");//printf("--------------------------------------\n");
    	    Console.WriteLine("SIZE = "+xsize+", "+ysize);//printf("SIZE = %d, %d\n", param->xsize, param->ysize);
    	    Console.WriteLine("Distortion factor = "+dist_factor[0]+" "+dist_factor[1]+" "+dist_factor[2]+" "+dist_factor[3]);//printf("Distortion factor = %f %f %f %f\n", param->dist_factor[0],param->dist_factor[1], param->dist_factor[2], param->dist_factor[3] );
    	    for(int j = 0; j < 3; j++ ) {//for(j = 0; j < 3; j++ ) {
    	        for(int i = 0; i < 4; i++ ){
    	    	    Console.Write(array34[j*4+i]+" ");//printf("%7.5f ", param->mat[j][i]);
    	        }
    	        Console.WriteLine();//    printf("\n");
    	    }//}
            Console.WriteLine("--------------------------------------");//printf("--------------------------------------\n");
            return 0;
        }
    //    /*int  arParamDecomp( ARParam *source, ARParam *icpara, double trans[3][4] );*/
    //    private static int arParamDecomp( NyARParam source, NyARParam icpara, double[][] trans)
    //    {
    //        icpara.xsize          = source.xsize;//icpara->xsize          = source->xsize;
    //        icpara.ysize          = source.ysize;//icpara->ysize          = source->ysize;
    //        icpara.dist_factor[0] = source.dist_factor[0];//icpara->dist_factor[0] = source->dist_factor[0];
    //        icpara.dist_factor[1] = source.dist_factor[1];// icpara->dist_factor[1] = source->dist_factor[1];
    //        icpara.dist_factor[2] = source.dist_factor[2];//icpara->dist_factor[2] = source->dist_factor[2];
    //        icpara.dist_factor[3] = source.dist_factor[3];//icpara->dist_factor[3] = source->dist_factor[3];
    //        return arParamDecompMat(source.mat, icpara.mat, trans );
    //    }
        /**
         * int arParamChangeSize( ARParam *source, int xsize, int ysize, ARParam *newparam );
         * 関数の代替関数
         * サイズプロパティをi_xsize,i_ysizeに変更します。
         * @param xsize
         * @param ysize
         * @param newparam
         * @return
         * 
         */
        public void changeSize(int i_xsize, int i_ysize)
        {
            double  scale;    
            scale = (double)i_xsize / (double)(xsize);//scale = (double)xsize / (double)(source->xsize);
        
            for(int i = 0; i < 4; i++ ) {
                array34[0*4+i]=array34[0*4+i]*scale;//newparam->mat[0][i] = source->mat[0][i] * scale;
                array34[1*4+i]=array34[1*4+i]*scale;//newparam->mat[1][i] = source->mat[1][i] * scale;
                array34[2*4+i]=array34[2*4+i];//newparam->mat[2][i] = source->mat[2][i];
            }
        
            dist_factor[0] = dist_factor[0] * scale;//newparam->dist_factor[0] = source->dist_factor[0] * scale;
            dist_factor[1] = dist_factor[1] * scale;//newparam->dist_factor[1] = source->dist_factor[1] * scale;
            dist_factor[2] = dist_factor[2] / (scale*scale);//newparam->dist_factor[2] = source->dist_factor[2] / (scale*scale);
            dist_factor[3] = dist_factor[3];//newparam->dist_factor[3] = source->dist_factor[3];
        
            xsize = i_xsize;//newparam->xsize = xsize;
            ysize = i_ysize;//newparam->ysize = ysize;
        }
        /**
         * int arParamIdeal2Observ( const double dist_factor[4], const double ix, const double iy,double *ox, double *oy )
         * 関数の代替関数
         * @param ix
         * @param iy
         * @param ox
         * @param oy
         */
        public void ideal2Observ(double ix,double iy,DoubleValue ox, DoubleValue oy)
        {

	        double    x, y, d;
	        double d0,d1,d3;
	        double[] df=this.dist_factor;
	        d0=df[0];
	        d1=df[1];
	        d3=df[3];
	        x = (ix - d0) * d3;
	        y = (iy - d1) * d3;
	        if( x == 0.0 && y == 0.0 ) {
	            ox.value=d0;
	            oy.value=d1;
	        }else{
	            d = 1.0 - df[2]/100000000.0 * (x*x+y*y);
	            ox.value=x * d + d0;
	            oy.value=y * d + d1;
	        }
        }
        /**
         * ideal2Observをまとめて実行します。
         * @param i_in
         * double[][2]
         * @param o_out
         * double[][2]
         */
        public void ideal2ObservBatch(double[,] i_in,double[,] o_out,int i_size)
        {

	        double    x, y, d;
	        double d0,d1,d3,d2_w;
	        double[] df=this.dist_factor;
	        d0=df[0];
	        d1=df[1];
	        d3=df[3];
	        d2_w=df[2]/100000000.0;
	        for(int i=0;i<i_size;i++){
                x = (i_in[i,0] - d0) * d3;
                y = (i_in[i,1] - d1) * d3;
                if( x == 0.0 && y == 0.0 ) {
    	        o_out[i,0]=d0;
    	        o_out[i,1]=d1;
                }else{
                    d = 1.0 - d2_w * (x*x+y*y);
                    o_out[i,0]=x * d + d0;
                    o_out[i,1]=y * d + d1;
                }
	        }
	        return;
        }    
        
        
        
        
        /**
         * int arParamObserv2Ideal( const double dist_factor[4], const double ox, const double oy,double *ix, double *iy );
         * 
         * @param ox
         * @param oy
         * @param ix
         * @param iy
         * @return
         */
        public int observ2Ideal(double ox,double oy,DoubleValue ix,DoubleValue iy)
        {
    	    double  z02, z0, p, q, z, px, py,opttmp_1;
	        double d0,d1,d3;
	        double[] df=this.dist_factor;
	        d0=df[0];
	        d1=df[1];
        	
    	    px = ox - d0;
    	    py = oy - d1;
    	    p = df[2]/100000000.0;
    	    z02 = px*px+py*py;
    	    q = z0 = Math.Sqrt(z02);//Optimize//q = z0 = Math.sqrt(px*px+ py*py);
        	
    	    for(int i = 1; ; i++ ) {
                if( z0 != 0.0 ) {
        	    //Optimize opttmp_1
        	    opttmp_1=p*z02;
                    z = z0 - ((1.0 - opttmp_1)*z0 - q) / (1.0 - 3.0*opttmp_1);
                    px = px*z/z0;
                    py = py*z/z0;
                }else {
                    px = 0.0;
                    py = 0.0;
                    break;
                }
                if( i == PD_LOOP ){
                    break;
                }
                z02 = px*px+ py*py;
                z0 = Math.Sqrt(z02);//Optimize//z0 = Math.sqrt(px*px+ py*py);
    	    }
    	    d3=df[3];
    	    ix.value=px / d3 + d0;
    	    iy.value=py / d3 + d1;
    	    return 0;
        }
        /**
         * 指定範囲のobserv2Idealをまとめて実行して、結果をo_idealに格納します。
         * @param i_x_coord
         * @param i_y_coord
         * @param i_start
         * coord開始点
         * @param i_num
         * 計算数
         * @param o_ideal
         * 出力バッファ[i_num][2]であること。
         */
        public void observ2IdealBatch(int[] i_x_coord,int[] i_y_coord,int i_start,int i_num,double[][] o_ideal)
        {
	        double  z02, z0,q, z, px, py,opttmp_1;
	        double[] df=this.dist_factor;
	        double d0=df[0];
	        double d1=df[1];
	        double d3=df[3];
	        double p = df[2]/100000000.0;
	        for(int j = 0; j < i_num; j++ ){

	            px = i_x_coord[i_start+j] - d0;
	            py = i_y_coord[i_start+j] - d1;

	            z02 = px*px+py*py;
	            q = z0 = Math.Sqrt(z02);//Optimize//q = z0 = Math.sqrt(px*px+ py*py);

	            for(int i = 1; ; i++ ) {
		            if( z0 != 0.0 ) {
		                //Optimize opttmp_1
		                opttmp_1=p*z02;
		                z = z0 - ((1.0 - opttmp_1)*z0 - q) / (1.0 - 3.0*opttmp_1);
		                px = px*z/z0;
		                py = py*z/z0;
		            }else {
		                px = 0.0;
		                py = 0.0;
		                break;
		            }
		            if( i == PD_LOOP ){
		                break;
		            }
		            z02 = px*px+ py*py;
		            z0 = Math.Sqrt(z02);//Optimize//z0 = Math.sqrt(px*px+ py*py);
	            }
	            o_ideal[j][0]=px / d3 + d0;
	            o_ideal[j][1]=py / d3 + d1;
            }
        }

        private static int endianConv(int i_val)
        {
            if (!BitConverter.IsLittleEndian)
            {
                return i_val;
            }
            byte[] ba = BitConverter.GetBytes(i_val);
            Array.Reverse(ba);
            return BitConverter.ToInt32(ba,0);
        }
        private static double endianConv(double i_val)
        {
            if (!BitConverter.IsLittleEndian)
            {
                return i_val;
            }
            byte[] ba = BitConverter.GetBytes(i_val);
            Array.Reverse(ba);
            return BitConverter.ToDouble(ba, 0);
        }        
        
        /**
         * int    arParamLoad( const char *filename, int num, ARParam *param, ...);
         * i_streamの入力ストリームからi_num個の設定を読み込み、パラメタを配列にして返します。
         * @param filename
         * @param num
         * @param param
         * @return
         * 	設定を格納した配列を返します。
         * @throws Exception
         * i_num個の設定が読み出せない場合、JartkExceptionを発生します。
         */
        private static NyARParam[] arParamLoad(Stream i_stream, int i_num)
        {
            try{
                int read_size=SIZE_OF_PARAM_SET*i_num;
                byte[] buf=new byte[read_size];
                BinaryReader br=new BinaryReader(i_stream);
                
//                //返却配列を確保
                NyARParam[] result=new NyARParam[i_num];
                
                //バッファを加工
//                ByteBuffer bb = ByteBuffer.wrap(buf);
//                bb.order(ByteOrder.BIG_ENDIAN);

                //固定回数パースして配列に格納
                for(int i=0;i<i_num;i++){
                    NyARParam new_param=new NyARParam();;
                    new_param.xsize = endianConv(br.ReadInt32());
                    new_param.ysize = endianConv(br.ReadInt32());
                    for(int i2=0;i2<3;i2++){
                        for(int i3=0;i3<4;i3++){
                            new_param.array34[i2 * 4 + i3] = endianConv(br.ReadDouble());
                        }
                    }
    		        for(int i2=0;i2<4;i2++){
                        new_param.dist_factor[i2] = endianConv(br.ReadDouble());
    		        }
    		        result[i]=new_param;
                }
                return result;
            }catch(Exception e){
                throw new NyARException(e);
            }
        }
/*        public static int arParamSave(String filename,int num, NyARParam[] param)
        {
	        NyARException.trap("未チェックの関数");
	        byte[] buf=new byte[SIZE_OF_PARAM_SET*param.Length];
	        //バッファをラップ
   	        ByteBuffer bb = ByteBuffer.wrap(buf);
	        bb.order(ByteOrder.BIG_ENDIAN);

	        //書き込み
	        for(int i=0;i<param.Length;i++){
                bb.putInt(param[i].xsize);
                bb.putInt(param[i].ysize);
                for(int i2=0;i2<3;i2++){
                    for(int i3=0;i3<4;i3++){
                        bb.putDouble(param[i].array34[i2*4+i3]);
                    }
                }
                for(int i2=0;i2<4;i2++){
                    bb.putDouble(param[i].dist_factor[i2]);
                }
            }
	        //ファイルに保存
	        FileOutputStream fs=new FileOutputStream(filename);
	        fs.write(buf);
	        fs.close();

	        return 0;
        }*/
    }

}
