/*
 *    Geotools2 - OpenSource mapping toolkit
 *    http://geotools.org
 *    (C) 2002-2006, Geotools Project Managment Committee (PMC)
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License as published by the Free Software Foundation;
 *    version 2.1 of the License.
 *
 *    This library 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
 *    Lesser General Public License for more details.
 */
package org.geotools.referencing.operation.builder;

import org.geotools.referencing.datum.BursaWolfParameters;
import org.geotools.referencing.operation.matrix.GeneralMatrix;
import org.geotools.referencing.operation.transform.GeocentricTranslation;
import org.opengis.referencing.FactoryException;
import org.opengis.referencing.cs.CartesianCS;
import org.opengis.referencing.datum.GeodeticDatum;
import org.opengis.referencing.operation.MathTransform;
import org.opengis.geometry.DirectPosition;
import org.opengis.geometry.MismatchedDimensionException;
import org.opengis.geometry.MismatchedReferenceSystemException;

// J2SE and extensions
import java.util.List;
import javax.vecmath.MismatchedSizeException;


/**
 * Builds {@linkplain org.opengis.referencing.operation.MathTransform
 * MathTransform} setup as BursaWolf transformation from a list of {@linkplain
 * org.geotools.referencing.operation.builder.MappedPosition MappedPosition}.
 * The calculation uses least square method.  Calculated parameters can be
 * used for following operations:<p></p>
 *  <p>The equations:<pre> X = q * R * x  +  T ,             </pre>Where X
 * is the Matrix of destination points, q is the scale,  R is the rotation
 * Matrix, x is the Matrix of source points  and T is matrix of translation.
 * Expressing the errors, we get this:<pre>        Err =  A * Dx + l </pre>where
 * Err is the Error Matrix, A is Matrix of derivations, Dx is Matrix  of
 * difference changes of 7 parameters, and l is value of DX, DY, DZ for
 * calculated from approximate values. Using the least square method to
 * minimalize the errors we get this result:<pre>
 *  Dx = (A<sup>T</sup>A)<sup>-1</sup> A<sup>T</sup>l  </pre></p>
 *
 * @since 2.4
 * @source $URL: http://svn.geotools.org/geotools/tags/2.4.1/modules/library/referencing/src/main/java/org/geotools/referencing/operation/builder/BursaWolfTransformBuilder.java $
 * @version $Id: BursaWolfTransformBuilder.java 24925 2007-03-27 20:12:08Z jgarnett $
 * @author Jan Jezek
 *
 */
public class BursaWolfTransformBuilder extends MathTransformBuilder {
    /** The Geodetic Datum of target reference system */
    private GeodeticDatum targetDatum;

    /** Matrix of source points */
    GeneralMatrix x;

    /** Matrix of destination points. */
    GeneralMatrix X;

    /** Bursa Wolf rotation in arc radians. */
    private double alfa = 0;

    /** Bursa Wolf rotation in arc radians. */
    private double beta = 0;

    /** Bursa Wolf rotation in arc radians. */
    private double gamma = 0;

    /** Bursa Wolf shift in meters. */
    private double dx = 0;

    /** Bursa Wolf shift in meters. */
    private double dy = 0;

    /** Bursa Wolf shift in meters. */
    private double dz = 0;

    /** Bursa Wolf scaling. */
    private double q = 1;

/**
     * Creates a BursaWolfTransformBuilder.
     * 
     * @param vectors list of {@linkplain
     * org.geotools.referencing.operation.builder.MappedPosition MappedPosition}
     */
    public BursaWolfTransformBuilder(List /*<MappedPosition>*/ vectors)
        throws MismatchedSizeException, MismatchedDimensionException,
            MismatchedReferenceSystemException {
        super.setMappedPositions(vectors);

        x = new GeneralMatrix(vectors.size(), 3);
        X = new GeneralMatrix(vectors.size(), 3);
        x = getx();
        X = getX();
        this.getDxMatrix();
    }

    /**
     * Returns the minimum number of points required by this builder,
     * which is 3.
     *
     * @return the minimum number of points required by this builder which is
     *         3.
     */
    public int getMinimumPointCount() {
        return 3;
    }

    /**
     * Returns the dimension for {@link #getSourceCRS source} and
     * {@link #getTargetCRS target} CRS, which is 2.
     *
     * @return dimension for {@linkplain #getSourceCRS source} and {@link
     *         #getTargetCRS target} CRS, which is 2.
     */
    public int getDimension() {
        return 3;
    }

    /**
     * Returns the required coordinate system type, which is
     * {@linkplain CartesianCS cartesian CS}.
     *
     * @return coordinate system type
     */
    public Class /*<? extends CartesianCS>*/ getCoordinateSystemType() {
        return CartesianCS.class;
    }

