/**
 * Describes a point or vector in 3-space, in homogeneous coordinates. Immutable.
 * 
 * Ported from Dr. Mount's Geometry3D, so all credits go to him, all bugs to me, etc. etc.
 */
public class Geometry3D {
    // types ///////////////////////////////////////////////////////////////////////////////////////
    private static enum Type {
        POINT (1),
        VECTOR (0),
        UNDEFINED (-1);
        
        private final int w;
        
        Type(final int w) {
            this.w = w;
        }
    }
    
    
    // constants ///////////////////////////////////////////////////////////////////////////////////
    private static final int NUM_DIMENSIONS = 3;
    private static final int NUM_COORDINATES = NUM_DIMENSIONS + 1;
    private static final int X = 0;
    private static final int Y = 1;
    private static final int Z = 2;
    private static final int W = 3;
    private static final int RADIUS = 0;
    private static final int THETA = 1;
    private static final int PHI = 2;
    
    
    // data ////////////////////////////////////////////////////////////////////////////////////////
    private final double[] coordinates = new double[NUM_COORDINATES];
    
    
    // constructors ////////////////////////////////////////////////////////////////////////////////
    public Geometry3D(final double x, final double y, final double z, final double w) {
        coordinates[X] = x;
        coordinates[Y] = y;
        coordinates[Z] = z;
        coordinates[W] = w;
    }
    
    public Geometry3D(final Geometry3D geom) {
        System.arraycopy(geom.coordinates, 0, coordinates, 0, coordinates.length);
    }
    
    
    // getters /////////////////////////////////////////////////////////////////////////////////////
    public double getX() { return coordinates[X]; }
    public double getY() { return coordinates[Y]; }
    public double getZ() { return coordinates[Z]; }
    public double getW() { return coordinates[W]; }
    public double getRadius() { return coordinates[RADIUS]; }
    public double getTheta() { return coordinates[THETA]; }
    public double getPhi() { return coordinates[PHI]; }
    
    public double[] getCoordinates() {
        double[] coordinates = new double[this.coordinates.length];
        System.arraycopy(this.coordinates, 0, coordinates, 0, coordinates.length);
        return coordinates;
    }
    
    public float[] getImpreciseCoordinates() {
        float[] imprecise = new float[coordinates.length];
        
        for (int i = 0; i < coordinates.length; i++) {
            imprecise[i] = (float) coordinates[i];
        }
        return imprecise;
    }
    
    public boolean isPoint() { return getType() == Type.POINT; }
    public boolean isVector() { return getType() == Type.VECTOR; }
    public boolean isUndefined() { return getType() == Type.UNDEFINED; }
    
    private Type getType() {
        final double w = getW();
        
        if (w == Type.POINT.w) {
            return Type.POINT;
        } else if (w == Type.VECTOR.w) {
            return Type.VECTOR;
        } else {
            return Type.UNDEFINED;
        }
    }
    
    
    // basic arithmetic ////////////////////////////////////////////////////////////////////////////
    public Geometry3D add(final Geometry3D geom) { return add(this, geom); }
    public Geometry3D subtract(final Geometry3D geom) { return subtract(this, geom); }
    public Geometry3D multiply(final double scalar) { return multiply(this, scalar); }
    public Geometry3D divide(final double scalar) { return divide(this, scalar); }
    public Geometry3D negate() { return negate(this); }
    
    public static Geometry3D add(final Geometry3D geom1, final Geometry3D geom2) {
        return new Geometry3D(geom1.getX() + geom2.getX(),
                geom1.getY() + geom2.getY(),
                geom1.getZ() + geom2.getZ(),
                geom1.getW() + geom2.getW());
    }
    
    public static Geometry3D subtract(final Geometry3D geom1, final Geometry3D geom2) {
        return new Geometry3D(geom1.getX() - geom2.getX(),
                geom1.getY() - geom2.getY(),
                geom1.getZ() - geom2.getZ(),
                Math.abs(geom1.getW() - geom2.getW()));
    }
    
    public static Geometry3D multiply(final double scalar, final Geometry3D geom) {
        return multiply(geom, scalar);
    }
    
    public static Geometry3D multiply(final Geometry3D geom, final double scalar) {
        return new Geometry3D(geom.getX() * scalar,
                geom.getY() * scalar,
                geom.getZ() * scalar,
                Math.abs(geom.getW() * scalar));
    }
    
    public static Geometry3D divide(final Geometry3D geom, final double scalar) {
        return new Geometry3D(geom.getX() / scalar,
                geom.getY() / scalar,
                geom.getZ() / scalar,
                Math.abs(geom.getW() / scalar));
    }
    
    public static Geometry3D negate(final Geometry3D geom) {
        return new Geometry3D(-geom.getX(),
                -geom.getY(),
                -geom.getZ(),
                geom.getW());
    }
    
    
    // coordinate conversions //////////////////////////////////////////////////////////////////////
    public Geometry3D cartesianToSpherical() {
        return cartesianToSpherical(this);
    }
    
    public Geometry3D sphericalToCartesian() {
        return sphericalToCartesian(this);
    }
    
    public static Geometry3D cartesianToSpherical(final Geometry3D v) {
        final double x = v.getX();
        final double y = v.getY();
        final double z = v.getZ();
        final double lengthXY = Math.sqrt(x * x + y * y);
        
        return new Geometry3D(v.length(),   // radius
                Math.atan2(y, x),           // theta
                Math.atan2(lengthXY, z),    // phi
                v.getW());
    }
    
