/**
 * Plane -- A concrete 2D plane in nD space.
 *
 * Copyright (C) 2009-2016, Joseph A. Huwaldt. All rights reserved.
 *
 * 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;
 * either version 2.1 of the License, or (at your option) any later version.
 *
 * 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.
 *
 * You should have received a copy of the GNU Lesser General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place -
 * Suite 330, Boston, MA 02111-1307, USA. Or visit: http://www.gnu.org/licenses/lgpl.html
 */
package geomss.geom;

import jahuwaldt.js.param.Parameter;
import java.text.MessageFormat;
import java.util.List;
import java.util.Objects;
import static java.util.Objects.requireNonNull;
import javax.measure.converter.ConversionException;
import javax.measure.quantity.Dimensionless;
import javax.measure.quantity.Length;
import javax.measure.unit.Unit;
import javolution.context.ImmortalContext;
import javolution.context.ObjectFactory;
import javolution.lang.MathLib;
import javolution.lang.ValueType;
import javolution.util.FastTable;
import javolution.xml.XMLFormat;
import javolution.xml.stream.XMLStreamException;
import org.apache.commons.math3.linear.MatrixUtils;
import org.apache.commons.math3.linear.RealMatrix;
import org.apache.commons.math3.linear.SingularValueDecomposition;
import org.jscience.mathematics.number.Float64;
import org.jscience.mathematics.vector.Float64Vector;

/**
 * A concrete 2D plane in n-dimensional space.
 * 
 * <p> Modified by: Joseph A. Huwaldt </p>
 * 
 * @author Joseph A. Huwaldt, Date: June 14, 2009
 * @version September 12, 2016
 */
@SuppressWarnings({"serial", "CloneableImplementsClone"})
public final class Plane extends GeomPlane implements ValueType {

    //  Smallest roundoff in quantities near 0, EPS, such that 0 + EPS > 0
    private static final double EPS = Parameter.EPS;
    private static final double SQRT_EPS = Parameter.SQRT_EPS;

    ///////////////////////
    // Factory creation. //
    ///////////////////////
    private Plane() { }

    private static final ObjectFactory<Plane> FACTORY = new ObjectFactory<Plane>() {
        @Override
        protected Plane create() {
            return new Plane();
        }

        @Override
        protected void cleanup(Plane obj) {
            obj.reset();
            obj._n = null;
            obj._const = null;
            obj._refPoint = null;
        }
    };

    private static Plane copyOf(Plane original) {
        Plane o = FACTORY.object();
        o._n = original._n.copy();
        o._const = original._const.copy();
        o._refPoint = original._refPoint.copy();
        original.copyState(o);
        return o;
    }

    private static Plane newInstance() {
        return FACTORY.object();
    }

    /**
     * A 3D plane that represents the YZ plane through the origin.
     */
    private static final Plane YZ;

    /**
     * A 3D plane that represents the XZ plane through the origin.
     */
    private static final Plane XZ;

    /**
     * A 3D plane that represents the XY plane through the origin.
     */
    private static final Plane XY;

    static {
        ImmortalContext.enter();
        try {
            YZ = Plane.newInstance();
            YZ._n = Vector.valueOf(1, 0, 0);
            YZ._const = Parameter.ZERO_LENGTH;
            YZ._refPoint = YZ.calcRefPoint();

            XZ = Plane.newInstance();
            XZ._n = Vector.valueOf(0, 1, 0);
            XZ._const = Parameter.ZERO_LENGTH;
            XZ._refPoint = XZ.calcRefPoint();

            XY = Plane.newInstance();
            XY._n = Vector.valueOf(0, 0, 1);
            XY._const = Parameter.ZERO_LENGTH;
            XY._refPoint = XY.calcRefPoint();
        } finally {
            ImmortalContext.exit();
        }
    }

    /**
     * Returns a 3D plane that represents the YZ plane through the origin.
     *
     * @return A 3D plane that represents the YZ plane through the origin.
     */
    public static Plane getYZ() {
        return YZ.copy();
    }

    /**
     * Return a 3D plane that represents the XZ plane through the origin.
     *
     * @return A 3D plane that represents the XZ plane through the origin.
     */
    public static Plane getXZ() {
        return XZ.copy();
    }

    /**
     * Return a 3D plane that represents the XY plane through the origin.
     *
     * @return A 3D plane that represents the XY plane through the origin.
     */
    public static Plane getXY() {
        return XY.copy();
    }

    /**
     * The normal vector for the plane.
     */
    private GeomVector<Dimensionless> _n;

    /**
     * The plane offset.
     */
    private Parameter<Length> _const;

    /**
     * A reference point for drawing this plane.
     */
    private Point _refPoint;