    /**
     * Fills the x matrix by coordinates of source points.
     *
     * @return x matrix.
     */
    protected GeneralMatrix getx() {
        final DirectPosition[] sourcePoints = getSourcePoints();
        GeneralMatrix x = new GeneralMatrix(3 * sourcePoints.length, 1);

        for (int j = 0; j < (x.getNumRow()); j = j + 3) {
            x.setElement(j, 0, sourcePoints[j / 3].getCoordinates()[0]);
            x.setElement(j + 1, 0, sourcePoints[j / 3].getCoordinates()[1]);
            x.setElement(j + 2, 0, sourcePoints[j / 3].getCoordinates()[2]);
        }

        return x;
    }

    /**
     * Fills the x matrix by  coordinates of destination points.
     *
     * @return the X matrix
     */
    protected GeneralMatrix getX() {
        final DirectPosition[] sourcePoints = getSourcePoints();
        final DirectPosition[] targetPoints = getTargetPoints();
        GeneralMatrix X = new GeneralMatrix(3 * sourcePoints.length, 1);

        for (int j = 0; j < (X.getNumRow()); j = j + 3) {
            X.setElement(j, 0, targetPoints[j / 3].getCoordinates()[0]);
            X.setElement(j + 1, 0, targetPoints[j / 3].getCoordinates()[1]);
            X.setElement(j + 2, 0, targetPoints[j / 3].getCoordinates()[2]);
        }

        return X;
    }

    /**
     * Generates rotation matrix around X axis.
     *
     * @return rotation Matrix
     */
    protected GeneralMatrix getRalfa() {
        GeneralMatrix Ralfa = new GeneralMatrix(3, 3);
        double[] m0 = { 1, 0, 0 };
        double[] m1 = { 0, Math.cos(alfa), Math.sin(alfa) };
        double[] m2 = { 0, -Math.sin(alfa), Math.cos(alfa) };
        Ralfa.setRow(0, m0);
        Ralfa.setRow(1, m1);
        Ralfa.setRow(2, m2);

        return Ralfa;
    }

    /**
     * Generates rotation matrix around Y axis.
     *
     * @return rotation Matrix.
     */
    protected GeneralMatrix getRbeta() {
        GeneralMatrix Rbeta = new GeneralMatrix(3, 3);
        double[] m0 = { Math.cos(beta), 0, -Math.sin(beta) };
        double[] m1 = { 0, 1, 0 };
        double[] m2 = { Math.sin(beta), 0, Math.cos(beta) };
        Rbeta.setRow(0, m0);
        Rbeta.setRow(1, m1);
        Rbeta.setRow(2, m2);

        return Rbeta;
    }

    /**
     * Generates rotation matrix around Z axis.
     *
     * @return rotation Matrix.
     */
    protected GeneralMatrix getRgamma() {
        GeneralMatrix Rgamma = new GeneralMatrix(3, 3);
        double[] m0 = { Math.cos(gamma), Math.sin(gamma), 0 };
        double[] m1 = { -Math.sin(gamma), Math.cos(gamma), 0 };
        double[] m2 = { 0, 0, 1 };
        Rgamma.setRow(0, m0);
        Rgamma.setRow(1, m1);
        Rgamma.setRow(2, m2);

        return Rgamma;
    }

    /**
     * Generates partial derivative with respect to alfa.
     *
     * @return Matrix, that represents partial derivation of rotation Matrix
     *         with respect to  alfa.
     */
    protected GeneralMatrix getDRalfa() {
        GeneralMatrix dRa = new GeneralMatrix(3, 3);

        double[] m0 = { 0, 0, 0 };
        double[] m1 = { 0, -Math.sin(alfa), Math.cos(alfa) };
        double[] m2 = { 0, -Math.cos(alfa), -Math.sin(alfa) };

        dRa.setRow(0, m0);
        dRa.setRow(1, m1);
        dRa.setRow(2, m2);

        dRa.mul(dRa, getRbeta());
        dRa.mul(dRa, getRgamma());

        return specialMul(dRa, x);
    }

    /**
     * Generates partial derivative with respect to beta.
     *
     * @return Matrix, that represents partial derivation of rotation Matrix
     *         with respect to  beta.
     */
    protected GeneralMatrix getDRbeta() {
        //GeneralMatrix dRbeta = new GeneralMatrix(3 * sourcePoints.size(), 1);  
        GeneralMatrix dRb = new GeneralMatrix(3, 3);
        double[] m0 = { -Math.sin(beta), 0, -Math.cos(beta) };
        double[] m1 = { 0, 0, 0 };
        double[] m2 = { Math.cos(beta), 0, -Math.sin(beta) };
        dRb.setRow(0, m0);
        dRb.setRow(1, m1);
        dRb.setRow(2, m2);

        dRb.mul(getRalfa(), dRb);
        dRb.mul(dRb, getRgamma());

        return specialMul(dRb, x);
    }

