import com.sun.j3d.utils.behaviors.mouse.MouseRotate;
import com.sun.j3d.utils.behaviors.mouse.MouseZoom;
import com.sun.j3d.utils.geometry.Box;
import com.sun.j3d.utils.geometry.Cylinder;
import java.applet.Applet;
import java.awt.BorderLayout;
import java.awt.event.*;
import com.sun.j3d.utils.applet.MainFrame;
import com.sun.j3d.utils.universe.*;
import java.io.*;
import java.util.*;
import javax.media.j3d.*;
import javax.vecmath.*;

import java.lang.Math;

/**
 * scene : petit programme de test de Java 3D.
 */

class Sonar 
{
    private Point3d pos;
    private Vector3d dir;
    private double offset;
        
    //--------------------------------------------------------------
    /**
     * Create an empty single sonar at (0.0 ; 0.0 ; 0.0) looking in
     * the direction (0.0 ; 0.0 ; -1.0) and with offset 0.0; 
     */
    public Sonar()
    {
	pos = new Point3d(0.0, 0.0, 0.0);
	dir = new Vector3d(0.0, 0.0, -1.0);
	dir.normalize();
	offset = 0.0;
    }
    
    //--------------------------------------------------------------
    /**
     * Create un new Sonar with the given values 
     */
    public Sonar(Point3d position_,
		 Vector3d direction_,
		 double offset_)
    {
	pos = new Point3d (position_);
	dir = new Vector3d(direction_);
	dir.normalize();
	offset = offset_;
	System.err.println("Sonar successfully created... ");
    }

    //----------xs----------------------------------------------------
    /**
     * Accessor to the position attribute 
     */
    public Point3d getPos () 
    {  return pos;  }
    
    //--------------------------------------------------------------
    /**
     * Accessor to the direction attribute 
     */
    public Vector3d getDir () 
    {  return (dir);  }

    //--------------------------------------------------------------
    /**
     * Accessor to the offset attribute 
     */
    public double getOffset () 
    {  return (offset);  }

    //--------------------------------------------------------------    
    /**
     * Build a PickRay object with the position and the direction of
     * the sonar sensor 
     */
    public PickRay getPickRay() 
    {return (new PickRay(pos, dir)); }

    //--------------------------------------------------------------    
    /**
     * Set the position of the sonar
     */
    public void setPos (Point3d position_) 
    {  pos = new Point3d(position_); }
    
    //-------------------------------------------------------------- 
    /**
     * Set the direction where the sonar is looking
     */
    public void setDir (Vector3d direction_)
    {
	dir = new Vector3d(direction_);
	dir.normalize();
    }
    
    //--------------------------------------------------------------    
    /**
     * Set the offset of the sonar. Offset are necessary since picking
     * may detect the robot as first obstacle if the position of the 
     * sonar is exactly on the robot case
     */
    public void setOffset (double off)
    {
	offset = off;
    }

}

//=====================================================================

public class testPicking extends Applet {

    public TransformGroup robot;              // robot node
    final static int SONAR_NUMBER = 16;

    Sonar[] sonars = new Sonar[SONAR_NUMBER];
    SceneGraphPath[] scgr = new SceneGraphPath[SONAR_NUMBER];

    private Point3d[] patch = {new Point3d (-1.0, -1.0, 0.0),     // 0
			       new Point3d (1.0, -1.0, 0.0),      // 1
			       new Point3d (1.0, 1.0, 0.0),       // 2
			       new Point3d (-1.0, 1.0, 0.0)};     // 3

    private Point3d[] patch2 = {new Point3d (0.0, 0.0, 0.0),       // 0
			        new Point3d (1.0, 0.0, 0.0),       // 1
			        new Point3d (1.0, 0.0, 1.0),       // 2
			        new Point3d (0.0, 0.0, 1.0)};      // 3

    public QuadArray patche = new QuadArray(4, GeometryArray.COORDINATES | 
					    GeometryArray.NORMALS);

    //--------------------------------------------------------------
    public void initSceneGraphPath()
    {
        System.out.println("Start initializing SceneGraphPath array...");
	for (int i = 0; i < 16; i++) {
	    scgr[i] = new SceneGraphPath();
	}
	System.out.println("SceneGraphPath has been initialized...");
    }

    //--------------------------------------------------------------
    public void initSonars()
    {
      System.out.println("Start initializing sonar array...");
      for (int i = 0 ; i < 16; i++) {
	  //System.out.println("     processing element :  " + i);
	  sonars[i] = new Sonar();
	  //sonars[i].pos = new Point3d(0.0, 0.0, 0.0);
	  
	  sonars[i].setPos(new Point3d(1.5 * Math.cos((i * Math.PI * 2.0) / 16.0),
				       0.0,
				       1.5 * Math.sin((i * Math.PI * 2.0) / 16.0)));
	  //System.out.println("          start position ok ");
	  sonars[i].setDir(new Vector3d(Math.cos((i * Math.PI * 2.0) / 16.0),
					0.0,
					Math.sin((i * Math.PI * 2.0) / 16.0)));
	  //System.out.println("          direction vector ok !");
      }
      System.out.println("Sonar array has been initialized...");
    }