    public static Geometry3D sphericalToCartesian(final Geometry3D v) {
        final double radius = v.getRadius();
        final double theta = v.getTheta();
        final double phi = v.getPhi();
        
        return new Geometry3D(radius * Math.cos(theta) * Math.sin(phi),
                radius * Math.sin(theta) * Math.sin(phi),
                radius * Math.cos(phi),
                v.getW());
    }
    
    
    // vector operations ///////////////////////////////////////////////////////////////////////////
    public double length() {
        double sum = 0;
        
        for (int i = 0; i < NUM_DIMENSIONS; i++) {
            sum += coordinates[i] * coordinates[i];
        }
        return Math.sqrt(sum);
    }
    
    public Vector3D normalize() {
        return normalize(this);
    }
    
    public Vector3D clampCoordinates(final double min, final double max) {
        return clampCoordinates(this, min, max);
    }
    
    public Vector3D clampLength(final double min, final double max) {
        return clampLength(this, min, max);
    }
    
    public double dot(final Geometry3D v) {
        return dot(this, v);
    }
    
    public Vector3D cross(final Geometry3D v) {
        return cross(this, v);
    }
    
    public Vector3D orthogonal(final Geometry3D v) {
        return orthogonal(this, v);
    }
    
    public Vector3D parallel(final Geometry3D v) {
        return parallel(this, v);
    }
    
    public static Vector3D normalize(final Geometry3D v) {
        final double length = v.length();
        return new Vector3D(v.getX() / length, v.getY() / length, v.getZ() / length);
    }
    
    public static Vector3D clampCoordinates(final Geometry3D v, final double min, final double max) {
        return new Vector3D(clampValue(v.getX(), min, max),
                clampValue(v.getY(), min, max),
                clampValue(v.getZ(), min, max));
    }
    
    public static double clampValue(final double value, final double min, final double max) {
        if (value < min) {
            return min;
        } else if (value > max) {
            return max;
        }
        return value;
    }
    
    public static Vector3D clampLength(final Geometry3D v, final double min, final double max) {
        final double length = v.length();
        double scale = 1;
        
        if (length < min) {
            scale = min / length;
        } else if (length > max) {
            scale = max / length;
        }
        return new Vector3D(v.getX() * scale, v.getY() * scale, v.getZ() * scale);
    }
    
    public static double dot(final Geometry3D u, final Geometry3D v) {
        double sum = 0;
        
        for (int i = 0; i < NUM_DIMENSIONS; i++) {
            sum += u.coordinates[i] * v.coordinates[i];
        }
        return sum;
    }
    
    public static Vector3D cross(final Geometry3D u, final Geometry3D v) {
        return new Vector3D(u.getY() * v.getZ() - u.getZ() * v.getY(),
                u.getZ() * v.getX() - u.getX() * v.getZ(),
                u.getX() * v.getY() - u.getY() * v.getX());
    }
    
    public static Vector3D orthogonal(final Geometry3D u, final Geometry3D v) {
        return new Vector3D(Geometry3D.subtract(u, parallel(u, v)));
    }
    
    public static Vector3D parallel(final Geometry3D u, final Geometry3D v) {
        return new Vector3D(Geometry3D.multiply(u.dot(v) / v.dot(v), v));
    }
    
    
    // point operations ////////////////////////////////////////////////////////////////////////////
    public double distance(final Geometry3D p) {
        return distance(this, p);
    }
    
    public Point3D combine(final double alpha, final Geometry3D p) {
        return combine(alpha, this, p);
    }
    
    public static double distance(final Geometry3D p, final Geometry3D q) {
        return new Vector3D(Geometry3D.subtract(q, p)).length();
    }
    
    public static Point3D combine(final double alpha, final Geometry3D p, final Geometry3D q) {
        return new Point3D((1 - alpha) * p.getX() + alpha * q.getX(),
                (1 - alpha) * p.getY() + alpha * q.getY(),
                (1 - alpha) * p.getZ() + alpha * q.getZ());
    }
    
    
    // utilities ///////////////////////////////////////////////////////////////////////////////////
    @Override
    public String toString() {
        return getType() + " [" + getX() + ", " + getY() + ", " + getZ() + ", " + getW() + "]";
    }
    
    // convenience classes /////////////////////////////////////////////////////////////////////////
    public static final class Point3D extends Geometry3D {
        public Point3D(final double x, final double y, final double z) {
            super(x, y, z, Type.POINT.w);
        }
        
        public Point3D(final Geometry3D geom) {
            super(geom.getX(), geom.getY(), geom.getZ(), Type.POINT.w);
        }
        
        public static Point3D origin() { return new Point3D(0, 0, 0); }
    }
    
    public static final class Vector3D extends Geometry3D {
        public Vector3D(final double x, final double y, final double z) {
            super(x, y, z, Type.VECTOR.w);
        }
        
        public Vector3D(final Geometry3D geom) {
            super(geom.getX(), geom.getY(), geom.getZ(), Type.VECTOR.w);
        }
        
        public static Vector3D zero() { return new Vector3D(0, 0, 0); }
        public static Vector3D xUnit() { return new Vector3D(1, 0, 0); }
        public static Vector3D yUnit() { return new Vector3D(0, 1, 0); }
        public static Vector3D zUnit() { return new Vector3D(0, 0, 1); }
    }
}