    /**
     * Generates partial derivative with respect to gamma.
     *
     * @return Matrix, that represents partial derivation of rotation Matrix
     *         with respect to  gamma.
     */
    protected GeneralMatrix getDRgamma() {
        //	GeneralMatrix dRgamma = new GeneralMatrix(3 * sourcePoints.size(), 1);
        GeneralMatrix dRg = new GeneralMatrix(3, 3);
        GeneralMatrix pom = new GeneralMatrix(3, 3);
        double[] m0 = { -Math.sin(gamma), Math.cos(gamma), 0 };
        double[] m1 = { -Math.cos(gamma), -Math.sin(gamma), 0 };
        double[] m2 = { 0, 0, 0 };
        dRg.setRow(0, m0);
        dRg.setRow(1, m1);
        dRg.setRow(2, m2);

        pom.mul(getRalfa(), getRbeta());
        dRg.mul(pom, dRg);

        return specialMul(dRg, x);
    }

    /**
     * Generates partial derivative in q (scale factor).
     *
     * @return rotation Matrix.
     */
    protected GeneralMatrix getDq() {
        //	GeneralMatrix Dq = new GeneralMatrix(3 * sourcePoints.size(), 1);
        GeneralMatrix R = new GeneralMatrix(3, 3);
        R.mul(getRalfa(), getRbeta());
        R.mul(R, getRgamma());

        return specialMul(R, x);
    }

    /**
     * Calculates the matrix of errors from aproximate values of
     * prameters.
     *
     * @return the l matrix.
     */
    protected GeneralMatrix getl() {
        GeneralMatrix l = new GeneralMatrix(3 * getMappedPositions().size(), 1);
        GeneralMatrix R = new GeneralMatrix(3, 3);
        GeneralMatrix T = new GeneralMatrix(3, 1, new double[] { -dx, -dy, -dz });
        GeneralMatrix qMatrix = new GeneralMatrix(1, 1, new double[] { q });
        GeneralMatrix qx = new GeneralMatrix(X.getNumRow(), X.getNumCol());
        qx.mul(x, qMatrix);
        R.mul(getRalfa(), getRbeta());
        R.mul(getRgamma());

        l.sub(specialMul(R, qx), X);
        l = specialSub(T, l);

        return l;
    }

    /**
     * Method for multiplying  matrix (3,3) by matrix of  coordintes (3
     * number of coordinates,1)
     *
     * @param R ratrix
     * @param x matrix
     *
     * @return matrix
     */
    protected GeneralMatrix specialMul(GeneralMatrix R, GeneralMatrix x) {
        GeneralMatrix dRx = new GeneralMatrix(3 * getMappedPositions().size(), 1);

        for (int i = 0; i < x.getNumRow(); i = i + 3) {
            GeneralMatrix subMatrix = new GeneralMatrix(3, 1);
            x.copySubMatrix(i, 0, 3, 1, 0, 0, subMatrix);
            subMatrix.mul(R, subMatrix);
            subMatrix.copySubMatrix(0, 0, 3, 1, i, 0, dRx);
        }

        return dRx;
    }

    /**
     * Method for addition matrix (3,3) with matrix of coordintes (3
     * number of coordinates,1)
     *
     * @param R ratrix
     * @param x matrix
     *
     * @return matrix
     */
    private GeneralMatrix specialSub(GeneralMatrix R, GeneralMatrix x) {
        GeneralMatrix dRx = new GeneralMatrix(3 * getMappedPositions().size(), 1);

        for (int i = 0; i < x.getNumRow(); i = i + 3) {
            GeneralMatrix subMatrix = new GeneralMatrix(3, 1);
            x.copySubMatrix(i, 0, 3, 1, 0, 0, subMatrix);
            subMatrix.sub(R, subMatrix);
            subMatrix.copySubMatrix(0, 0, 3, 1, i, 0, dRx);
        }

        return dRx;
    }

    /**
     * Glues the submatrix of derivations into the A matrix.
     *
     * @return A mtarix
     */
    protected GeneralMatrix getA() {
        final int size = getMappedPositions().size();
        GeneralMatrix A = new GeneralMatrix(3 * size, 7);
        GeneralMatrix DT = new GeneralMatrix(3, 3);

        // the partial derivative with respect to dx,dy,dz. 
        double[] m0 = { 1, 0, 0 };
        double[] m1 = { 0, 1, 0 };
        double[] m2 = { 0, 0, 1 };
        DT.setRow(0, m0);
        DT.setRow(1, m1);
        DT.setRow(2, m2);

        for (int i = 0; i < A.getNumRow(); i = i + 3) {
            DT.copySubMatrix(0, 0, 3, 3, i, 0, A);
        }

        getDRalfa().copySubMatrix(0, 0, 3 * size, 1, 0, 3, A);
        getDRbeta().copySubMatrix(0, 0, 3 * size, 1, 0, 4, A);
        getDRgamma().copySubMatrix(0, 0, 3 * size, 1, 0, 5, A);
        getDq().copySubMatrix(0, 0, 3 * size, 1, 0, 6, A);

        return A;
    }