    //------------------------------------------------------------
    /**
     * This method create a patche for the robot environment
     */
    
    public TransformGroup CreatePatche (Color3f color, 
					Vector3d translation,
					Vector3d scaling)
    {
	// Create a box with a QuadArray

	PolygonAttributes polyAttrib = new PolygonAttributes();
	polyAttrib.setCullFace(PolygonAttributes.CULL_NONE);
	
	Material objMaterial = new Material();        
	objMaterial.setDiffuseColor(color);
	objMaterial.setAmbientColor(new Color3f(0.0f, 1.0f, 0.5f));
	objMaterial.setSpecularColor(new Color3f(1.0f, 1.0f, 1.0f));
	Appearance objAppearance = new Appearance();  
	//	objAppearance.setColoringAttributes (ColoringAttributes.ALLOW_COLOR_WRITE); 
	objAppearance.setMaterial(objMaterial);
	objAppearance.setPolygonAttributes (polyAttrib);

	TransformGroup boxTrans = new TransformGroup();
	boxTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE | 
			       TransformGroup.ALLOW_TRANSFORM_READ |
			       TransformGroup.ENABLE_PICK_REPORTING);

	Transform3D boxShift = new Transform3D();
	boxShift.setTranslation(translation);
	boxShift.setScale(scaling);
	boxTrans.setTransform(boxShift);

	patche.setCoordinates (0, patch);
	patche.setCapability (Geometry.ALLOW_INTERSECT);
	Shape3D boxNode = new Shape3D(patche,
				      objAppearance);

	boxNode.setCapability(Geometry.ALLOW_INTERSECT |
			      Node.ENABLE_PICK_REPORTING |
			      Node.ALLOW_PICKABLE_READ |
			      Node.ALLOW_PICKABLE_WRITE);
	boxNode.setPickable(true);
	//boxNode.setAppearance (objAppearance);
	

	boxTrans.setCapability(Geometry.ALLOW_INTERSECT | 
			       Node.ENABLE_PICK_REPORTING |
			       Node.ALLOW_PICKABLE_READ |
			       Node.ALLOW_PICKABLE_WRITE);
	boxTrans.setPickable(true);
	boxTrans.addChild(boxNode);

