/*
 *      @(#)PickObject.java 1.2 99/04/13 16:09:36
 *
 * Copyright (c) 1996-1998 Sun Microsystems, Inc. All Rights Reserved.
 *
 * Sun grants you ("Licensee") a non-exclusive, royalty free, license to use,
 * modify and redistribute this software in source and binary code form,
 * provided that i) this copyright notice and license appear on all copies of
 * the software; and ii) Licensee does not utilize the software in a manner
 * which is disparaging to Sun.
 *
 * This software is provided "AS IS," without a warranty of any kind. ALL
 * EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND WARRANTIES, INCLUDING ANY
 * IMPLIED WARRANTY OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE OR
 * NON-INFRINGEMENT, ARE HEREBY EXCLUDED. SUN AND ITS LICENSORS SHALL NOT BE
 * LIABLE FOR ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING
 * OR DISTRIBUTING THE SOFTWARE OR ITS DERIVATIVES. IN NO EVENT WILL SUN OR ITS
 * LICENSORS BE LIABLE FOR ANY LOST REVENUE, PROFIT OR DATA, OR FOR DIRECT,
 * INDIRECT, SPECIAL, CONSEQUENTIAL, INCIDENTAL OR PUNITIVE DAMAGES, HOWEVER
 * CAUSED AND REGARDLESS OF THE THEORY OF LIABILITY, ARISING OUT OF THE USE OF
 * OR INABILITY TO USE SOFTWARE, EVEN IF SUN HAS BEEN ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGES.
 *
 * This software is not designed or intended for use in on-line control of
 * aircraft, air traffic, aircraft navigation or aircraft communications; or in
 * the design, construction, operation or maintenance of any nuclear
 * facility. Licensee represents and warrants that it will not use or
 * redistribute the Software for such purposes.
 */

/*
New Base level methods:

ISSUE :
        How about PickPoint and PickSegment ?

DONE :

        // These set/get the pick modes
        public void setPickMode(int mode);
        public int getPickMode() {
        public void setPickShapeMode(int mode);
        public int getPickShapeMode();
        public void setPickAperture(Point[] aperturePts);
        public Point[] getPickAperture();

        // these generate pick shapes
        PickRay generatePickRay(int x, int y)
        PickBounds generatePickAperture(int x, int y)
        // generate a shape based on the pick shape mode and pick type
        PickShape generatePickShape(int x, int y, int type)

        // These key off the pick mode to switch between bounds and 
        // geometry picking
        SceneGraphPath[] pickAll(int x, int y)
        SceneGraphPath[] pickAllSorted(int x, int y)
        SceneGraphPath   pickAny(int x, int y)
        SceneGraphPath   pickClosest(int x, int y)

        // these specify whether to use bounds or geometry picking via
        // the flag
        SceneGraphPath[] pickAll(int x, int y, int flag)
        SceneGraphPath[] pickAllSorted(int x, int y, int flag)
        SceneGraphPath   pickAny(int x, int y, int flag)
        SceneGraphPath   pickClosest(int x, int y, int flag)

        // these always use bounds picking
        private SceneGraphPath[] pickBoundsAll(int x, int y)
        private SceneGraphPath[] pickBoundsAllSorted(int x, int y)
        private SceneGraphPath   pickBoundsAny(int x, int y)
        private SceneGraphPath   pickBoundsClosest(int x, int y)

        // these always use geom picking
        private SceneGraphPath[] pickGeomAll(int x, int y)
        private SceneGraphPath[] pickGeomAllSorted(int x, int y)
        private SceneGraphPath   pickGeomAny(int x, int y)
        private SceneGraphPath   pickGeomClosest(int x, int y)

        Node getPickedNode(SceneGraphPath, flag)
        Node getPickedNode(SceneGraphPath, flag, int count)
               where flag can be any combo of:
                       Group, Morph, Primitive, Shape3D,
                       TransformGroup, Switch
TODO :

        bool intersect(SceneGraphPath, PickShape)


        Eventually:
                getClosestVtx(ScenGraphPath, PickShape)


Misc:
        Mouse should stay on top of object it is dragging

        */

import java.awt.*;
import java.awt.event.*;
import java.util.*;
import javax.media.j3d.*;
import com.sun.j3d.utils.geometry.Primitive;
import javax.vecmath.*;


/**
 * Contains methods to aid in picking.  A PickObject is created
 * for a given Canvas3D and a BranchGroup.  SceneGraphObjects
 * under the specified BranchGroup can then be checked to determine
 * if they have been picked.  
 * <p> The pick modes are
 * PICK_BOUNDS and PICK_GEOMETRY.  PICK_BOUNDS traverses the scene graph and
 * returns the SceneGraphPaths whose bounds intersect the pick shape.
 * PICK_GEOMETRY takes the result of a PICK_BOUNDS then returns the
 * SceneGraphPaths whose geometry intersects the pick shape.  
 * <p> The pick shape is generated using the x,y location of the pick and the 
 * pick shape mode. The pick shape modes are SHAPE_RAY, SHAPE_APERTURE and
 * SHAPE_RAY_APERTURE. SHAPE_RAY generates a PickRay through the x,y location
 * of the pick for use for both the bounds and geometry phases of the pick. 
 * SHAPE_APERTURE generates a PickBounds (using a BoundingPolytope) around the 
 * x,y location of the pick in the shape specified by the pick aperture.  The 
 * PickBounds is is used for both the bounds and geometry phases of the pick.  
 * SHAPE_RAY_APERTURE uses a PickRay for the bounds phase of the pick and 
 * then a PickBounds for the geometry phase of the pick. 
 * <p>
 * The best performance comes from doing PICK_BOUNDS picking using a SHAPE_RAY
 * pick shape.
 * <p>
 * More accurate picks come from using PICK_GEOMETRY.  When using
 * PICK_GEOMETRY, the ALLOW_INTERSECT bit must be set on the Shape3D nodes to
 * be picked.  Picking lines is difficult or impossible using SHAPE_RAY, use
 * SHAPE_APERTURE or SHAPE_RAY_APERTURE instead.  SHAPE_RAY_APERTURE should 
 * give better performance than SHAPE_APERTURE, but it may make it slightly
 * harder to pick shapes that are near the back of the scene.
 */