    /**
     * Returns a <code>Plane</code> instance with the specified 3D plane equation values.
     * The plane equation is:  <code>A*x + B*y + C*z = D</code>.
     *
     * @param A The coefficient on the X axis term.
     * @param B The coefficient on the Y axis term.
     * @param C The coefficient on the Z axis term.
     * @param D The constant term ((normal DOT p0) where p0 is a point in the plane). May
     *          not be null.
     * @return the plane having the specified values.
     */
    public static Plane valueOf(double A, double B, double C, Parameter<Length> D) {
        requireNonNull(D);
        Plane P = newInstance();
        Vector n = Vector.valueOf(A, B, C);
        P._n = n.toUnitVector();
        P._const = D.divide(n.magValue());
        P._refPoint = P.calcRefPoint();
        P._n.setOrigin(P._refPoint);
        return P;
    }

    /**
     * Returns a <code>Plane</code> instance with the specified normal vector and plane
     * equation constant.
     *
     * @param normal   The unit normal of the plane. May not be null.
     * @param constant The constant term of the plane equation ((normal DOT p0) where p0
     *                 is a point in the plane). May not be null.
     * @return the plane having the specified values.
     * @throws DimensionException if the input normal vector is not at least 3
     * dimensional.
     */
    public static Plane valueOf(GeomVector<Dimensionless> normal, Parameter<Length> constant) {
        requireNonNull(constant);
        int numDims = normal.getPhyDimension();
        if (numDims < 3)
            throw new DimensionException(
                    MessageFormat.format(RESOURCES.getString("dimensionNotAtLeast3"),
                            "input normal vector", numDims));

        Plane P = newInstance();
        P._n = normal.toUnitVector();
        P._const = constant;
        P._refPoint = P.calcRefPoint();
        P._n.setOrigin(P._refPoint);
        return P;
    }

    /**
     * Returns a <code>Plane</code> instance with the specified normal vector and plane
     * equation constant.
     *
     * @param normal The unit normal of the plane. May not be null.
     * @param p      A point in the plane (the reference point for the plane). May not be
     *               null.
     * @return the plane having the specified values.
     * @throws DimensionException if the input normal vector is not at least 3
     * dimensional.
     */
    public static Plane valueOf(GeomVector<Dimensionless> normal, Point p) {
        int numDims = normal.getPhyDimension();
        if (numDims < 3)
            throw new DimensionException(
                    MessageFormat.format(RESOURCES.getString("dimensionNotAtLeast3"),
                            "input normal vector", numDims));

        //  Convert all the points to the highest dimension between them.
        if (p.getPhyDimension() > numDims)
            numDims = p.getPhyDimension();
        p = p.toDimension(numDims);
        normal = normal.toDimension(numDims);

        Plane P = newInstance();
        P._n = normal.toUnitVector();
        P._const = (Parameter<Length>)P._n.dot(p.toGeomVector());
        P._refPoint = p;
        P._n.setOrigin(p);

        return P;
    }

    /**
     * Returns a <code>Plane</code> instance that contains the specified 3 independent
     * points.
     *
     * @param p A point in the plane (not equal to one of the other two). May not be null.
     * @param q A point in the plane (not equal to one of the other two). May not be null.
     * @param r A point in the plane (not equal to one of the other two). May not be null.
     * @return The plane containing the specified points. The plane will match the
     *         dimensions of the highest dimension point passed to this method.
     * @throws DimensionException if one of the input points does not have at least 3
     * physical dimensions.
     * @throws IllegalArgumentException if any of the input points are equal to the
     * others.
     */
    public static Plane valueOf(GeomPoint p, GeomPoint q, GeomPoint r) throws DimensionException {

        //  Convert all the points to the highest dimension between them.
        int numDims = GeomUtil.maxPhyDimension(p, q, r);
        if (numDims < 3)
            throw new DimensionException(
                    MessageFormat.format(RESOURCES.getString("dimensionNotAtLeast3"),
                            "input points", numDims));

        p = p.toDimension(numDims);
        q = q.toDimension(numDims);
        r = r.toDimension(numDims);

        Vector<Length> PQ = q.minus(p).toGeomVector();
        Vector<Length> PR = r.minus(p).toGeomVector();
        if (PQ.magValue() < SQRT_EPS || PR.magValue() < SQRT_EPS)
            throw new IllegalArgumentException(RESOURCES.getString("pointsEqualErr"));

        Vector PQcrossPR = PQ.cross(PR);
        if (PQcrossPR.magValue() < SQRT_EPS)
            throw new IllegalArgumentException(RESOURCES.getString("pointsEqualErr"));

        Point refP = p.immutable();
        Plane P = newInstance();
        P._n = PQcrossPR.toUnitVector();
        P._const = (Parameter<Length>)P._n.dot(refP.toGeomVector());
        P._refPoint = refP;
        P._n.setOrigin(refP);

        return P;
    }

