import java.util.*;
import javax.vecmath.*;
import com.navtools.util.*;

/**
 *
 * @author Bobby Martin
 * @copyright 2000
 * @version 0.1
 * @since 0.1
 */
public class VectorUtil
{
    public static double min2DDistance(List/*Point3d*/ points)
    {
        double retval = Double.POSITIVE_INFINITY;

        Iterator iter = points.iterator();
        while( iter.hasNext() )
        {
            Point3d i = (Point3d)iter.next();
            Point2d i2d = asPoint2d(i);
            Iterator iter2 = points.iterator();
            while( iter2.hasNext() )
            {
                Point3d j = (Point3d)iter2.next();
                Point2d j2d = asPoint2d(j);
                if( i != j )
                {
                    retval = Math.min(retval, i2d.distance(j2d));
                }
            }
        }

        return retval;
    }

    public static Point2d asPoint2d(Tuple3d tuple)
    {
        return new Point2d(tuple.x,tuple.z);
    }

    /**
     * Returns the cross product of a and b.  Since a & b both
     * have an implicit z coord of 0, the cross product always
     * has a 0 x and y.
     */
    public static Vector3d cross(Tuple2d a, Tuple2d b)
    {
        return new Vector3d(0.0, 0.0, crossValue(a,b));
    }

    /**
     * Returns the z portion of the cross product of a and b.
     */
    public static double crossValue(Tuple2d a, Tuple2d b)
    {
        return a.x*b.y - a.y*b.x;
    }

    /**
     * Returns the angle between vectors a and b.  The angle is
     * constrained to the range [0, 2*PI)
     */
    public static double angle(Vector2d a, Vector2d b)
    {
        double angle = a.angle(b);
        if( crossValue(a,b) > 0 )
            angle += Math.PI;
        return angle;
    }

    public static boolean sameSide(Tuple2d lineBegin, Tuple2d lineEnd,
                                   Tuple2d point1, Tuple2d point2)
    {
        Point2d lineVect = new Point2d(lineEnd);
        lineVect.sub(lineBegin);

        Point2d p1Vect = new Point2d(point1);
        p1Vect.sub(lineBegin);
        debug("p1Vect: " + p1Vect);

        Point2d p2Vect = new Point2d(point2);
        p2Vect.sub(lineBegin);
        debug("p2Vect: " + p2Vect);

        int cross1 = MathUtil.sign(crossValue(lineVect,p1Vect));
        debug("cross1: " + cross1);
        int cross2 = MathUtil.sign(crossValue(lineVect,p2Vect));
        debug("cross2: " + cross2);

        return cross1 == cross2;

    }

    public static Point2d fromPolar(double radius, double radians)
    {
        double x = radius * Math.cos(radians);
        double y = radius * Math.sin(radians);

        x = MathUtil.snap(x,1E-14);
        x = MathUtil.snapTo(x, 1E-14, 0.70710678118654);
        x = MathUtil.snapTo(x, 1E-14, -0.70710678118654);
        y = MathUtil.snap(y,1E-14);
        y = MathUtil.snapTo(y, 1E-14, 0.70710678118654);
        y = MathUtil.snapTo(y, 1E-14, -0.70710678118654);

        return new Point2d(x,y);
    }

    public static Vector3d rotate(Vector3d toRotate,
                                  float xAngle, float yAngle, float zAngle)
    {
        Matrix3d rotMatrix = new Matrix3d();
        rotMatrix.rotX(xAngle);
        rotMatrix.rotY(yAngle);
        rotMatrix.rotZ(zAngle);

        rotMatrix.transform(toRotate);
        return toRotate;
    }

    public static Point3d getXYZAngle(Vector3d vect)
    {
        Point3d retval = new Point3d();

        Vector2d projection = new Vector2d(vect.y, vect.z);
        retval.x = angle(projection, UNIT_2DX);

        projection.set(vect.x, vect.z);
        retval.y = angle(projection, UNIT_2DX);

        projection.set(vect.x, vect.y);
        retval.z = angle(projection, UNIT_2DX);

        return retval;
    }

    public static Vector2d UNIT_2DX = new Vector2d(1,0);
    public static Vector2d UNIT_2DY = new Vector2d(0,1);

    //TEST CODE FOLLOWS:
    public static void main(String[] args)
    {
        Vector3d origVector = new Vector3d(4,-8.5,6.3);

        origVector.normalize();
        System.out.println("Original vector: " + origVector);

        Point3d angles = getXYZAngle(origVector);
        Vector3d testVector = new Vector3d(0,0,1);

        Matrix3d rot = new Matrix3d();
        rot.setIdentity();
        rot.rotX(angles.x);
        rot.rotY(angles.y);
        rot.rotZ(angles.z);

        rot.transform(testVector);
        System.out.println("Test vector (should equal orig): " +
                           testVector);

        final double pi = Math.PI;
        Point2d[] pointsPIby4 = new Point2d[8];
        for( int i = 0; i < 8; ++i )
        {
            pointsPIby4[i] = fromPolar(1, i * pi/4);
        }

        Point2d origin = new Point2d();

        for( int i = 0; i < 8; ++i )
        {
            for( int j = 0; j < 8; ++j )
            {
                for( int k = 0; k < 8; ++k )
                {
                    int diffIJ = (j - i + 8) % 8;
                    int diffIK = (k - i + 8) % 8;
                    int classifyIJ = (diffIJ == 0 || diffIJ == 4) ? 0 : (diffIJ > 4) ? 1 : -1;
                    int classifyIK = (diffIK == 0 || diffIK == 4) ? 0 : (diffIK > 4) ? 1 : -1;

                    boolean shouldBeSameSide = classifyIJ == classifyIK;

                    if( shouldBeSameSide != sameSide(origin, pointsPIby4[i],
                                                     pointsPIby4[j], pointsPIby4[k]) )
                    {
                        System.err.println(pointsPIby4[j] + " and " +
                                           pointsPIby4[k] +
                                           (shouldBeSameSide?" not ":"") +
                                           "on same side of " +
                                           pointsPIby4[i]);
                    }
                }
            }
        }
    }

    public static final boolean DEBUG = false;
    public static void debug(String s)
    {
        if( DEBUG )
        {
            System.err.println(s);
        }
    }
}