	return (boxTrans);
    }

    //------------------------------------------------------------
    /**
     * This method create a cylinder which simulate the Nomadic Robot
     */
    public TransformGroup  CreateCylinder (Color3f colorAmb, 
					   Color3f colorDif,
					   Vector3d translation,
					   Vector3d scaling)
    {
        //Create a cylinder.
        Material objMaterial = new Material();        
	objMaterial.setAmbientColor(colorAmb);
	objMaterial.setDiffuseColor(colorDif);
	Appearance objAppearance = new Appearance(); 
	objAppearance.setMaterial(objMaterial);

	TransformGroup boxTrans = new TransformGroup();
	Transform3D boxShift = new Transform3D();
	boxShift.setTranslation(translation);
	boxShift.setScale(scaling);
	// boxShift.rotX(Math.PI / 2.0);
	boxTrans.setTransform(boxShift);
	Cylinder boxObj = new Cylinder(1.0f, 2.0f,
				       Cylinder.GENERATE_NORMALS | 
				       Cylinder.ENABLE_GEOMETRY_PICKING,
				       objAppearance);
	boxObj.setCapability(Node.ENABLE_PICK_REPORTING | 
			     Geometry.ALLOW_INTERSECT);
	boxTrans.setCapability(TransformGroup.ENABLE_PICK_REPORTING);
	boxTrans.addChild(boxObj);
	return(boxTrans);
    }

    //------------------------------------------------------------
    public BranchGroup createSceneGraph()
    {
        // Create the root of the branch graph
        BranchGroup objRoot = new BranchGroup();
	
	// Create a Transformgroup to scale all objects so they
	// appear in the scene.
	TransformGroup objScale = new TransformGroup();
	Transform3D t3d = new Transform3D();
	//t3d.setScale(0.2);
	//objScale.setTransform(t3d);
	objScale.setCapability(TransformGroup.ENABLE_PICK_REPORTING);
	objRoot.addChild(objScale);
	objRoot.setCapability(TransformGroup.ENABLE_PICK_REPORTING);

	Material objMaterial = new Material();        
	objMaterial.setDiffuseColor(new Color3f(1.0f, 1.0f, 1.0f));
	Appearance objAppearance = new Appearance();  
	objAppearance.setMaterial(objMaterial);


	// This Transformgroup is used by the mouse manipulators to
	// move the Box.
	TransformGroup objTrans = new TransformGroup();
	objTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
	objTrans.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
	objTrans.setCapability(TransformGroup.ENABLE_PICK_REPORTING);
	objScale.addChild(objTrans);

	// Create first patche
	objTrans.addChild(CreatePatche (new Color3f(0.5f, 0.8f, 0.9f),
					new Vector3d(0.0, 0.0, -10.0),
					new Vector3d(10.0, 1.0, 0.2)));
	// Create second patche
	objTrans.addChild(CreatePatche (new Color3f(0.5f, 0.8f, 0.9f),
					new Vector3d(0.0, 0.0, 10.0),
					new Vector3d(10.0, 1.0, 0.2)));
	// Create robot prototype

	robot = CreateCylinder (new Color3f(0.0f, 0.0f, 1.0f),
				new Color3f(1.0f, 0.8f, 0.0f),
				new Vector3d(0.0, 0.0, 0.0),
				new Vector3d(1.0, 1.0, 1.0));
	
        objTrans.addChild(robot);
	
	BoundingSphere bounds =
	    new BoundingSphere(new Point3d(0.0,0.0,0.0), 1000.0);
  
      
	// Create the rotate behavior node
	MouseRotate behavior = new MouseRotate(objTrans);
	objTrans.addChild(behavior);
	behavior.setSchedulingBounds(bounds);

	// Create the zoom behavior node
	MouseZoom behavior2 = new MouseZoom(objTrans);
	objTrans.addChild(behavior2);
	behavior2.setSchedulingBounds(bounds);
    

	//Shine it with two colored lights.
	Color3f lColor1 = new Color3f(1.0f, 1.0f, 1.0f);
	Color3f lColor2 = new Color3f(1.0f, 1.0f, 1.0f);
	Vector3f lDir1  = new Vector3f(-1.0f, -1.0f, -1.0f);
	Vector3f lDir2  = new Vector3f(0.0f, 1.0f, -1.0f);
	DirectionalLight lgt1 = new DirectionalLight(lColor1, lDir1);
	DirectionalLight lgt2 = new DirectionalLight(lColor2, lDir2);
	
	AmbientLight algt = new AmbientLight(lColor1);

	lgt1.setInfluencingBounds(bounds);
	lgt2.setInfluencingBounds(bounds);
	algt.setInfluencingBounds(bounds);
	//objScale.addChild(lgt1);
	objScale.addChild(lgt2);
	objScale.addChild(algt);

	// Let Java 3D perform optimizations on this scene graph.
	objRoot.compile();

	return objRoot;
    }
  
    //--------------------------------------------------------------
  
    //--------------------------------------------------------------
    public testPicking ()
    {
	double dist[] = new double[10];
        setLayout(new BorderLayout());
	Canvas3D c = new Canvas3D(null);
	add("Center", c);
    
	Shape3D  hitObj = new Shape3D();

	this.initSonars();
	this.initSceneGraphPath();

	// Create a simple scene and attach it to the virtual universe
	BranchGroup scene = createSceneGraph();
	SimpleUniverse u = new SimpleUniverse(c);


	// This will move the ViewPlatform back a bit so the
	// objects in the scene can be viewed.
	u.getViewingPlatform().setNominalViewingTransform();

	u.addBranchGraph(scene);

	//SceneGraphPath scgr = new SceneGraphPath();
	PickRay pick;
	boolean isHit  = false;
	Node hitShape;
	for (int i = 0; i < 16; i++)
	{ 
	    System.out.println("Initializing pick ray for sonar["+i+"]...");
	    pick = new PickRay(sonars[i].getPos(), sonars[i].getDir());
	    scgr[i] = scene.pickClosest(pick);
	    System.out.println(" pick ray for sonar["+i+"] initialized ...");
	    
	    // SceneGraphPath IS ALWAYS NULL WITH JAVA3D 1.2BETA BUT NOT FOR 
	    // JAVA3D 1.1.3 (BOTH OPENGL VERSION)
	    if (scgr[i] == null) {
	      System.out.println("Scene graph for index " + i + " is null ");
	    } else {
	        System.out.println("Scene graphPath : " + scgr[i].toString() );
		System.out.println();
		try {
		    if (scgr[i] != null) {
			hitShape = scgr[i].getObject();
		    
			if (hitShape instanceof Shape3D) {
			    // System.out.println("on a retrouvé un Shape3D !!");
			    isHit= ((Shape3D)hitShape).intersect(scgr[i], pick, dist);
			    if (isHit) {
				System.out.println("Distance to shape " + dist[0]);
			    }			    
			} else {
			    System.out.println("this node isn't a Shape3D !!");
			}
		    }
		} catch (CapabilityNotSetException e) {
		    System.out.println ("Opération non autorisée sur l'objet[" + i + "] : " + e);
		}
	    }
	    System.out.println();

	}
    }
  
  
    //--------------------------------------------------------------
    //--------------------------------------------------------------
    public static void main(String argv[])
    {
        BranchGroup group;
    
	new MainFrame(new testPicking(), 400, 400);
    }
}