public class PickObject extends Object {

  // Have to rethink what to support. Is this complete.
  
  /**
   * A flag to indicate to the pickNode method to return a
   * <code>Shape3D</code> node from 
   * a given <code>SceneGraphPath</code>. 
   *
   * @see PickObject#pickNode 
   */
  public static final int SHAPE3D = 0x1;

  /**
   * A flag to indicate to the pickNode method to return a
   * <code>Morph</code> node from 
   * a given <code>SceneGraphPath</code>. 
   *
   * @see PickObject#pickNode 
   */
  public static final int MORPH = 0x2;

  /**
   * A flag to indicate to the pickNode method to return a
   * <code>Primitive</code> node 
   * from a given <code>SceneGraphPath</code>. 
   *
   * @see PickObject#pickNode 
   */
  public static final int PRIMITIVE = 0x4;

  /**
   * A flag to indicate to the pickNode method to return a
   * <code>Link</code> node from 
   * a given <code>SceneGraphPath</code>. 
   *
   * @see PickObject#pickNode 
   */
  public static final int LINK = 0x8;

  /**
   * A flag to indicate to the pickNode method to return a
   * <code>Group</code> node from 
   * a given <code>SceneGraphPath</code>. 
   *
   * @see PickObject#pickNode 
   */
  public static final int GROUP = 0x10;
  
  /**
   * A flag to indicate to the pickNode method to return a
   * <code>TransformGroup</code> 
   * node from a given <code>SceneGraphPath</code>. 
   *
   * @see PickObject#pickNode 
   */
  public static final int TRANSFORM_GROUP = 0x20;
 
  /**
   * A flag to indicate to the pickNode method to return a
   * <code>BranchGroup</code> 
   * node from a given <code>SceneGraphPath</code>. 
   *
   * @see PickObject#pickNode 
   */
  public static final int BRANCH_GROUP = 0x40;

  /**
   * A flag to indicate to the pickNode method to return a
   * <code>Switch</code> node from 
   * a given <code>SceneGraphPath</code>. 
   *
   * @see PickObject#pickNode 
   */
  public static final int SWITCH = 0x80;

  /**
   * Set this flag if you want to pick by bounds and then geometry.
   */
  public static final int USE_GEOMETRY = 0x100;

  /**
   * Set this flag if you want to pick by bounds.
   */
  public static final int USE_BOUNDS = 0x200;

  /**
   * Sets the pick shape mode to use a ray-based pick shape
   */
  public static final int SHAPE_RAY = 0x400;

  /**
   * Sets the pick shape mode to use an aperture-based pick shape
   */
  public static final int SHAPE_APERTURE = 0x800;

  /**
   * Sets the pick shape mode to use a ray-based pick shape for bounds 
   * intersections and an aperture-based pick shape for geometry intersections
   */
  public static final int SHAPE_RAY_APERTURE = 0x1000;
  

  BranchGroup pickRoot;
  Canvas3D canvas;
  Point3d origin = new Point3d();
  Vector3d direction = new Vector3d();
  int pickMode = USE_BOUNDS;         // To pick by Bounds or Bounds/Geometry.
  int pickShapeMode = SHAPE_RAY;     // what kind of pick shape(s)

  Point[] pickApertureShape = null;

  static final boolean debug = false;
  static final boolean verbose = false;

  /**
   * Creates a PickObject with a ray-based pick shape
   * @param c Current J3D canvas.
   * @param root The portion of the scenegraph for which picking is to occur
   * on.  It has to be a <code>BranchGroup</code>.
   *
   * @see BranchGroup
   * @see Canvas3D
   */ 
  public PickObject(Canvas3D c, BranchGroup root)
    {
      pickRoot = root;
      canvas = c;      
      pickApertureShape = new Point[4];
      pickApertureShape[0] = new Point(-4, -4);
      pickApertureShape[1] = new Point(4, -4);
      pickApertureShape[2] = new Point(4, 4);
      pickApertureShape[3] = new Point(-4, 4);
    }


  /**
   * Creates a PickRay that starts at the viewer position and points into
   * the scene in the direction of (xpos, ypos) specified in window space.
   *
   * @param xpos The value along the x-axis.
   * @param ypos The value along the y-axis.
   * @return A PickShape object that is the constructed PickRay.
   */ 
  public PickRay generatePickRay(int xpos, int ypos)
    {
            
      Transform3D motion=new Transform3D();
      Point3d eyePosn = new Point3d();
      Point3d mousePosn = new Point3d();
      Vector3d dirVec=new Vector3d();
      
      canvas.getPixelLocationInImagePlate(xpos,ypos,mousePosn);
      canvas.getCenterEyeInImagePlate(eyePosn);
      if (canvas.getView().getProjectionPolicy() == 
                                View.PARALLEL_PROJECTION) {
          // Correct for the parallel projection: keep the eye's z
          // coordinate, but make x,y be the same as the mouse, this
          // simulates the eye being at "infinity"
          eyePosn.x = mousePosn.x;
          eyePosn.y = mousePosn.y;
      }
      canvas.getImagePlateToVworld(motion);

      if (debug) {
        System.out.println("mouse position " + xpos + " " + ypos);
        System.out.println("before, mouse " + mousePosn + " eye " + eyePosn);
      }
      
      motion.transform(eyePosn);
      motion.transform(mousePosn);
      dirVec.sub(mousePosn, eyePosn);
      dirVec.normalize();
      
      if (debug) {
        System.out.println(motion + "\n");
        System.out.println("after, mouse " + mousePosn + " eye " + 
                        eyePosn + " dirVec " + dirVec);
      }

      PickRay pickRay = new PickRay();
      pickRay.set(eyePosn, dirVec);
            
      return pickRay;      
      
    }


