/**	
	Company:		Shout Interactive
	Project:		Shout3D 1.0
	Class:			WalkPanel
	Date:			September 15, 1999
	Description:	Class for Walking
	(C) Copyright Shout Interactive, Inc. - 1997/1998/1999 - All rights reserved
 */

package applets;

import java.applet.*;
import java.awt.*;
import java.awt.image.*;
import java.io.*;
import java.util.Date;
import java.net.URL;
import java.util.*;
import shout3d.core.*;
import shout3d.math.*;
import shout3d.*;

import custom_nodes.Boid;		// Get Boid

/**
 * Shout3D BoidWalkPanel. This class is meant to provide the user with the ability to \
 * navigate around a 3D world in a similar manner to the VRML "WALK" mode.
 * 
 * @author Jim Stewartson
 * @author Paul Isaacs
 * @author Rory Lane Lutter
 * @author Dave Westwood
 */

public class BoidWalkPanel extends Shout3DPanel implements RenderObserver, DeviceObserver{
	
    // cache the initial camera and the root of the scene
    Viewpoint camera;
    Transform root;
    
    // quaternion for the camera
    Quaternion cameraQuat = new Quaternion();
    
    // variables for camera calculations 
    float cameraHeading = 0;
    float headingDelta = 0;
    float speed = 0;
    float startX = 0;
    float startY = 0;
    
    // the avatarRadius, avatarHeight, and collideHeight of the avatar
    // can all be set using applet parameters.
    // The camera is always placed at a height of "avatarHeight"
    // Collisions are done in the plane that lies at the level of "collideHeight"
    // The avatarRadius is the closest that the avatar may get to a surface in the scene.
    // The collision algorithm starts with the camera position (which is at a height of avatarHeight) and
    // finds the imaginary collider, which is the point below the camera at the height "collideHeight"
    // Then it attemps to move that collider forward through the scene.  If the proposed motion
    // will move the collider closer than avatarRadius to a surface in the scene, then a collision is
    // declared and the camera position will not be changed.
    public float avatarRadius = 2f;
    public float avatarHeight = 2f;
    public float collideHeight = .25f;

    // 
    // Variable that define the flock
    //

    // Flock dynamics parameters
    private float   inertiaFactor;
    private float   pullFactor;
    private float   proximityDistance;
    private float   proximityFactor;
    private float   maxVelocity;
    private boolean canPerch;
    private float   perchTime;
    private float   perchVariable;
    private float   flockBoundCenter[];
    private float   flockBoundSize[];
    
    // Run time calculation space
    private float boids_pos[][];
    private float boids_vel[][];
    private float boids_resting[];
    private long  lasttime;
    private Random rnd;

    
    // Actual pointers to scene graph nodes    
    private Boid  boids[];	
    private TimeSensor  boid_timers[];	

    /**
     * Construct me
     */
    public BoidWalkPanel(Shout3DApplet applet){
	super(applet);
    }
    
    /**
     * Remove observers when done with the panel
     */
    public void finalize()throws Throwable {
	applet.getDeviceListener().removeDeviceObserver(this, "DeviceInput");
	applet.getRenderer().removeRenderObserver(this);		
	super.finalize();
    }
	
    /**
	 * Overrides Shout3DPanel.customInitialize()
	 */
    public void customInitialize() {

	// Read the 3 avatar parameters from applet parameters, if specified:
	String avatarHeightString = applet.getParameter("avatarHeight");
	if (avatarHeightString != null){
	    avatarHeight = Float.valueOf(avatarHeightString).floatValue();
	}
	String avatarRadiusString = applet.getParameter("avatarRadius");
	if (avatarRadiusString != null){
	    avatarRadius = Float.valueOf(avatarRadiusString).floatValue();
	}
	String collideHeightString = applet.getParameter("collideHeight");
	if (collideHeightString != null){
	    collideHeight = Float.valueOf(collideHeightString).floatValue();
	}
	
	// cache the camera and the scene root
	camera = (Viewpoint)(applet.getCurrentBindableNode("Viewpoint"));
        root = (Transform)getScene();
		
	// set the position to your avatar's height
	camera.position.set1Value(1, avatarHeight);
		
	// Set the camera heading to align with that of the active viewpoint.
	Quaternion startCamQuat = new Quaternion();
	startCamQuat.setAxisAngle(camera.orientation.getValue());
	float[] startCamEulers = new float[3];
	startCamQuat.getEulers(startCamEulers);
	cameraHeading = startCamEulers[0];
		
	// register for device events
	getDeviceListener().addDeviceObserver(this, "DeviceInput", null);	
	// register for render events
	getRenderer().addRenderObserver(this, null);

	//
	// Boid Setup
	//
	boidsInit();
    }
	