    /**
     * Returns a <code>Plane</code> instance that contains the specified triangle.
     *
     * @param tri A triangle that defines the plane. May not be null.
     * @return The plane containing the specified triangle. The plane will match the
     *         dimensions of the triangle.
     * @throws DimensionException if the input triangle does not have at least 3
     * physical dimensions.
     * @throws IllegalArgumentException if the triangle is degenerate (has two vertices
     * equal to each other).
     */
    public static Plane valueOf(GeomTriangle tri) throws DimensionException {
        return valueOf(tri.getP1(), tri.getP2(), tri.getP3());
    }
 
    /**
     * Return a <code>Plane</code> instance that is fit through the input list of points
     * resulting in the least-squared orthogonal error between the points and the plane.
     *
     * @param points The list of points to fit a plane to.
     * @return A plane that represents the least squared error fit to the input points.
     */
    public static Plane fitPoints(List<? extends GeomPoint> points) {
        //  Reference: http://mathforum.org/library/drmath/view/63765.html

        int numPoints = points.size();
        if (numPoints < 3)
            throw new IllegalArgumentException(MessageFormat.format(
                    RESOURCES.getString("planeNumPointsErr"), numPoints));
        if (numPoints == 3)
            return Plane.valueOf(points.get(0), points.get(1), points.get(2));

        //  Convert the input list of points to a PointString.
        PointString<? extends GeomPoint> pnts;
        if (points instanceof PointString)
            pnts = (PointString)points;
        else
            pnts = PointString.valueOf(null, points);
        int numDims = pnts.getPhyDimension();
        if (numDims < 3)
            throw new IllegalArgumentException(MessageFormat.format(
                    RESOURCES.getString("dimensionNotAtLeast3"), "input points", numDims));

        //  Get the units of the points.
        Unit refUnit = pnts.getUnit();

        // Find the centroid of all the points.
        Point centroid = pnts.getAverage();

        //  Form a matrix of input points.
        double[] tmpArr = new double[numDims];
        RealMatrix Mmat = MatrixUtils.createRealMatrix(numPoints, numDims);
        for (int i = 0; i < numPoints; ++i) {
            GeomPoint point = pnts.get(i).to(refUnit);
            Mmat.setRow(i, point.toArray(tmpArr));
        }

        //  Calculate the singular value decomposition.
        SingularValueDecomposition svd = new SingularValueDecomposition(Mmat);

        //  Get the matrix V of the decomposition. The columns of V are the singular vectors.
        //  They are sorted from the largest to the smallest singular value.
        RealMatrix V = svd.getV();

        //  The normal vector corresponds to the smallest singular value from SVD, which is always the last one.
        Vector n = Vector.valueOf(V.getColumn(numDims - 1));

        //  The best-fit plane is the one passing through the centroid with the
        //  calculated normal vector.
        return Plane.valueOf(n, centroid);
    }

    /**
     * Calculate and return an arbitrary reference point that is <i>any</i> point in this
     * plane.
     */
    private Point calcRefPoint() {

        //  Find a non-zero axis.
        int dim = getPhyDimension();
        int axis = 0;
        for (; axis < dim; ++axis)
            if (MathLib.abs(_n.getValue(axis)) >= EPS)
                break;

        //  Set all axes except the chosen one to zero and solve for the point.
        Float64 v = Float64.valueOf(_const.getValue() / _n.getValue(axis));
        FastTable<Float64> values = FastTable.newInstance();

        for (int i = 0; i < dim; ++i) {
            if (i != axis)
                values.add(Float64.ZERO);
            else
                values.add(v);
        }

        Point p = Point.valueOf(Float64Vector.valueOf(values), _const.getUnit());

        return p;
    }

    /**
     * Return an immutable version of this plane.
     *
     * @return An immutable version of this plane.
     */
    @Override
    public Plane immutable() {
        return this;
    }

    /**
     * Recycles a <code>Plane</code> instance immediately (on the stack when executing in
     * a <code>StackContext</code>).
     *
     * @param instance The instance to be recycled.
     */
    public static void recycle(Plane instance) {
        FACTORY.recycle(instance);
    }

    /**
     * Returns the number of physical dimensions of the geometry element.
     *
     * @return The number of physical dimensions of the geometry element.
     */
    @Override
    public int getPhyDimension() {
        return _n.getPhyDimension();
    }