  /**
   * Creates a bounding polytope PickShape which is a set of planes around the
   * point (xpos,ypos).  There will be a plane projected through each segment
   * of the the pick aperature and planes for the front and back clipping 
   * planes.
   *
   * @param xpos The value along the x-axis.
   * @param ypos The value along the y-axis.
   * @return A PickShape object that is the bounding polytope.
   */ 
  public PickBounds generatePickAperture(int xpos, int ypos) {

        Transform3D motion=new Transform3D();
        Point3d eyePosn = new Point3d();
        Point3d center = new Point3d();
        Point3d planePt = new Point3d();
        Vector3d planeNorm = new Vector3d();
        Point3d[] aperture = new Point3d[pickApertureShape.length];
        Point3d[] apertureEyePt = null; // for parallel projection
        View view = canvas.getView();

        for (int i = 0; i < pickApertureShape.length; i++) {
            aperture[i] = new Point3d();
            canvas.getPixelLocationInImagePlate(
                pickApertureShape[i].x + xpos,
                pickApertureShape[i].y + ypos,
                aperture[i]);
        }

        canvas.getCenterEyeInImagePlate(eyePosn);
        canvas.getImagePlateToVworld(motion);

        // Note: this code will only work if the screen is in the z=0 plane in 
        // image plate coords

        // get the center pt = eye pt projected to image plate
        center.set(eyePosn);
        center.z = 0.0;

        // save the eye to image plate distance for plane distance
        // calculations
        double ipEyeDist = eyePosn.z;

        int projectionType = view.getProjectionPolicy();

        if (projectionType == View.PARALLEL_PROJECTION) {
            // align the eye pt with the aperture pt to get a plane 
            // perpendicular to the image plate
            apertureEyePt = new Point3d[pickApertureShape.length];
            for (int i = 0; i < pickApertureShape.length; i++) {
                apertureEyePt[i] = new Point3d(aperture[i].x, aperture[i].y,
                                                eyePosn.z);
            }
        }

        if (debug) {
            System.out.println("mouse position " + xpos + " " + ypos);
            System.out.println("before:\n\taperture =");
            for (int i = 0; i < aperture.length; i++) {
                System.out.println("\t\t" + aperture[i]);
            }
            System.out.println("eye " + eyePosn);
        }

        motion.transform(eyePosn);
        motion.transform(center);
        for (int i = 0; i < aperture.length; i++) {
            motion.transform(aperture[i]);
        }
        if (projectionType == View.PARALLEL_PROJECTION) {
            for (int i = 0; i < aperture.length; i++) {
                motion.transform(apertureEyePt[i]);
            }
        }

        if (debug) {
            System.out.println(motion + "\n");
            System.out.println("after:\n\taperture =");
            for (int i = 0; i < aperture.length; i++) {
                System.out.println("\t\t" + aperture[i]);
            }
            System.out.println("eye " + eyePosn);
        }

        // we need a plane for each aperture pt, plus front/back clip
        Vector4d[] planes = new Vector4d[aperture.length + 2];

        for (int i = 0; i < aperture.length-1; i++) {
            if (projectionType == View.PARALLEL_PROJECTION) {
                planes[i] = planeFromPoints(apertureEyePt[i], aperture[i], 
                                                aperture[i+1]);
            } else {
                planes[i] = planeFromPoints(eyePosn, aperture[i], 
                                                aperture[i+1]);
            }
        }
        if (projectionType == View.PARALLEL_PROJECTION) {
            planes[aperture.length-1] = planeFromPoints(
                    apertureEyePt[aperture.length-1], 
                    aperture[aperture.length-1], aperture[0]);
        } else {
            planes[aperture.length-1] = planeFromPoints(eyePosn, 
                    aperture[aperture.length-1], aperture[0]);
        }

        // front / back clip planes

        // get plane normal and ipToVirtScale for plane dist calcs
        planeNorm.sub(center, eyePosn); // from eye to center
        double virtEyeDist = planeNorm.length(); 
        planeNorm.scale(1.0/virtEyeDist); // normalize using length
        double ipToVirtScale = virtEyeDist / ipEyeDist;

        // front plane
        double frontPlaneDist = view.getFrontClipDistance();
        int frontPlanePolicy = view.getFrontClipPolicy();
        setPlanePt(planePt, frontPlanePolicy, eyePosn, center, planeNorm, 
                ipToVirtScale, frontPlaneDist);
        Vector4d frontPlane = new Vector4d(planeNorm.x, planeNorm.y,planeNorm.z,
                -(planeNorm.x * planePt.x + planeNorm.y * planePt.y + 
                        planeNorm.z * planePt.z));
        // negate the terms since the plane norm points away from eye
        // and we want the ptope plane to point towards eye
        frontPlane.scale(-1.0);
        planes[aperture.length] = frontPlane;

        // back plane
        double backPlaneDist = view.getBackClipDistance();
        int backPlanePolicy = view.getBackClipPolicy();
        setPlanePt(planePt, backPlanePolicy, eyePosn, center, planeNorm, 
                ipToVirtScale, backPlaneDist);
        Vector4d backPlane = new Vector4d(planeNorm.x, planeNorm.y, planeNorm.z,
                -(planeNorm.x * planePt.x + planeNorm.y * planePt.y + 
                    planeNorm.z * planePt.z));
        planes[aperture.length+1] = backPlane;

        if (debug) {
            System.out.println("front plane = " + frontPlane);
            System.out.println("back plane = " + backPlane);
        }

        Bounds bounds = new BoundingPolytope(planes);

        PickBounds pickBounds = new PickBounds(bounds);

        return pickBounds;      

    }