    void boidsInit() {
	// 
	// Find the boids
	//
	Searcher boid_search = applet.getNewSearcher();
	
	boid_search.setType("Boid");
	Node result[][] = boid_search.searchAll(applet.getScene());
	Node subresult[];

	// Allocate the array of boids and run-time space
	boids = new Boid[result.length];
	boid_timers = new TimeSensor[result.length];
	boids_pos = new float[result.length][3];
	boids_vel = new float[result.length][3];
	boids_resting = new float[result.length];
	
  	for(int i=0; i<result.length;i++) {
//  	    System.out.println("Boid " + i + " length "+result[i].length);

	    // Find the boid
	    boids[i]= (Boid)result[i][result[i].length-1];

	    // Initial flock values
	    boids_pos[i][0] = boids[i].translation.getValue()[0];
	    boids_pos[i][1] = boids[i].translation.getValue()[1];
	    boids_pos[i][2] = boids[i].translation.getValue()[2];
	    boids_vel[i][0] = 0.0f;
	    boids_vel[i][1] = 0.0f;
	    boids_vel[i][2] = 0.0f;
	    boids_resting[i] = 0.0f;
	}
	
	boid_search.setType("TimeSensor");
	result = boid_search.searchAll(applet.getScene());
	
	/* NB, the timesensors were moved to the top level in the
           scene graph on conversion to .s3d, so all we can do is
           assume that the same number exist */

  	for(int i=0; i<result.length;i++) {
  	    System.out.println("TS " + i + " length "+result[i].length);
	    boid_timers[i] = (TimeSensor)result[i][result[i].length-1];
  	}

	// Customisable aspects
	String tmp = applet.getParameter("inertiaFactor");
	if (tmp != null){
	    inertiaFactor = Float.valueOf(tmp).floatValue();
	}
	else {
	    inertiaFactor = 0.1f;
	}

	tmp = applet.getParameter("pullFactor");
	if (tmp != null){
	    pullFactor = Float.valueOf(tmp).floatValue();
	}
	else {
	    pullFactor = 3.0f;
	}

	tmp = applet.getParameter("proximityDistance");
	if (tmp != null){
	    proximityDistance = Float.valueOf(tmp).floatValue();
	}
	else {
	    proximityDistance = 4.0f;
	}

	tmp = applet.getParameter("proximityFactor");
	if (tmp != null){
	    proximityFactor = Float.valueOf(tmp).floatValue();
	}
	else {
	    proximityFactor = 5.0f;
	}

	tmp = applet.getParameter("maxVelocity");
	if (tmp != null){
	    maxVelocity = Float.valueOf(tmp).floatValue();
	}
	else {
	    maxVelocity = 5.0f;
	}

	// Non-customisable
	canPerch = true;
	perchTime = 5.0f;
	perchVariable = 5.0f;

	flockBoundCenter = new float[3];
	flockBoundCenter[0] = 0.0f;
	flockBoundCenter[1] = 10.0f;
	flockBoundCenter[2] = 0.0f;

	flockBoundSize = new float[3];
	flockBoundSize[0] = 20.0f;
	flockBoundSize[1] = 10.0f;
	flockBoundSize[2] = 20.0f;

	rnd = new Random();
    }
    
    MouseInput mi;
	