    /**
     * Returns array of doubles of transformation parameters (dx, dy,
     * dz, ex, ey, ez, scale).
     *
     * @return array of doubles of transformation parameters (dx, dy, dz, ex,
     *         ey, ez, scale).
     */
    protected double[] getParameters() {
        return getDxMatrix().getElements()[0];
    }

    /**
     * Method that claculates the parameters by iteration. The
     * tolarance is set to 1  10<sub>-8</sub>  and max �number of steps is set
     * to 20.
     *
     * @return Matrix of parameters (dx, dy, dz, ex, ey, ez, scale).
     */
    public GeneralMatrix getDxMatrix() {
        return getDxMatrix(0.00000001, 20);
    }

    /**
     * Iterates the parameters..
     *
     * @param tolerance for iteration steps (the max difference between last
     *        two steps)
     * @param maxSteps highest number of iteations.
     *
     * @return GeneralMatrix of calculated parameters.
     */
    private GeneralMatrix getDxMatrix(double tolerance, int maxSteps) {
        // Matriix of new calculated coefficeients
        GeneralMatrix xNew = new GeneralMatrix(7, 1);

        // Matrix of coefficients claculated in previous iteration
        GeneralMatrix xOld = new GeneralMatrix(7, 1);

        // diference between each steps of old iteration    	
        GeneralMatrix dxMatrix = new GeneralMatrix(7, 1);

        GeneralMatrix zero = new GeneralMatrix(7, 1);
        zero.setZero();

        // i is a number of iterations
        int i = 0;

        // cicle for iteration
        do {
            xOld.set(new double[] { dx, dy, dz, alfa, beta, gamma, q });

            GeneralMatrix A = getA();
            GeneralMatrix l = getl();

            GeneralMatrix AT = (GeneralMatrix) A.clone();
            AT.transpose();

            GeneralMatrix ATA = new GeneralMatrix(7, 7);
            GeneralMatrix ATl = new GeneralMatrix(7, 1);

            // dx = A^T * A  * A^T * l    	
            ATA.mul(AT, A);
            ATA.invert();
            ATl.mul(AT, l);

            dxMatrix.mul(ATA, ATl);

            // New values of x = dx + previous values
            xOld.negate();
            xNew.sub(dxMatrix, xOld);

            // New values are setup for another iteration
            dx = xNew.getElement(0, 0);
            dy = xNew.getElement(1, 0);
            dz = xNew.getElement(2, 0);
            alfa = xNew.getElement(3, 0);
            beta = xNew.getElement(4, 0);
            gamma = xNew.getElement(5, 0);
            q = xNew.getElement(6, 0);

            i++;
        } while ((!dxMatrix.epsilonEquals(zero, tolerance) & (i < maxSteps)));

        xNew.transpose();

        return xNew;
    }

    /**
     * Coverts radians to seconds.
     *
     * @param rad Angle in radians
     *
     * @return Angle is seconds
     */
    private static double radiansToSeconds(double rad) {
        return (rad * (180 / Math.PI) * (3600));
    }

    /**
     * Returns Bursa Wolf Transformation parameters.
     *
     * @param Datum The target datum for this parameters.
     *
     * @return parameters the BursaWolfParameters
     */
    public BursaWolfParameters getBursaWolfParameters(GeodeticDatum Datum) {
        BursaWolfParameters parameters = new BursaWolfParameters(Datum);
        parameters.dx = dx;
        parameters.dy = dy;
        parameters.dz = dz;
        parameters.ex = -radiansToSeconds(alfa);
        parameters.ey = -radiansToSeconds(beta);
        parameters.ez = -radiansToSeconds(gamma);
        parameters.ppm = (q - 1) * 1000000;

        return parameters;
    }

    public void setTargetGeodeticDatum(GeodeticDatum gd) {
        this.targetDatum = gd;
    }

    /**
     * Returns MathtTransform setup as BursaWolf transformation.
     *
     * @return calculated MathTransform
     *
     * @throws FactoryException when the size of source and destination point
     *         is not the same or if the number of points is too small to
     *         define such transformation.
     */
    protected MathTransform computeMathTransform() throws FactoryException {
        return new GeocentricTranslation(getBursaWolfParameters(targetDatum));
    }
}
