/*
 * Created on 07-Aug-2004 by Ryan McNally
 */

package com.speckled.specksim.imp.motion;

import java.util.Collections;
import java.util.HashMap;
import java.util.Map;
import java.util.Random;

import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import com.ryanm.config.Configurator;
import com.ryanm.config.ValueListener;
import com.ryanm.config.imp.AnnotatedConfigurator;
import com.ryanm.config.imp.ConfigurableType;
import com.ryanm.config.imp.Description;
import com.ryanm.config.imp.NumberRange;
import com.ryanm.config.imp.Variable;
import com.speckled.specksim.MovementModel;
import com.speckled.specksim.Speck;
import com.speckled.specksim.SpeckPosition;

/**
 * Simulates a {@link MovementModel} where specks traverse an infinite
 * series of random checkpoints. All specks travel at the same speed.
 * For a given offset and {@link Speck}.id(), the same series of
 * checkpoints will be traversed. Speck orientations are initialised
 * to random values, and then are given a random rotational velocity
 * at each checkpoint.
 * 
 * @author ryanm
 */
@ConfigurableType( "Waypoint" )
public class WaypointMovementModel extends AbstractMovementModel
{
	/**
	 * The speed of motion, in units per second
	 */
	@Variable( "Speed" )
	@NumberRange( { 0, 2 } )
	@Description( "The movement speed of the specks, in units per second" )
	public float speed = 0;

	/**
	 * The maximum speed of rotation, in degrees per second
	 */
	@Variable( "Rotation" )
	@NumberRange( { 0, 360 } )
	@Description( "The maximum speed of rotation of the specks, in degrees per second" )
	public float rotSpeed = 0;

	/**
	 * Controls the size of the box from which waypoints are chosen
	 */
	private float range = 0.5f;

	/**
	 * Maps specks to their Itineraries
	 */
	private Map<Speck, Itinerary> speckRoutes =
			Collections.synchronizedMap( new HashMap<Speck, Itinerary>() );

	private Configurator conf = null;

	/**
	 * 
	 */
	public WaypointMovementModel()
	{
		super( "Waypoint" );
	}

	@Override
	public void initialise()
	{
		synchronized( speckRoutes )
		{
			speckRoutes.clear();
		}
	}

	@Override
	protected void deinitialise()
	{
		synchronized( speckRoutes )
		{
			speckRoutes.clear();
		}
	}

	@Override
	public SpeckPosition computeLocation( Speck speck, float time, int speckIndex, int population )
	{
		synchronized( speckRoutes )
		{
			Itinerary itin = speckRoutes.get( speck );

			if( itin == null )
			{
				itin = new Itinerary( speck );
				speckRoutes.put( speck, itin );
			}

			return new SpeckPosition( itin.getStateAt( time ) );
		}
	}

	@Override
	public Vector3d computeVelocity( Speck speck, float time, int speckIndex, int population )
	{
		synchronized( speckRoutes )
		{
			Itinerary itin = speckRoutes.get( speck );

			if( itin == null )
			{
				itin = new Itinerary( speck );
				speckRoutes.put( speck, itin );
			}

			Vector3d velocity = itin.getVelocity( time );

			if( is2D )
			{
				velocity.z = 0;
			}

			return velocity;
		}
	}

	@Override
	public double getMaxSpeed()
	{
		return speed;
	}

	/**
	 * The range has been altered, we need to re-evaluate all the
	 * waypoints
	 */
	private void rangeAltered()
	{
		for( Itinerary itin : speckRoutes.values() )
		{
			itin.rangeSet();
		}
	}

	/**
	 * Rebuilds the Random field of all itineraries
	 */
	private void refreshRandoms()
	{
		for( Itinerary itin : speckRoutes.values() )
		{
			itin.refreshRandom();
		}
	}

	@Override
	public Configurator getConfigurator()
	{
		if( conf == null )
		{
			conf = AnnotatedConfigurator.buildConfigurator( this );
			conf.addValueListener( new ValueListener() {
				public void valueChanged( Configurator conf, String name )
				{
					clearCache();

					if( name.equals( "RNG seed offset" ) )
					{
						refreshRandoms();
					}
				}
			}, false );
		}

		return conf;
	}

	/**
	 * Works out the path and rotation for a given speck for a given
	 * time
	 * 
	 * @author ryanm
	 */
	private class Itinerary
	{
		/**
		 * The id of the speck that this Itinerary controls
		 */
		private int speckId;

		/**
		 * The Random number generator for this itinerary
		 */
		private Random random;

		/**
		 * The next waypoint
		 */
		private Point3d next = new Point3d();

		/**
		 * The simulation time last time we checked this speck
		 */
		private float lastTime = 0;

		/**
		 * The position of the speck
		 */
		private SpeckPosition orient = new SpeckPosition();

		/**
		 * The rotational velocities
		 */
		private double rollv, elevv, headv;

		private Itinerary( Speck speck )
		{
			speckId = speck.id();

			random = new Random( speckId * rngSeedOffset );

			// throw away the first couple
			random.nextDouble();
			random.nextDouble();

			Point3d center = new Point3d( 0.5, 0.5, 0.5 );

			// choose starting position
			randomPoint( orient.position, center, 0.5f );

			// choose first waypoint
			randomPoint( next, orient.position, range );

			orient.orientation.roll = random.nextDouble() * Math.PI * 2;
			orient.orientation.elevation = random.nextDouble() * Math.PI * 2;
			orient.orientation.heading = random.nextDouble() * Math.PI * 2;

			rollv = 2 * random.nextDouble() - 1;
			elevv = 2 * random.nextDouble() - 1;
			headv = 2 * random.nextDouble() - 1;
		}