    public boolean onDeviceInput(DeviceInput di, Object userData){
	if (di instanceof MouseInput){
	    mi = (MouseInput)di;
	    switch (mi.which){
	    case MouseInput.DOWN:
		startX = mi.x;
		startY = mi.y;
		break;
	    case MouseInput.DRAG:
				// if the Shift key is down
		if ((mi.modifiers & DeviceInput.SHIFT_MASK) != 0){
		    // go fast
		    headingDelta = -(mi.x-startX)/200f;				                
		    speed = (mi.y-startY)/10f;				      
		}
		else {
		    // otherwise go slower
		    headingDelta = -(mi.x-startX)/400f;
		    speed = (mi.y-startY)/20f;				      
		}
		break;
	    case MouseInput.UP:
				// stop
		headingDelta = 0;
		speed = 0;
		break;
				
	    }
	}
	// return false, let other entities handle the events too.
	return false;
    }
	

    /**
	 * Do camera stuff before rendering
	 */
    public void onPreRender(Renderer r, Object userData){
		
	// adjust the camera heading according to user input
	cameraHeading += headingDelta/getFramesPerSecond();
	// set the camera orientation to be an axis/angle rotation equivalent to
	// the new heading.
	cameraQuat.setEulers(cameraHeading, 0, 0);
	float[] tempOr = new float[4];
	cameraQuat.getAxisAngle(tempOr);
	camera.orientation.setValue(tempOr);
		
	// Determine the proposed camera motion according to user input
	float[] cameraMotion = new float[3];
	cameraMotion[2] = (float)(speed/getFramesPerSecond());
	cameraQuat.xform(cameraMotion);

	// Collisions are done at the level of an imaginary collider located below
	// the camera at a level of collideHeight.  Calculate the old collider location
	// and the proposed new collider location.
	float[] oldColliderLoc = new float[3];
	oldColliderLoc[0] = camera.position.getValue()[0];
	oldColliderLoc[1] = camera.position.getValue()[1] - avatarHeight + collideHeight;
	oldColliderLoc[2] = camera.position.getValue()[2];
		
	float[] newColliderLoc = new float[3];
	newColliderLoc[0] = oldColliderLoc[0] + cameraMotion[0];
	newColliderLoc[1] = oldColliderLoc[1] + cameraMotion[1];
	newColliderLoc[2] = oldColliderLoc[2] + cameraMotion[2];

	// check for collision in moving knee from old to new location
	if (!checkCollision(oldColliderLoc, newColliderLoc)){
	    // If no collision, move camera by the given amount.
	    camera.position.set1Value(0, camera.position.getValue()[0] + cameraMotion[0]);
	    camera.position.set1Value(1, camera.position.getValue()[1] + cameraMotion[1]);
	    camera.position.set1Value(2, camera.position.getValue()[2] + cameraMotion[2]);
	}

	//
	// Boids callback
	//
	boidsFrame();
    }