    private void setPlanePt(Point3d planePt, int planePolicy, Point3d eyePosn,
            Point3d center, Vector3d planeNorm, double ipToVirtScale, 
            double planeDist) {

        if (debug) {
            System.out.println("setPlanePt: eyePosn = " + eyePosn + 
                "\ncenter = " + center + "\nplaneNorm = " + planeNorm);
        }
        switch (planePolicy) {
          case View.PHYSICAL_EYE:
            // plane distance is in image plate units, scale by virt/ip and
            // offset from eye
            planePt.x = eyePosn.x + planeNorm.x * ipToVirtScale * planeDist;
            planePt.y = eyePosn.y + planeNorm.y * ipToVirtScale * planeDist;
            planePt.z = eyePosn.z + planeNorm.z * ipToVirtScale * planeDist;
            if (debug) {
                System.out.println("PHYS_EYE ipDist = " + planeDist + 
                        " virtDist = " + ipToVirtScale * planeDist);
            }
            break;
          case View.PHYSICAL_SCREEN:
            // plane distance is in image plate units, scale by virt/ip and
            // offset from center
            planePt.x = center.x + planeNorm.x * ipToVirtScale * planeDist;
            planePt.y = center.y + planeNorm.y * ipToVirtScale * planeDist;
            planePt.z = center.z + planeNorm.z * ipToVirtScale * planeDist;
            if (debug) {
                System.out.println("PHYS_SCR ipDist = " + planeDist + 
                        " virtDist = " + ipToVirtScale * planeDist);
            }
            break;
          case View.VIRTUAL_EYE:
            // plane distance is in virt units, offset from eye
            planePt.x = eyePosn.x + planeNorm.x * planeDist;
            planePt.y = eyePosn.y + planeNorm.y * planeDist;
            planePt.z = eyePosn.z + planeNorm.z * planeDist;
            if (debug) {
                System.out.println("VIRT_EYE virtDist = " + planeDist);
            }
            break;
          case View.VIRTUAL_SCREEN:
            // plane distance is in virt units, offset from center 
            planePt.x = center.x + planeNorm.x * planeDist;
            planePt.y = center.y + planeNorm.y * planeDist;
            planePt.z = center.z + planeNorm.z * planeDist;
            if (debug) {
                System.out.println("VIRT_SCR virtDist = " + planeDist);
            }
            break;
        }
        if (debug) {
            System.out.println("returning planePt = " + planePt);
        }
    }

    private Vector4d planeFromPoints(Point3d base, Point3d rightPt,
                Point3d leftPt) {
        Vector3d rightVec = new Vector3d();
        rightVec.sub(rightPt, base);
        Vector3d leftVec = new Vector3d();
        leftVec.sub(leftPt, base);
        Vector3d normal = new Vector3d();
        // normal should point "out"
        normal.cross(leftVec, rightVec);
        normal.normalize();
        Vector4d plane = new Vector4d(normal.x, normal.y, normal.z,
                -(normal.x * base.x + normal.y * base.y + normal.z * base.z));

        if (debug) {
           System.out.println("plane = " + plane);
        }
        return plane;
    }

  /**
   * Creates a PickShape using the window space selection point (xpos, ypos).
   * The returned shape will be either a PickRay or a PickBounds, depending on
   * the pick shape mode and the pick mode 
   *
   * @param xpos The value along the x-axis.
   * @param ypos The value along the y-axis.
   * @param mode The pick mode, either USE_BOUNDS or USE_GEOMETRY
   * @return The constructed PickShape 
   */ 
  public PickShape generatePickShape(int xpos, int ypos, int pickType) {
    int shape = SHAPE_RAY;
    if (pickShapeMode == SHAPE_RAY) {
        shape = SHAPE_RAY;
    } else if (pickShapeMode == SHAPE_APERTURE) {
        shape = SHAPE_APERTURE;
    } else if (pickShapeMode == SHAPE_RAY_APERTURE) {
        if (pickType == USE_BOUNDS) {
            shape = SHAPE_RAY;
        } else if (pickType == USE_GEOMETRY) { 
            shape = SHAPE_APERTURE;
        }
    }
    if (shape == SHAPE_RAY) {
        return generatePickRay(xpos, ypos);
    } else {
        return generatePickAperture(xpos, ypos);
    }
  }


  /**
   * Sets the pick mode 
   * @param mode The pick mode, either USE_GEOMETRY or USE_BOUNDS. The default
   * is USE_BOUNDS.
   */
  public void setPickMode(int mode) {
      if ((mode != USE_GEOMETRY) && (mode != USE_BOUNDS)) {
        throw new IllegalArgumentException("Pick mode must be " +
                "PickObject.USE_GEOMETRY or PickObject.USE_BOUNDS");
      }
      if (verbose) {
          if (mode == USE_GEOMETRY) {
              System.out.println("PickObject.setPickMode(USE_GEOMETRY)");
          } else if (mode == USE_BOUNDS) {
              System.out.println("PickObject.setPickMode(USE_BOUNDS)");
          } else {
              System.out.println("PickObject.setPickMode(???)");
          }
      }
      pickMode = mode;
  }

  /**
   * Returns the pick mode 
   */
  public int getPickMode() {
      return pickMode;
  }

  /**
   * Sets the pick shape mode 
   * @param mode The pick shape mode, either SHAPE_RAY, SHAPE_APERTURE, or
   * SHAPE_RAY_APERTURE. The default is SHAPE_RAY.
   */
  public void setPickShapeMode(int mode) {
      if ((mode != SHAPE_RAY) && (mode != SHAPE_APERTURE) &&
          (mode != SHAPE_RAY_APERTURE)) {
        throw new IllegalArgumentException("Pick shape mode must be " +
                "PickObject.SHAPE_RAY, PickObject.SHAPE_APERTURE, or " +
                "PickObject.SHAPE_RAY_APERTURE");
      }
      if (verbose) {
          if (mode == SHAPE_APERTURE) {
              System.out.println(
                "PickObject.setPickBoundsShape(SHAPE_APERTURE)");
          } else if (mode == SHAPE_RAY) {
              System.out.println("PickObject.setPickBoundsShape(SHAPE_RAY)");
          } else if (mode == SHAPE_RAY_APERTURE) {
              System.out.println(
                "PickObject.setPickBoundsShape(SHAPE_RAY_APERTURE)");
          } else {
              System.out.println("PickObject.setPickBoundsShape(???)");
          }
      }
      pickShapeMode = mode;
  }