		/**
		 * Called when the range is altered. Will reevaluate the next
		 * waypoint if necessary
		 */
		public void rangeSet()
		{
			double dx = next.x - orient.position.x;
			double dy = next.y - orient.position.y;
			double dz = next.z - orient.position.z;

			if( dx > range )
			{
				dx = range;
			}
			if( dx < -range )
			{
				dx = -range;
			}
			if( dy > range )
			{
				dy = range;
			}
			if( dy < -range )
			{
				dy = -range;
			}
			if( dz > range )
			{
				dz = range;
			}
			if( dz < -range )
			{
				dz = -range;
			}

			next.x = orient.position.x + dx;
			next.y = orient.position.y + dy;
			next.z = orient.position.z + dz;
		}

		/**
		 * 
		 */
		public void refreshRandom()
		{
			random.setSeed( speckId * rngSeedOffset );

			// throw away the first couple
			random.nextDouble();
			random.nextDouble();
		}

		private SpeckPosition getStateAt( float time )
		{
			float elapsed = time - lastTime;

			if( elapsed != 0 )
			{ // we have work to do
				assert elapsed > 0.0f : speckId + " Cannot go back in time! : " + time + " < "
						+ lastTime;

				double distanceTravelled = elapsed * speed;
				double distanceToWaypoint = orient.position.distance( next );
				double rv = Math.toRadians( rotSpeed );

				while( distanceTravelled >= distanceToWaypoint )
				{
					// first get us to the waypoint
					float timeToWaypoint =
							( float ) ( ( distanceTravelled - distanceToWaypoint ) / speed );

					orient.orientation.roll = orient.orientation.roll + rollv * rv * timeToWaypoint;
					orient.orientation.elevation =
							orient.orientation.elevation + elevv * rv * timeToWaypoint;
					orient.orientation.heading += headv * rv * timeToWaypoint;
					orient.position.x = next.x;
					orient.position.y = next.y;
					orient.position.z = next.z;

					// generate a new waypoint and rotational
					// speeds
					assert range != 0;
					randomPoint( next, orient.position, range );
					rollv = 2 * random.nextDouble() - 1;
					elevv = 2 * random.nextDouble() - 1;
					headv = 2 * random.nextDouble() - 1;

					elapsed -= timeToWaypoint;

					// try again
					distanceTravelled -= distanceToWaypoint;
					distanceToWaypoint = orient.position.distance( next );
				}

				if( distanceTravelled < distanceToWaypoint )
				{
					double d = distanceTravelled / distanceToWaypoint;

					// update position
					orient.position.x += ( next.x - orient.position.x ) * d;
					orient.position.y += ( next.y - orient.position.y ) * d;
					orient.position.z += ( next.z - orient.position.z ) * d;

					// update rotation
					orient.orientation.roll = orient.orientation.roll + rollv * rv * elapsed;
					orient.orientation.elevation = orient.orientation.elevation + elevv * rv * elapsed;
					orient.orientation.heading += headv * rv * elapsed;
				}
			}

			lastTime = time;

			return orient;
		}

		/**
		 * Gets the velocity of this Itinerary at a given time
		 * 
		 * @param time
		 *           The time
		 * @return The velocity
		 */
		public Vector3d getVelocity( float time )
		{
			getState( time );
			Vector3d velocity = new Vector3d();
			velocity.sub( next, orient.position );

			velocity.normalize();
			velocity.scale( speed );

			return velocity;
		}

		/**
		 * Generates a random point that lies within a cube with sides
		 * of length 2 * range. Initially, the cube is centered at
		 * <code>center</code>, but if any part of the cube lies outside
		 * the unit cube lying from (0, 0, 0) to (1, 1, 1), it will be
		 * translated so that all of the cube lies within.
		 * 
		 * @param v
		 *           The vector to update with the generated components
		 * @param center
		 *           The center of the bounding cube
		 * @param range
		 *           The magnitude of the bounding cube
		 */
		private void randomPoint( Point3d v, Point3d center, float range )
		{
			double upperRange = 1 - range;

			double cx = center.x;
			double cy = center.y;
			double cz = center.z;

			if( cx < range )
			{
				cx = range;
			}
			if( cx > upperRange )
			{
				cx = upperRange;
			}
			if( cy < range )
			{
				cy = range;
			}
			if( cy > upperRange )
			{
				cy = upperRange;
			}
			if( cz < range )
			{
				cz = range;
			}
			if( cz > upperRange )
			{
				cz = upperRange;
			}

			double x = cx + range * ( 2 * random.nextDouble() - 1 );
			double y = cy + range * ( 2 * random.nextDouble() - 1 );
			double z = cz + range * ( 2 * random.nextDouble() - 1 );

			if( x < 0 )
			{
				x = 0;
			}
			if( x > 1 )
			{
				x = 1;
			}
			if( y < 0 )
			{
				y = 0;
			}
			if( y > 1 )
			{
				y = 1;
			}
			if( z < 0 )
			{
				z = 0;
			}
			if( z > 1 )
			{
				z = 1;
			}

			v.x = x;
			v.y = y;
			v.z = z;

			applyConstraints( v );
		}
	}

	/**
	 * Sets the maximum distance to the next waypoint
	 * 
	 * @param range
	 */
	@Variable( "Range" )
	@NumberRange( { 0.001f, 0.5f } )
	@Description( "Each waypoint will never be chosen from a cube of "
			+ "size 2 * range. The cube is ideally centered on the last "
			+ "waypoint but is always on the interior of the field" )
	public void setRange( float range )
	{
		this.range = range;
		rangeAltered();
	}

	/**
	 * @return the maximum distance to the next waypoint
	 */
	@Variable( "Range" )
	public float getRange()
	{
		return range;
	}
}