    void boidsFrame() {
	int active=0;
	int i;
	int proximate=0;
	float time = 1.0f/(float)getFramesPerSecond(); // XX re-calculate this
	float length;

	float center[] = new float[3];
	float lcenter[] = new float[3];
	float inertia[] = new float[3];
	float tmp[] = new float[3];

	center[0] = 0.0f; center[1] = 0.0f; center[2] = 0.0f;
	inertia[0] = 0.0f; inertia[1] = 0.0f; inertia[2] = 0.0f;

	/* Calculate center and inertia */
	for (i=0;i<boids.length;i++) {
	    if (boids_resting[i] <= 0.0f) {
		center[0] += boids_pos[i][0];
		center[1] += boids_pos[i][1];
		center[2] += boids_pos[i][2];
		inertia[0] += boids_vel[i][0];
		inertia[1] += boids_vel[i][1];
		inertia[2] += boids_vel[i][2];
		active++;
	    }
	}
    
	if (active!=0) {
	    center[0] /= (float) active;
	    center[1] /= (float) active;
	    center[2] /= (float) active;

	    inertia[0] *= inertiaFactor*time/ (float) active;
	    inertia[1] *= inertiaFactor*time/ (float) active;
	    inertia[2] *= inertiaFactor*time/ (float) active;
	}
	
    
	for (i=0;i<boids.length;i++) {
	    if (boids_resting[i] > 0.0f) {
		boids_resting[i] -= time;
	    }
	    else {
	    
		/* Flock pull */
		tmp[0] = center[0];
		tmp[1] = center[1];
		tmp[2] = center[2];
		tmp[0] -= boids_pos[i][0];
		tmp[1] -= boids_pos[i][1];
		tmp[2] -= boids_pos[i][2];
		tmp[0] *= pullFactor*time;
		tmp[1] *= pullFactor*time;
		tmp[2] *= pullFactor*time;

		boids_vel[i][0] += tmp[0];
		boids_vel[i][1] += tmp[1];
		boids_vel[i][2] += tmp[2];
	    
		/* Flock inertia */
		boids_vel[i][0] += inertia[0];
		boids_vel[i][1] += inertia[1];
		boids_vel[i][2] += inertia[2];
	    
		/* Neighbour avoidance */
		proximate = 0;
	    
		lcenter[0] = 0.0f; lcenter[1] = 0.0f; lcenter[2] = 0.0f; 

		for (int j=0; j< boids.length; j++) {
		    if (i!=j) {
			tmp[0] = boids_pos[i][0] - boids_pos[j][0];
			tmp[1] = boids_pos[i][1] - boids_pos[j][1];
			tmp[2] = boids_pos[i][2] - boids_pos[j][2];

			length = (float)Math.sqrt((tmp[0]*tmp[0]) + 
						  (tmp[1]*tmp[1]) + 
						  (tmp[2]*tmp[2]));
			if (length < proximityDistance) {
			    proximate++;
			    lcenter[0] += tmp[0];
			    lcenter[1] += tmp[1];
			    lcenter[2] += tmp[2];
			}
		    }
		}
		if (proximate !=0 ) {
		    lcenter[0] *= proximityFactor*time;
		    lcenter[1] *= proximityFactor*time;
		    lcenter[2] *= proximityFactor*time;
		    boids_vel[i][0] += lcenter[0];
		    boids_vel[i][1] += lcenter[1];
		    boids_vel[i][2] += lcenter[2];
		}
	    
		/* Calc new position */
		float l = (float)Math.sqrt((boids_vel[i][0]*boids_vel[i][0]) +
					   (boids_vel[i][1]*boids_vel[i][1]) +
					   (boids_vel[i][2]*boids_vel[i][2]));
		if (l > maxVelocity) {
		    boids_vel[i][0] *= maxVelocity/l;
		    boids_vel[i][1] *= maxVelocity/l;
		    boids_vel[i][2] *= maxVelocity/l;
		}
	    
		tmp[0] = boids_vel[i][0]; 
		tmp[1] = boids_vel[i][1]; 
		tmp[2] = boids_vel[i][2]; 
		tmp[0] *= time;
		tmp[1] *= time;
		tmp[2] *= time;
		boids_pos[i][0] += tmp[0];
		boids_pos[i][1] += tmp[1];
		boids_pos[i][2] += tmp[2];
	    
		/* Flock bounds */
		if (boids_pos[i][0] > (flockBoundCenter[0] + 
				       flockBoundSize[0])) {
		    boids_pos[i][0] = (flockBoundCenter[0] + 
				       flockBoundSize[0]);
		    boids_vel[i][0] = - Math.abs(boids_vel[i][0]);
		}
	    
		if (boids_pos[i][0] < (flockBoundCenter[0] - 
				       flockBoundSize[0])) {
		    boids_pos[i][0] = (flockBoundCenter[0] - 
				       flockBoundSize[0]);
		    boids_vel[i][0] =  Math.abs(boids_vel[i][0]);
		}
	    
		if (boids_pos[i][2] > (flockBoundCenter[2] + 
				       flockBoundSize[2])) {
		    boids_pos[i][2] = (flockBoundCenter[2] + 
				       flockBoundSize[2]);
		    boids_vel[i][2] = - Math.abs(boids_vel[i][2]);
		}
	    
		if (boids_pos[i][2] < (flockBoundCenter[2] - 
				       flockBoundSize[2])) {
		    boids_pos[i][2] = (flockBoundCenter[2] - 
				       flockBoundSize[2]);
		    boids_vel[i][2] = Math.abs(boids_vel[i][2]);
		}
	    
		if (boids_pos[i][1] > (flockBoundCenter[1] + 
				       flockBoundSize[1])) {
		    boids_pos[i][1] = (flockBoundCenter[1] + 
				       flockBoundSize[1]);
		    boids_vel[i][1] = -Math.abs(boids_vel[i][1]);
		}
	    
		if (boids_pos[i][1] < (flockBoundCenter[1] - 
				       flockBoundSize[1])) {
		    boids_pos[i][1] = (flockBoundCenter[1] - 
				       flockBoundSize[1]);
		    boids_vel[i][1] = Math.abs(boids_vel[i][1]);
		    if (canPerch) {
			boids_resting[i] = perchTime + 
			    (rnd.nextFloat() * perchVariable);
		    }
		}
	    
		float rot[] = new float[4];
		rot[0] = 0.0f;
		rot[1] = 1.0f;
		rot[2] = 0.0f;
		rot[3] = (float)Math.atan2(-boids_vel[i][0], -boids_vel[i][2]);
		boids[i].rotation.setValue(rot);
		
		if (boid_timers[i] != null) {
		    /* Test if rising or falling */
		    if (boids[i].translation.getValue()[1] <= 
			boids_pos[i][1]) {
			boid_timers[i].start();
		    }
		    else {
			boid_timers[i].stop();
		    }
		}
		
		/* The reason for the copy - we do actually need the
                   previous value so we can do the calculation above */
		boids[i].translation.set1Value(0, boids_pos[i][0]);
		boids[i].translation.set1Value(1, boids_pos[i][1]);
		boids[i].translation.set1Value(2, boids_pos[i][2]);
	    }
	}
    }