  /**
   * Returns the pick shape mode 
   */
  public int getPickShapeMode() {
      return pickShapeMode;
  }


  /**
   * Sets the pick aperture for SHAPE_APERTURE or SHAPE_RAY_APERTURE mode 
   * @param Point[] aperturePts The pick aperture, which should be a convex,
   * counter-clockwise loop of Points around 0,0
   */
  public void setPickAperture(Point[] aperturePts) {
      pickApertureShape = new Point[aperturePts.length];
      for (int i = 0; i < aperturePts.length; i++) {
          pickApertureShape[i] = new Point(aperturePts[i]);
      }
  }

  /**
   * Returns the pick aperture for SHAPE_APERTURE or SHAPE_RAY_APERTURE mode 
   */
  public Point[] getPickAperture() {
      Point[] retval = new Point[pickApertureShape.length];
      for (int i = 0; i < pickApertureShape.length; i++) {
          retval[i] = new Point(pickApertureShape[i]);
      }
      return retval;
  }

  /**
   * Returns an array referencing all the items that are pickable below the
   * <code>BranchGroup</code> (specified in the PickObject constructor) that
   * intersect with the pick shape specified by the pick shape mode, centered
   * on the point (xpos, ypos) in window space. The resultant array is 
   * unordered.
   *
   * @param xpos The x coordinate of the pick shape
   * @param ypos The y coordinate of the pick shape
   * @return The array of SceneGraphPath objects that contain Objects that
   *  were picked
   * If no pickable object is found <code>null</code> is returned..
   *
   * @see SceneGraphPath
   */  
  public SceneGraphPath[] pickAll(int xpos, int ypos)
    {

      if (pickMode == USE_BOUNDS) {
        return pickBoundsAll(xpos, ypos);
      } else if (pickMode == USE_GEOMETRY) {   
        return pickGeomAll(xpos, ypos);
      } else 
        return null;
    }

  /**
   * Returns a sorted array of references to all the Pickable items below the
   * <code>BranchGroup</code> (specified in the PickObject constructor) that
   * intersects with pick shape specified by the pick shape mode, centered
   * on the point (xpos, ypos) in window space. 
   * Element [0] references the item closest to viewer.
   *
   * @param xpos The x coordinate of the pick shape
   * @param ypos The y coordinate of the pick shape
   * @return A sorted arrayof SceneGraphPath objects that contain Objects that
   * were picked.  The array is sorted from closest to farthest from the
   * viewer
   * If no pickable object is found <code>null</code> is returned..
   *
   * @see SceneGraphPath
   */
  public SceneGraphPath[] pickAllSorted(int xpos, int ypos)
    {
      if (pickMode == USE_BOUNDS) {
        return pickBoundsAllSorted(xpos, ypos);
      } else if (pickMode == USE_GEOMETRY) {   
        return pickGeomAllSorted(xpos, ypos);
      } else 
        return null;

    }
  

  /**
   * Returns a reference to any item that is Pickable below the specified
   * <code>BranchGroup</code> (specified in the PickObject constructor) which
   * intersects with the pick shape specified by the pick shape mode, centered
   * on the point (xpos, ypos) in window space. 
   *
   * @param xpos The x coordinate of the pick shape
   * @param ypos The y coordinate of the pick shape
   * @return A SceneGraphPath of an object that was picked.  This is not
   * guarenteed to return the same result for multiple picks
   * If no pickable object is found <code>null</code> is returned..
   *
   * @see SceneGraphPath
   */
  public SceneGraphPath pickAny(int xpos, int ypos)
    {

      if (pickMode == USE_BOUNDS) {
        return pickBoundsAny(xpos, ypos);
      } else if (pickMode == USE_GEOMETRY) {   
        return pickGeomAny(xpos, ypos);
      } else 
        return null;
    }

  /**
   * Returns a reference to the item that is closest to the viewer and is
   * Pickable below the <code>BranchGroup</code> (specified in the PickObject
   * constructor) which intersects with the pick shape specified by the pick 
   * shape mode, centered on the point (xpos, ypos) in window space. 
   *
   * @param xpos The x coordinate of the pick shape
   * @param ypos The y coordinate of the pick shape
   * @return A SceneGraphPath which contains the closest pickable object.
   * If no pickable object is found, <code>null</code> is returned.
   *
   * @see SceneGraphPath
   */
  public SceneGraphPath pickClosest(int xpos, int ypos)
    {
      if (pickMode == USE_BOUNDS) {
        return pickBoundsClosest(xpos, ypos);
      } else if (pickMode == USE_GEOMETRY) {   
        return pickGeomClosest(xpos, ypos);
      }
      else 
        return null;
    }


  /**
   * Returns an array referencing all the items that are pickable below the
   * <code>BranchGroup</code> (specified in the PickObject constructor) that
   * intersect with the pick shape specified by the pick shape mode, centered
   * on the point (xpos, ypos) in window space. The resultant array is 
   * unordered.
   *
   * @param xpos The x coordinate of the pick shape
   * @param ypos The y coordinate of the pick shape
   * @param flag Specifies USE_BOUNDS or USE_GEOMETRY
   * @return The array of SceneGraphPath objects that contain Objects that
   *  were picked
   * If no pickable object is found <code>null</code> is returned..
   *
   * @see SceneGraphPath
   */  
  public SceneGraphPath[] pickAll(int xpos, int ypos, int flag)
    {

      if (flag == USE_BOUNDS) {
        return pickBoundsAll(xpos, ypos);
      } else if (flag == USE_GEOMETRY) {   
        return pickGeomAll(xpos, ypos);
      } else 
        return null;
    }

