/* 
 * 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 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>
 * 
 */
using System;
namespace jp.nyatla.nyartoolkit.cs.core
{


    /**
     * このクラスは、OpenCV distortion modelの樽型歪み設定/解除クラスです。
     */
    public class NyARCameraDistortionFactorV4 : INyARCameraDistortionFactor
    {
        public const int NUM_OF_FACTOR = 9;
        private double _k1;
        private double _k2;
        private double _p1;
        private double _p2;
        private double _fx;
        private double _fy;
        private double _x0;
        private double _y0;
        private double _s;
        private double getSizeFactor(double x0, double y0, int xsize, int ysize)
        {
            double ox, oy;
            double olen, ilen;
            double sf1;

            double sf = 100.0;

            ox = 0.0;
            oy = y0;
            olen = x0;
            NyARDoublePoint2d itmp = new NyARDoublePoint2d();
            this.observ2Ideal(ox, oy, itmp);
            ilen = x0 - itmp.x;
            //printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf) sf = sf1;
            }

            ox = xsize;
            oy = y0;
            olen = xsize - x0;
            this.observ2Ideal(ox, oy, itmp);
            ilen = itmp.x - x0;
            //printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf) sf = sf1;
            }

            ox = x0;
            oy = 0.0;
            olen = y0;
            this.observ2Ideal(ox, oy, itmp);
            ilen = y0 - itmp.y;
            //printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf) sf = sf1;
            }

            ox = x0;
            oy = ysize;
            olen = ysize - y0;
            this.observ2Ideal(ox, oy, itmp);
            ilen = itmp.y - y0;
            //printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf) sf = sf1;
            }


            ox = 0.0;
            oy = 0.0;
            this.observ2Ideal(ox, oy, itmp);
            ilen = x0 - itmp.x;
            olen = x0;
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf) sf = sf1;
            }
            ilen = y0 - itmp.y;
            olen = y0;
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf) sf = sf1;
            }

            ox = xsize;
            oy = 0.0;
            this.observ2Ideal(ox, oy, itmp);
            ilen = itmp.x - x0;
            olen = xsize - x0;
            //printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf) sf = sf1;
            }
            ilen = y0 - itmp.y;
            olen = y0;
            //printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf) sf = sf1;
            }

            ox = 0.0;
            oy = ysize;
            this.observ2Ideal(ox, oy, itmp);
            ilen = x0 - itmp.x;
            olen = x0;
            //printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf) sf = sf1;
            }
            ilen = itmp.y - y0;
            olen = ysize - y0;
            //printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf) sf = sf1;
            }

            ox = xsize;
            oy = ysize;
            this.observ2Ideal(ox, oy, itmp);
            ilen = itmp.x - x0;
            olen = xsize - x0;
            //printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf) sf = sf1;
            }
            ilen = itmp.y - y0;
            olen = ysize - y0;
            //printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
            if (ilen > 0)
            {
                sf1 = ilen / olen;
                if (sf1 < sf)
                {
                    sf = sf1;
                }
            }

            if (sf == 100.0)
            {
                sf = 1.0;
            }

            return sf;
        }

        public double getS()
        {
            return this._s;
        }       /**
         * この関数は、参照元から歪みパラメータ値をコピーします。
         * @param i_ref
         * コピー元のオブジェクト。
         */
        public void copyFrom(INyARCameraDistortionFactor i_ref)
        {
            NyARCameraDistortionFactorV4 src = (NyARCameraDistortionFactorV4)i_ref;
            this._k1 = src._k1;
            this._k2 = src._k2;
            this._p1 = src._p1;
            this._p2 = src._p2;
            this._fx = src._fx;
            this._fy = src._fy;
            this._x0 = src._x0;
            this._y0 = src._y0;
            this._s = src._s;
        }
        /**
         * @param i_size
         * @param i_intrinsic_matrix
         * 3x3 matrix
         * このパラメータは、OpenCVのcvCalibrateCamera2関数が出力するintrinsic_matrixの値と合致します。
         * @param i_distortion_coeffs
         * 4x1 vector
         * このパラメータは、OpenCVのcvCalibrateCamera2関数が出力するdistortion_coeffsの値と合致します。
         */
        public void setValue(NyARIntSize i_size, double[] i_intrinsic_matrix, double[] i_distortion_coeffs)
        {
            this._k1 = i_distortion_coeffs[0];
            this._k2 = i_distortion_coeffs[1];
            this._p1 = i_distortion_coeffs[2];
            this._p2 = i_distortion_coeffs[3];
            this._fx = i_intrinsic_matrix[0 * 3 + 0];//0,0
            this._fy = i_intrinsic_matrix[1 * 3 + 1];//1,1
            this._x0 = i_intrinsic_matrix[0 * 3 + 2];//0,2
            this._y0 = i_intrinsic_matrix[1 * 3 + 2];//1,2
            this._s = 1.0;
            //update s
            this._s = this.getSizeFactor(this._x0, this._y0, i_size.w, i_size.h);
        }
        /**
         * この関数は、配列の値を歪みパラメータ値として、このインスタンスにセットします。
         * @param i_factor
         * 歪みパラメータ値を格納した配列。
         */
        public void setValue(double[] i_factor)
        {
            this._k1 = i_factor[0];
            this._k2 = i_factor[1];
            this._p1 = i_factor[2];
            this._p2 = i_factor[3];
            this._fx = i_factor[4];
            this._fy = i_factor[5];
            this._x0 = i_factor[6];
            this._y0 = i_factor[7];
            this._s = i_factor[8];
        }

        /**
         * この関数は、パラメータ値を配列へ返します。
         * o_factorには要素数{@link #NUM_OF_FACTOR}の
         * @param o_factor
         * 歪みパラメータ値の出力先配列。
         */
        public void getValue(double[] o_factor)
        {
            o_factor[0] = this._k1;
            o_factor[1] = this._k2;
            o_factor[2] = this._p1;
            o_factor[3] = this._p2;
            o_factor[4] = this._fx;
            o_factor[5] = this._fy;
            o_factor[6] = this._x0;
            o_factor[7] = this._y0;
            o_factor[8] = this._s;
        }

        /**
         */
        public void changeScale(double i_x_scale, double i_y_scale)
        {
            this._fx = this._fx * i_x_scale;   /*  fx  */
            this._fy = this._fy * i_y_scale;   /*  fy  */
            this._x0 = this._x0 * i_x_scale;   /*  x0  */
            this._y0 = this._y0 * i_y_scale;   /*  y0  */
            return;
        }

        public void ideal2Observ(NyARDoublePoint2d i_in, NyARDoublePoint2d o_out)
        {
            this.ideal2Observ(i_in.x, i_in.y, o_out);
        }

        public void ideal2Observ(NyARDoublePoint2d i_in, NyARIntPoint2d o_out)
        {
            this.ideal2Observ(i_in.x, i_in.y, o_out);
        }

        /**
         * この関数は、座標点を理想座標系から観察座標系へ変換します。
         * @param i_in
         * 変換元の座標
         * @param o_out
         * 変換後の座標を受け取るオブジェクト
         */
        public void ideal2Observ(double i_x, double i_y, NyARDoublePoint2d o_out)
        {
            double k1 = this._k1;
            double k2 = this._k2;
            double p1 = this._p1;
            double p2 = this._p2;
            double fx = this._fx;
            double fy = this._fy;
            double x0 = this._x0;
            double y0 = this._y0;
            double s = this._s;

            double x = (i_x - x0) * s / fx;
            double y = (i_y - y0) * s / fy;
            double l = x * x + y * y;
            o_out.x = (x * (1.0 + k1 * l + k2 * l * l) + 2.0 * p1 * x * y + p2 * (l + 2.0 * x * x)) * fx + x0;
            o_out.y = (y * (1.0 + k1 * l + k2 * l * l) + p1 * (l + 2.0 * y * y) + 2.0 * p2 * x * y) * fy + y0;
        }


        public void ideal2Observ(double i_x, double i_y, NyARIntPoint2d o_out)
        {
            double k1 = this._k1;
            double k2 = this._k2;
            double p1 = this._p1;
            double p2 = this._p2;
            double fx = this._fx;
            double fy = this._fy;
            double x0 = this._x0;
            double y0 = this._y0;
            double s = this._s;

            double x = (i_x - x0) * s / fx;
            double y = (i_y - y0) * s / fy;
            double l = x * x + y * y;
            o_out.x = (int)((x * (1.0 + k1 * l + k2 * l * l) + 2.0 * p1 * x * y + p2 * (l + 2.0 * x * x)) * fx + x0);
            o_out.y = (int)((y * (1.0 + k1 * l + k2 * l * l) + p1 * (l + 2.0 * y * y) + 2.0 * p2 * x * y) * fy + y0);
        }

        /**
         * この関数は、複数の座標点を、一括して理想座標系から観察座標系へ変換します。
         * i_inとo_outには、同じインスタンスを指定できます。
         * @param i_in
         * 変換元の座標配列
         * @param o_out
         * 変換後の座標を受け取る配列
         * @param i_size
         * 変換する座標の個数。
         * @todo should optimize!
         */
        public void ideal2ObservBatch(NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] o_out, int i_size)
        {
            double k1 = this._k1;
            double k2 = this._k2;
            double p1 = this._p1;
            double p2 = this._p2;
            double fx = this._fx;
            double fy = this._fy;
            double x0 = this._x0;
            double y0 = this._y0;
            double s = this._s;

            for (int i = 0; i < i_size; i++)
            {
                double x = (i_in[i].x - x0) * s / fx;
                double y = (i_in[i].y - y0) * s / fy;
                double l = x * x + y * y;
                o_out[i].x = ((x * (1.0 + k1 * l + k2 * l * l) + 2.0 * p1 * x * y + p2 * (l + 2.0 * x * x)) * fx + x0);
                o_out[i].y = ((y * (1.0 + k1 * l + k2 * l * l) + p1 * (l + 2.0 * y * y) + 2.0 * p2 * x * y) * fy + y0);
            }
            return;
        }

        /**
         * この関数は、複数の座標点を、一括して理想座標系から観察座標系へ変換します。
         * i_inとo_outには、同じインスタンスを指定できます。
         * @param i_in
         * 変換元の座標配列
         * @param o_out
         * 変換後の座標を受け取る配列
         * @param i_size
         * 変換する座標の個数。
         * @todo should optimize!
         */
        public void ideal2ObservBatch(NyARDoublePoint2d[] i_in, NyARIntPoint2d[] o_out, int i_size)
        {
            double k1 = this._k1;
            double k2 = this._k2;
            double p1 = this._p1;
            double p2 = this._p2;
            double fx = this._fx;
            double fy = this._fy;
            double x0 = this._x0;
            double y0 = this._y0;
            double s = this._s;

            for (int i = 0; i < i_size; i++)
            {
                double x = (i_in[i].x - x0) * s / fx;
                double y = (i_in[i].y - y0) * s / fy;
                double l = x * x + y * y;
                o_out[i].x = (int)((x * (1.0 + k1 * l + k2 * l * l) + 2.0 * p1 * x * y + p2 * (l + 2.0 * x * x)) * fx + x0);
                o_out[i].y = (int)((y * (1.0 + k1 * l + k2 * l * l) + p1 * (l + 2.0 * y * y) + 2.0 * p2 * x * y) * fy + y0);
            }
            return;
        }
        private const int PD_LOOP2 = 4;
        /**
         * この関数は、座標を観察座標系から理想座標系へ変換します。
         * @param ix
         * 変換元の座標
         * @param iy
         * 変換元の座標
         * @param o_point
         * 変換後の座標を受け取るオブジェクト
         * @todo should optimize!
         */
        public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point)
        {
            // OpenCV distortion model, with addition of a scale factor so that
            // entire image fits onscreen.

            double k1 = this._k1;
            double k2 = this._k2;
            double p1 = this._p1;
            double p2 = this._p2;
            double fx = this._fx;
            double fy = this._fy;
            double x0 = this._x0;
            double y0 = this._y0;

            double px = (ix - x0) / fx;
            double py = (iy - y0) / fy;

            double x02 = px * px;
            double y02 = py * py;

            for (int i = 1; ; i++)
            {
                if (x02 != 0.0 || y02 != 0.0)
                {
                    px = px - ((1.0 + k1 * (x02 + y02) + k2 * (x02 + y02) * (x02 + y02)) * px + 2.0 * p1 * px * py + p2 * (x02 + y02 + 2.0 * x02) - ((ix - x0) / fx)) / (1.0 + k1 * (3.0 * x02 + y02) + k2 * (5.0 * x02 * x02 + 3.0 * x02 * y02 + y02 * y02) + 2.0 * p1 * py + 6.0 * p2 * px);
                    py = py - ((1.0 + k1 * (x02 + y02) + k2 * (x02 + y02) * (x02 + y02)) * py + p1 * (x02 + y02 + 2.0 * y02) + 2.0 * p2 * px * py - ((iy - y0) / fy)) / (1.0 + k1 * (x02 + 3.0 * y02) + k2 * (x02 * x02 + 3.0 * x02 * y02 + 5.0 * y02 * y02) + 6.0 * p1 * py + 2.0 * p2 * px);
                }
                else
                {
                    px = 0.0;
                    py = 0.0;
                    break;
                }
                if (i == PD_LOOP2) break;

                x02 = px * px;
                y02 = py * py;
            }

            o_point.x = px * fx / this._s + x0;
            o_point.y = py * fy / this._s + y0;

            return;
        }

        public void observ2Ideal(NyARDoublePoint2d i_in, NyARDoublePoint2d o_point)
        {
            this.observ2Ideal(i_in.x, i_in.y, o_point);
        }

        /**
         * 座標配列全てに対して、{@link #observ2Ideal(double, double, NyARDoublePoint2d)}を適応します。
         * @param i_in
         * @param o_out
         * @param i_size
         */
        public void observ2IdealBatch(NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] o_out, int i_size)
        {
            for (int i = i_size - 1; i >= 0; i--)
            {
                this.observ2Ideal(i_in[i].x, i_in[i].y, o_out[i]);
            }
            return;
        }


    }
}