    // cache a picker
    Picker picker;
	
    /**
	 * Check to see if the motion has resulted in a collision
	 * 
	 * @return whether there is a collision
	 */
	
    boolean checkCollision(float[] oldPos, float[] newPos){
	if (picker == null) {
	    // Get a new picker 
	    picker = getNewPicker();
	    // set the picker to provide the hit point
	    picker.setPickInfo(Picker.POINT, true);
	    // set the picker's scene to be while scene
	    picker.setScene(getScene());
	}
	// This returns the path to the geometry that has the closest intersection 
	// to oldPos that lies along the directed ray from oldPos to newPos.
	// If there is no geometry that intersects that ray, results will be null.
	Node[] results = picker.pickClosestFromTo(oldPos, newPos);
		
	if (results == null) {
	    // Nothing in the way, no collision.
	    return false;
	}
	else {
	    // Retrieve the point of intersection from the picker.
	    float[] intersection = picker.getPickInfo(Picker.POINT);
			
	    // Find the distance between oldPos/newPos and oldPos/intersection
	    float oldToNewDist   = getDistance(oldPos, newPos);
	    float oldToInterDist = getDistance(oldPos, intersection);
			
	    // If newPos is further from oldPos than the intersection, you'd collide with
	    // the geometry in travelling from oldPos to newPos, so return true
	    if (oldToNewDist >= oldToInterDist)
		return true;
			
	    // If the difference in the distances is less than avatarRadius, the newPos is too
	    // close to the intersection for the avatar to fit.  That counts as a collision, return true
	    if ((oldToInterDist - oldToNewDist) < avatarRadius)
		return true;
	}
	// There's an intersection point but it is not located where it would cause a collision.
	return false;
    }
	
    // Distance function
    float getDistance(float[] from, float[] to){
	float x = from[0] - to[0];
	float y = from[1] - to[1];
	float z = from[2] - to[2];
	float dist = (float)Math.sqrt(x*x + y*y + z*z);
	//System.out.println(dist);
	return dist;
    }
	
    public void onPostRender(Renderer r, Object userData){
    }

}