  /**
   * Returns a sorted array of references to all the Pickable items below the
   * <code>BranchGroup</code> (specified in the PickObject constructor) that
   * intersects with pick shape specified by the pick shape mode, centered
   * on the point (xpos, ypos) in window space. 
   * Element [0] references the item closest to viewer.
   *
   * @param xpos The x coordinate of the pick shape
   * @param ypos The y coordinate of the pick shape
   * @param flag Specifies USE_BOUNDS or USE_GEOMETRY
   * @return A sorted arrayof SceneGraphPath objects that contain Objects that
   * were picked.  The array is sorted from closest to farthest from the
   * viewer
   * If no pickable object is found <code>null</code> is returned..
   *
   * @see SceneGraphPath
   */
  public SceneGraphPath[] pickAllSorted(int xpos, int ypos, int flag)
    {

      if (pickMode == USE_BOUNDS) {
        return pickBoundsAllSorted(xpos, ypos);
      } else if (pickMode == USE_GEOMETRY) {   
        return pickGeomAllSorted(xpos, ypos);
      } else 
        return null;

    }
  
  /**
   * Returns a reference to any item that is Pickable below the specified
   * <code>BranchGroup</code> (specified in the PickObject constructor) which
   * intersects with the pick shape specified by the pick shape mode, centered
   * on the point (xpos, ypos) in window space. 
   *
   * @param xpos The x coordinate of the pick shape
   * @param ypos The y coordinate of the pick shape
   * @param flag Specifies USE_BOUNDS or USE_GEOMETRY
   * @return A SceneGraphPath of an object that was picked.  This is not
   * guarenteed to return the same result for multiple picks
   * If no pickable object is found <code>null</code> is returned..
   *
   * @see SceneGraphPath
   */
  public SceneGraphPath pickAny(int xpos, int ypos, int flag)
    {

      if (flag == USE_BOUNDS) {
        return pickBoundsAny(xpos, ypos);
      } else if (flag == USE_GEOMETRY) {   
        return pickGeomAny(xpos, ypos);
      } else 
        return null;
    }

  /**
   * Returns a reference to the item that is closest to the viewer and is
   * Pickable below the <code>BranchGroup</code> (specified in the PickObject
   * constructor) which intersects with the pick shape specified by the pick 
   * shape mode, centered on the point (xpos, ypos) in window space. 
   *
   * @param xpos The x coordinate of the pick shape
   * @param ypos The y coordinate of the pick shape
   * @param flag Specifys picking by Geometry or Bounds.
   * @return A SceneGraphPath which contains the closest pickable object.
   * If no pickable object is found, <code>null</code> is returned.
   *
   * @see SceneGraphPath
   */
  public SceneGraphPath pickClosest(int xpos, int ypos, int flag)
    {
      if (flag == USE_BOUNDS) {
        return pickBoundsClosest(xpos, ypos);
      } else if (flag == USE_GEOMETRY) {   
        return pickGeomClosest(xpos, ypos);
      } else 
        return null;
    }
  
  private SceneGraphPath[] pickBoundsAll(int xpos, int ypos)
    {
      PickShape pickShape = generatePickShape(xpos, ypos, USE_BOUNDS);
      SceneGraphPath sceneGraphPathArr[] = pickRoot.pickAll(pickShape);
      if (verbose) {
          System.out.print("pickBoundsdAll: pickShape:" +
                pickShape + "->");
          if (sceneGraphPathArr == null) {
            System.out.println(" no pick hits");
          } else {
            System.out.println(sceneGraphPathArr.length +
                " pick hits");
          }
      }
      return sceneGraphPathArr;
    }
  
  private SceneGraphPath[] pickBoundsAllSorted(int xpos, int ypos)
    {
      PickShape pickShape = generatePickShape(xpos, ypos, USE_BOUNDS);
      SceneGraphPath sceneGraphPathArr[] = pickRoot.pickAllSorted(pickShape);
      if (verbose) {
          System.out.print("pickBoundsdAllSorted: pickShape:" +
                pickShape + "->");
          if (sceneGraphPathArr == null) {
            System.out.println(" no pick hits");
          } else {
            System.out.println(sceneGraphPathArr.length +
                " pick hits");
          }
      }
      return sceneGraphPathArr;
    }

  private SceneGraphPath pickBoundsAny(int xpos, int ypos)
    {
      PickShape pickShape = generatePickShape(xpos, ypos, USE_BOUNDS);
      SceneGraphPath sceneGraphPath = pickRoot.pickAny(pickShape);
      if (verbose) {
          if (sceneGraphPath != null) {
              System.out.println("pickBoundsAny: pick hit"); 
          } else {
              System.out.println("pickBoundsAny: no pick hit"); 
          }
      }
      return sceneGraphPath;
    }

  private SceneGraphPath pickBoundsClosest(int xpos, int ypos)
    {
      if (pickShapeMode == SHAPE_APERTURE) {
          // TODO: do a pickAll, sort by distance from eye to bounds
          return pickAny(xpos, ypos);
      } 
      PickRay pickRay = generatePickRay(xpos, ypos);
      SceneGraphPath sceneGraphPath = pickRoot.pickClosest(pickRay);
      if (verbose) {
          if (sceneGraphPath != null) {
              System.out.println("pickBoundsClosest: pick hit"); 
          } else {
              System.out.println("pickBoundsClosest: no pick hit"); 
          }
      }
      return sceneGraphPath;
    }