    /**
     * Return the equivalent of this plane converted to the specified number of physical
     * dimensions. If the number of dimensions is greater than this element, then zeros
     * are added to the additional dimensions. If the number of dimensions is less than
     * this element, then the extra dimensions are simply dropped (truncated). If the new
     * dimensions are the same as the dimension of this element, then this element is
     * simply returned.
     *
     * @param newDim The dimension of the plane to return.
     * @return A copy of this plane converted to the new dimensions.
     */
    @Override
    public Plane toDimension(int newDim) {
        if (newDim < 3)
            throw new DimensionException(
                    MessageFormat.format(RESOURCES.getString("dimensionNotAtLeast3"),
                            "input normal vector", newDim));
        int thisDim = this.getPhyDimension();
        if (newDim == thisDim)
            return this;

        Plane P = Plane.valueOf(getNormal().toDimension(newDim), getRefPoint().toDimension(newDim));
        return P;
    }

    /**
     * Return the normal vector for the plane. The normal vector is a unit vector that is
     * perpendicular to the plane.
     *
     * @return The normal vector for the plane.
     */
    @Override
    public GeomVector<Dimensionless> getNormal() {
        return _n.copy();
    }

    /**
     * Return the constant term of the plane equation (e.g.: "D" for a 3D plane:
     * <code>A*x + B*y + C*z = D</code>).
     *
     * @return The constant term of the plane equation for this plane.
     */
    @Override
    public Parameter<Length> getConstant() {
        return _const;
    }

    /**
     * Return the reference point for this plane. The reference point is an arbitrary
     * point that is contained in the plane and is used as a reference when drawing the
     * plane.
     *
     * @return The reference point for this plane.
     */
    @Override
    public Point getRefPoint() {
        return _refPoint;
    }

    /**
     * Returns the unit in which the geometry in this element are stated.
     *
     * @return The unit in which the geometry in this element are stated.
     */
    @Override
    public Unit<Length> getUnit() {
        return _const.getUnit();
    }

    /**
     * Returns the equivalent to this element but stated in the specified unit.
     *
     * @param unit the length unit of the element to be returned. May not be null.
     * @return an equivalent to this element but stated in the specified unit.
     * @throws ConversionException if the the input unit is not a length unit.
     */
    @Override
    public Plane to(Unit<Length> unit) {
        if (unit.equals(getUnit()))
            return this;
        Plane newPlane = Plane.valueOf(_n, _const.to(unit));
        //newPlane.setRefPoint(_refPoint.to(unit));
        copyState(newPlane);
        return newPlane;
    }

    /**
     * Returns a copy of this Plane instance
     * {@link javolution.context.AllocatorContext allocated} by the calling thread
     * (possibly on the stack).
     *
     * @return an identical and independent copy of this point.
     */
    @Override
    public Plane copy() {
        return copyOf(this);
    }

    /**
     * Return a copy of this object with any transformations or subranges removed
     * (applied).
     *
     * @return A copy of this object with any transformations or subranges removed.
     */
    @Override
    public Plane copyToReal() {
        return copy();
    }

    /**
     * Compares this Plane against the specified object for strict equality (same values
     * and same units).
     *
     * @param obj the object to compare with.
     * @return <code>true</code> if this object is identical to that object;
     *         <code>false</code> otherwise.
     */
    @Override
    public boolean equals(Object obj) {
        if (this == obj)
            return true;
        if ((obj == null) || (obj.getClass() != this.getClass()))
            return false;

        Plane that = (Plane)obj;
        return this._n.equals(that._n)
                && this._const.equals(that._const)
                && super.equals(obj);
    }

    /**
     * Returns the hash code for this parameter.
     *
     * @return the hash code value.
     */
    @Override
    public int hashCode() {
        return 31*super.hashCode() + Objects.hash(_n, _const);
    }

    /**
     * Holds the default XML representation for this object.
     */
    @SuppressWarnings("FieldNameHidesFieldInSuperclass")
    protected static final XMLFormat<Plane> XML = new XMLFormat<Plane>(Plane.class) {

        @Override
        public Plane newInstance(Class<Plane> cls, InputElement xml) throws XMLStreamException {
            return FACTORY.object();
        }

        @Override
        public void read(InputElement xml, Plane obj) throws XMLStreamException {
            GeomPlane.XML.read(xml, obj);     // Call parent read.

            obj._n = (GeomVector<Dimensionless>)xml.getNext();
            obj._const = (Parameter)xml.getNext();

            obj._refPoint = obj._n.getOrigin();
        }

        @Override
        public void write(Plane obj, OutputElement xml) throws XMLStreamException {
            GeomPlane.XML.write(obj, xml);    // Call parent write.

            xml.add(obj._n);
            xml.add(obj._const);

        }
    };

}