  private SceneGraphPath[] pickGeomAll(int xpos, int ypos)
    {
      Node obj;
      int i, cnt=0;

      // do the bounds pass
      SceneGraphPath[] sceneGraphPathArr = pickBoundsAll(xpos, ypos);

      if(sceneGraphPathArr == null)
        return null;
      
      PickShape pickShape = generatePickShape(xpos, ypos, USE_GEOMETRY);

      boolean found[] = new boolean[sceneGraphPathArr.length];

      for(i=0; i<sceneGraphPathArr.length; i++) {
        obj = sceneGraphPathArr[i].getObject();
        if(obj instanceof Shape3D) {
          found[i] = ((Shape3D) obj).intersect(sceneGraphPathArr[i], pickShape);       
        } else if(obj instanceof Morph) {
          found[i] = ((Morph) obj).intersect(sceneGraphPathArr[i],  pickShape); 
        }
        if(found[i] == true)
          cnt++;        
      }

      if (verbose) {
          System.out.println("pickGeomAll: " + cnt+" pick hits intersect");
      }
      
      if(cnt == 0)
        return null;

      SceneGraphPath newSceneGraphPathArr[] = new SceneGraphPath[cnt];

      cnt = 0; // reset for reuse.
      for(i=0; i<sceneGraphPathArr.length; i++) {
        if(found[i] == true)
          newSceneGraphPathArr[cnt++] = sceneGraphPathArr[i];
      }
      
      return newSceneGraphPathArr;
    }

  private double distance[];

  private SceneGraphPath[] pickGeomAllSorted(int xpos, int ypos)
    {
      Node obj;
      int i, cnt=0;
      double dist[] = new double[1];

      if ((pickShapeMode == SHAPE_APERTURE) || 
          (pickShapeMode == SHAPE_RAY_APERTURE)) {
          // TODO: sort by distance from eye to bounds below
          return pickGeomAll(xpos, ypos);
      }

      // System.out.print("In pickGeomAllSorted\n");
      SceneGraphPath[] sceneGraphPathArr = pickBoundsAll(xpos, ypos);

      if (verbose) {
          System.out.print("pickGeomAllSorted: ");
          if (sceneGraphPathArr == null) {
            System.out.println(" no pick hits");
          } else {
            System.out.println(sceneGraphPathArr.length +
                " pick hits");
          }
      }
      
      if(sceneGraphPathArr == null)
        return null;
      
      PickRay pickRay = generatePickRay(xpos, ypos);
      boolean found[] = new boolean[sceneGraphPathArr.length];
      double distArr[] = new double[sceneGraphPathArr.length];

      for(i=0; i<sceneGraphPathArr.length; i++) {
        obj = sceneGraphPathArr[i].getObject();
        if(obj instanceof Shape3D) {
          found[i] = ((Shape3D) obj).intersect(sceneGraphPathArr[i], 
                                               pickRay, dist);
          distArr[i] = dist[0];
        } else if(obj instanceof Morph) {
          found[i] = ((Morph) obj).intersect(sceneGraphPathArr[i], 
                                             pickRay, dist);
          distArr[i] = dist[0];
        }
        if(found[i] == true)
          cnt++;        
      }

      if (verbose) {
          System.out.println("pickGeomAllSorted: " + cnt +
                " pick hits intersect");
      }
      
      if (cnt == 0)
        return null;

      SceneGraphPath newSceneGraphPathArr[] = new SceneGraphPath[cnt];
      distance = new double[cnt];

      cnt = 0; // reset for reuse.
      for(i=0; i<sceneGraphPathArr.length; i++) {
        if(found[i] == true) {
          newSceneGraphPathArr[cnt] = sceneGraphPathArr[i];
          distance[cnt++] = distArr[i];
        }
      }
      
      return sort(newSceneGraphPathArr);
    }

  
  private SceneGraphPath pickGeomClosest(int xpos, int ypos)
    {
      SceneGraphPath sgpArr[] = pickGeomAllSorted(xpos, ypos);
      
      if (sgpArr == null)
        return null;
      
      return sgpArr[0];    
    }


  private SceneGraphPath pickGeomAny(int xpos, int ypos)
    {
      Node obj;
      int i;

      SceneGraphPath[] sceneGraphPathArr = pickBoundsAll(xpos, ypos);

      if (sceneGraphPathArr == null) {
          return null;
      }

      PickShape pickShape = generatePickShape(xpos, ypos, USE_GEOMETRY);

      for(i=0; i<sceneGraphPathArr.length; i++) {
        obj = sceneGraphPathArr[i].getObject();
        if(obj instanceof Shape3D) {
          if(((Shape3D) obj).intersect(sceneGraphPathArr[i], pickShape))
            return sceneGraphPathArr[i];
        } else if(obj instanceof Morph) {
          if(((Morph) obj).intersect(sceneGraphPathArr[i], pickShape))
            return sceneGraphPathArr[i];
        }
      }
      if (verbose) {
          System.out.println("pickGeomAny: none intersect");
      }
      
      return null;
    }


  /**
   * Sort the elements in sgpArr and
   * return the sorted list in SceneGraphPath
   *
   * Sorts on the distance but also adjusts an array of positions
   * this allows the sort to operate on small data elements rather
   * than the possibly large SceneGraphPath
   *
   * Initial implementation is a Quick Sort
   */
  
  private int position[];
  
  private SceneGraphPath[] sort(SceneGraphPath sgpArr[]) {
    
    if (sgpArr == null) 
      return null;
    
    SceneGraphPath sorted[] = new SceneGraphPath[sgpArr.length];
    position = new int[sgpArr.length];
                
    for(int i=0; i<sgpArr.length; i++) {
      position[i]=i;
    }
    
    
    /*
      System.out.println("Before Sort :");
      for(int i=0; i<distance.length; i++) {
      System.out.println("pos " + position[i] +" dist "+ distance[i] + 
      " sgp "+ sgpArr[i]);
      }
      */

    quicksort( 0, distance.length-1 );
    
    for(int i=0; i<distance.length; i++) {
      sorted[i]= sgpArr[position[i]];
    }

    /*    
          System.out.println("\nAfter Sort :");
          for(int i=0; i<distance.length; i++) {
          System.out.println("pos " + position[i] +" dist "+ distance[i] + 
          " sorted sgp "+ sorted[i]);
          }
          */

    return sorted;
  }
  
  
  private final void quicksort( int l, int r ) {
    int p,i,j;
    double tmp,k;
    
    i = l;
    j = r;
    k = distance[(l+r) / 2];
    do {
      while (distance[i]<k) i++;
      while (k<distance[j]) j--;
      if (i<=j) {
        tmp = distance[i];
        distance[i] =distance[j];
        distance[j] = tmp;
        
        p=position[i];
        position[i]=position[j];
        position[j]=p;
        i++;
        j--;
      }
    } while (i<=j);
    
    if (l<j) quicksort(l,j);
    if (l<r) quicksort(i,r);
  }


  /**
   * Returns a reference to a Pickable <code>Node</code> that
   * is of the specified type
   * that is contained in the specified <code>SceneGraphPath</code>.
   * If more than one node of the same type is encountered, the node
   * closest to the <code>Object</code> will be returned.
   *
   * @param sgPath The <code>SceneGraphPath</code> to be traversed.
   * @param flags The <code>Node</code> types interested in picking.
   * @return This first occurrence of the specified <code>Node</code> type.
   * Starting from the Locale. If no pickable object is found <code>null</code> 
   * of the specifed types, <code>null</code> is returned.
   *
   */
  public Node pickNode(SceneGraphPath sgPath, int flags)
    {
      
      if (sgPath != null) {   
        Node pickedNode = sgPath.getObject();
        
        if ((pickedNode instanceof Shape3D) && ((flags & SHAPE3D) != 0)){
          if (debug) System.out.println("Shape3D found");
          return pickedNode;
        } 
        else if ((pickedNode instanceof Morph) && ((flags & MORPH) != 0)){
          if (debug) System.out.println("Morph found"); 
          return pickedNode;
        }
        else {    
          for (int j = 0; j < sgPath.nodeCount(); j++){
            pickedNode = sgPath.getNode(j); 
            if (debug) System.out.println("looking at node " + pickedNode);
            
            if ((pickedNode instanceof Primitive) &&
                ((flags & PRIMITIVE) != 0)){
              if (debug) System.out.println("Primitive found");
              return pickedNode;
            }
            else if ((pickedNode instanceof Link) && ((flags & LINK) != 0)){
              if (debug) System.out.println("Link found");
              return pickedNode;
            }
            else if ((pickedNode instanceof Switch) && ((flags & SWITCH) != 0)){
              if (debug) System.out.println("Switch found");
              return pickedNode;
            }
            else if ((pickedNode instanceof TransformGroup) &&
                     ((flags & TRANSFORM_GROUP) != 0)){
              if (debug) System.out.println("xform group found");
              return pickedNode;
            }
            else if ((pickedNode instanceof BranchGroup) &&
                     ((flags & BRANCH_GROUP) != 0)){
              if (debug) System.out.println("Branch group found");
              return pickedNode;
            }
            else if ((pickedNode instanceof Group) && ((flags & GROUP) != 0)){
              if (debug) System.out.println("Group found");
              return pickedNode;
            }        
          }
          
          if (pickedNode == null)
            if (debug) System.out.println("ERROR: null SceneGraphPath");
        }

      }
      
      return null;
      
    }

  /**
   * Returns a reference to a Pickable <code>Node</code> that
   * is of the specified type
   * that is contained in the specified <code>SceneGraphPath</code>.
   * The <code>Node</code> returned is the nth <code>occurrence</code>
   * of a <code>Node</code> that is of the specified type.
   *
   * @param sgPath The <code>SceneGraphPath</code> to be traversed.
   * @param flags The <code>Node</code> types interested.
   * @param occurrence Which occurrence of a <code>Node</code> that
   * matches the specified type to return.  An <code>occurrence</code> of
   * 1 means to return the first occurrence of that object type (the object
   * closest to the viewer).
   * @return The nth <code>occurrence</code> of a <code>Node</code<code>
   * of type <code>flags</code>, starting from the Locale. If no pickable object is 
   * found, <code>null</code> is returned.
   */  
  public Node pickNode(SceneGraphPath sgPath, int flags, int occurrence)
    {
      int curCnt=0;
      
      if (sgPath != null) {   
        Node pickedNode = sgPath.getObject();
        
        // Shape3D and Morph are leaf nodes and have no children. It doesn't
        // make sense to do occurrence check here. We'll just return it for now.
        if ((pickedNode instanceof Shape3D) && ((flags & SHAPE3D) != 0)){
          if (debug) System.out.println("Shape3D found");
          return pickedNode;
        } else if ((pickedNode instanceof Morph) && ((flags & MORPH) != 0)){
          if (debug) System.out.println("Morph found"); 
          return pickedNode;
        }
        else {    

          for (int j = 0; j < sgPath.nodeCount(); j++){
            pickedNode = sgPath.getNode(j);
            if (debug) System.out.println("looking at node " + pickedNode);
            
            if ((pickedNode instanceof Group) && ((flags & GROUP) != 0)){
              if (debug) System.out.println("Group found"); 
              curCnt++;
              if(curCnt == occurrence)
                return pickedNode;
            }
            else if ((pickedNode instanceof BranchGroup) &&
                    ((flags & BRANCH_GROUP) != 0)){
              if (debug) System.out.println("Branch group found");
              curCnt++;
              if(curCnt == occurrence)
                return pickedNode;            
            }
            else if ((pickedNode instanceof TransformGroup) &&
                    ((flags & TRANSFORM_GROUP) != 0)){
              if (debug) System.out.println("xform group found");
              curCnt++;
              if(curCnt == occurrence)
                return pickedNode;
            }
            else if ((pickedNode instanceof Primitive) &&
                    ((flags & PRIMITIVE) != 0)){
              if (debug) System.out.println("Primitive found");
              curCnt++;
              if(curCnt == occurrence)
                return pickedNode;
            }
            else if ((pickedNode instanceof Link) && ((flags & LINK) != 0)){
              if (debug) System.out.println("Link found");
              curCnt++; 
              if(curCnt == occurrence)
                return pickedNode;
            }
          }
          
          if (pickedNode == null)
            if (debug) System.out.println("ERROR: null SceneGraphPath");
        }

      }
      
      return null;
      
    }

}

