package cz.cuni.amis.pogamut.base3d.worldview.objects;

import javax.vecmath.Tuple3d;
import javax.vecmath.Point3d;

/**
 * Location within the world.
 *
 * Location is represented as a point within the world's coordinates.
 *
 * @author Juraj 'Loque' Simlovic
 */
public class Location implements ILocated
{
	/** X coordinate. */
	public double x = 0;
	/** Y coordinate. */
	public double y = 0;
	/** Z coordinate. */
	public double z = 0;

	/* ********************************************************************** */

	/**
	 * X coordinate.
	 * @return X coordinate.
	 */
	public double getX()
	{
		return x;
	}

	/**
	 * Y coordinate.
	 * @return Y coordinate.
	 */
	public double getY()
	{
		return y;
	}

	/**
	 * Z coordinate.
	 * @return Z coordinate.
	 */
	public double getZ()
	{
		return z;
	}

	/* ********************************************************************** */

	/**
	 * Retreives sum of this location and given location.
	 * @param l Location to be added to this location.
	 * @return Sum of the two locations.
	 */
	public Location add (Location l)
	{
		// create sum of the locations
		return new Location (x + l.x, y + l.y, z + l.z);
	}

	/**
	 * Retreives sum of two given locations.
	 * @param l1 First location to be summed.
	 * @param l2 Second location to be summed.
	 * @return Sum of the two locations.
	 */
	public static Location add (Location l1, Location l2)
	{
		// create sum of the locations
		return new Location (l1.x + l2.x, l1.y + l2.y, l1.z + l2.z);
	}

	/**
	 * Retreives subtraction of given location from this location.
	 * @param l Location to be subtracted.
	 * @return Subtraction of the two locations.
	 */
	public Location sub (Location l)
	{
		// create substraction of the locations
		return new Location (x - l.x, y - l.y, z - l.z);
	}

	/**
	 * Retreives subtraction of two given locations.
	 * @param l1 Location to be subtracted from.
	 * @param l2 Location to be subtracted.
	 * @return Subtraction of the two locations.
	 */
	public static Location sub (Location l1, Location l2)
	{
		// create substraction of the locations
		return new Location (l1.x - l2.x, l1.y - l2.y, l1.z - l2.z);
	}

	/* ********************************************************************* */

	/**
	 * Adds given velocity to this location.
	 * @param v Velocity to be added to this location.
	 * @return Sum of the location and velocity.
	 */
	public Location add (Velocity v)
	{
		// create sum of the locations
		return new Location (x + v.x, y + v.y, z + v.z);
	}

	/**
	 * Adds given velocity to given location.
	 * @param l Location to be summed.
	 * @param v Velocity to be summed.
	 * @return Sum of the location and velocity.
	 */
	public static Location add (Location l, Velocity v)
	{
		// create sum of the locations
		return new Location (l.x + v.x, l.y + v.y, l.z + v.z);
	}

	/**
	 * Subtracts given velocity from this location.
	 * @param v Velocity to be subtracted.
	 * @return Subtraction of the velocity from the location.
	 */
	public Location sub (Velocity v)
	{
		// create substraction of the locations
		return new Location (x - v.x, y - v.y, z - v.z);
	}

	/**
	 * Subtracts given velocity from given location.
	 * @param l Location to be subtracted from.
	 * @param v Velocity to be subtracted.
	 * @return Subtraction of the velocity from the location.
	 */
	public static Location sub (Location l, Velocity v)
	{
		// create substraction of the locations
		return new Location (l.x - v.x, l.y - v.y, l.z - v.z);
	}

	/* ********************************************************************* */

	/**
	 * Scales values of all three coordinates by given multiplier.
	 * @param d Scaling multiplier.
	 * @return Location with all three coordinates negated.
	 */
	public Location scale (double d)
	{
		// create location with scaled values
		return new Location (x * d, y * d, z * d);
	}

	/* ********************************************************************* */

	/**
	 * Linearly interpolates between this location and given location.
	 * @param l Location to be interpolated to.
	 * @param d Interpolation parameter.
	 * @return Linear interpolation between the two locations.
	 */
	public Location interpolate(Location l, double d)
	{
		// from the other side
		double d1 = 1.0D - d;
		// create interpolation of the locations
		return new Location (d1 * x + d * l.x, d1 * y + d * l.y, d1 * z + d * l.z);
	}

	/**
	 * Linearly interpolates between two given locations.
	 * @param l1 Location to be interpolated from.
	 * @param l2 Location to be interpolated to.
	 * @param d Interpolation parameter.
	 * @return Linear interpolation between the two locations.
	 */
	public static Location interpolate(Location l1, Location l2, double d)
	{
		// from the other side
		double d1 = 1.0D - d;
		// create interpolation of the locations
		return new Location (d1 * l1.x + d * l2.x, d1 * l1.y + d * l2.y, d1 * l1.z + d * l2.z);
	}

	/* ********************************************************************** */

	/**
	 * Tells, whether this location equals to given location.
	 * @param v Location to be compared with.
	 * @return True, if the locations have the same values of all three
	 * corresponding coordinates.
	 */
	public boolean equals (Location v)
	{
		// test all three elements
		return (x == v.x) && (y == v.y) && (z == v.z);
	}

	/**
	 * Tells, whether two given locations equal.
	 * @param l1 First location to comapre.
	 * @param l2 Second location to comapre.
	 * @return True, if the locations have the same values of all three
	 * corresponding coordinates.
	 */
	public static boolean equal (Location l1, Location l2)
	{
		// test all three elements
		return (l1.x == l2.x) && (l1.y == l2.y) && (l1.z == l2.z);
	}

	/**
	 * Tells, whether the distance between coordinates of this location
	 * and given location is less than or equal to the given epsilon.
	 * @param l Location to comapre with.
	 * @param epsilon Epsilon to compare with.
	 * @return True, if the distance between the locations is less than the
	 * epsilon, false otherwise.
	 */
	public boolean equals (Location l, double epsilon)
	{
		double d;

		// x axes distance
		d = x - l.x;
		if((d >= 0 ? d : -d) > epsilon)
			return false;

		// y axes distance
		d = y - l.y;
		if((d >= 0.0D ? d : -d) > epsilon)
			return false;

		// z axes distance
		d = z - l.z;
		if((d >= 0.0D ? d : -d) > epsilon)
			return false;

		// aye, aye, sir..
		return true;
	}

	/**
	 * Tells, whether the distance between coordinates of two given locations
	 * is less than or equal to the given epsilon.
	 * @param l1 First location to comapre.
	 * @param l2 Second location to comapre.
	 * @param epsilon Epsilon to compare with.
	 * @return True, if the distance between the locations is less than the
	 * epsilon, false otherwise.
	 */
	public static boolean equal (Location l1, Location l2, double epsilon)
	{
		double d;

		// x axes distance
		d = l1.x - l2.x;
		if((d >= 0 ? d : -d) > epsilon)
			return false;

		// y axes distance
		d = l1.y - l2.y;
		if((d >= 0.0D ? d : -d) > epsilon)
			return false;

		// z axes distance
		d = l1.z - l2.z;
		if((d >= 0.0D ? d : -d) > epsilon)
			return false;

		// aye, aye, sir..
		return true;
	}

	/* ********************************************************************** */

	/**
	 * Calculates the distance between this and given location.
	 * @param l Location to be calculated the distance to.
	 * @return Euclidean distance between the two locations.
	 */
	public double getDistance(Location l)
	{
		double dx = l.x - x;
		double dy = l.y - y;
		double dz = l.z - z;
		return Math.sqrt(dx * dx + dy * dy + dz * dz);
	}

	/**
	 * Calculates the distance between two given locations.
	 * @param l1 Location to be calculated the distance from.
	 * @param l2 Location to be calculated the distance to.
	 * @return Euclidean distance between the two locations.
	 */
	public static double getDistance(Location l1, Location l2)
	{
		double dx = l2.x - l1.x;
		double dy = l2.y - l1.y;
		double dz = l2.z - l1.z;
		return Math.sqrt(dx * dx + dy * dy + dz * dz);
	}

	/**
	 * Calculates the square of the distance between this and given location.
	 * @param l Location to be calculated the distance to.
	 * @return Square of the euclidean distance between the two locations.
	 */
	public double getDistanceSquare(Location l)
	{
		double dx = l.x - x;
		double dy = l.y - y;
		double dz = l.z - z;
		return dx * dx + dy * dy + dz * dz;
	}

	/**
	 * Calculates the square of the distance between two given locations.
	 * @param l1 Location to be calculated the distance from.
	 * @param l2 Location to be calculated the distance to.
	 * @return Square of the euclidean distance between the two locations.
	 */
	public static double getDistanceSquare(Location l1, Location l2)
	{
		double dx = l2.x - l1.x;
		double dy = l2.y - l1.y;
		double dz = l2.z - l1.z;
		return dx * dx + dy * dy + dz * dz;
	}

	/**
	 * Calculates the Manhattan distance between this and given location.
	 * @param l Location to be calculated the distance to.
	 * @return Manhattan (i.e. 1-norm) distance between the two locations.
	 */
	public double getDistanceL1(Location l)
	{
		double dx = Math.abs(l.x - x);
		double dy = Math.abs(l.y - y);
		double dz = Math.abs(l.z - z);
		return dx + dy + dz;
	}

	/**
	 * Calculates the Manhattan distance between two given locations.
	 * @param l1 Location to be calculated the distance from.
	 * @param l2 Location to be calculated the distance to.
	 * @return Manhattan (i.e. 1-norm) distance between the two locations.
	 */
	public static double getDistanceL1(Location l1, Location l2)
	{
		double dx = Math.abs(l2.x - l1.x);
		double dy = Math.abs(l2.y - l1.y);
		double dz = Math.abs(l2.z - l1.z);
		return dx + dy + dz;
	}

	/**
	 * Calculates the Chebyshev distance between this and given location.
	 * @param l Location to be calculated the distance to.
	 * @return Chebyshev (i.e. infinity-norm) distance between the two locations.
	 */
	public double getDistanceLinf(Location l)
	{
		double dx = Math.abs(l.x - x);
		double dy = Math.abs(l.y - y);
		double dz = Math.abs(l.z - z);
		return Math.max(Math.max(dx, dy), dz);
	}

	/**
	 * Calculates the Chebyshev distance between two given locations.
	 * @param l1 Location to be calculated the distance from.
	 * @param l2 Location to be calculated the distance to.
	 * @return Chebyshev (i.e. infinity-norm) distance between the two locations.
	 */
	public static double getDistanceLinf(Location l1, Location l2)
	{
		double dx = Math.abs(l2.x - l1.x);
		double dy = Math.abs(l2.y - l1.y);
		double dz = Math.abs(l2.z - l1.z);
		return Math.max(Math.max(dx, dy), dz);
	}

	/**
	 * Calculates the distance between this and given location after being
	 * projected to the (x,y) plane.
	 * @param l Location to be calculated the distance to.
	 * @return Plane-projected distance between the two locations.
	 */
	public double getDistancePlane(Location l)
	{
		double dx = l.x - x;
		double dy = l.y - y;
		return Math.sqrt(dx * dx + dy * dy);
	}

	/**
	 * Calculates the distance between two given locations after being
	 * projected to the (x,y) plane.
	 * @param l1 Location to be calculated the distance from.
	 * @param l2 Location to be calculated the distance to.
	 * @return Plane-projected distance between the two locations.
	 */
	public static double getDistancePlane(Location l1, Location l2)
	{
		double dx = l2.x - l1.x;
		double dy = l2.y - l1.y;
		return Math.sqrt(dx * dx + dy * dy);
	}

	/* ********************************************************************** */

	/**
	 * Retreives the location itself to implement {@link ILocated}.
	 * @return The location itself (note: does not create a copy).
	 */
	@Override
	public Location getLocation()
	{
		return this;
	}

	/**
	 * Retreives javax.vecmath.Point3d representation of the location.
	 * @return javax.vecmath.Point3d representation with x, y and z values set.
	 */
	public Point3d getPoint3d()
	{
		return new Point3d (x, y, z);
	}

	/* ********************************************************************** */

	/**
	 * Creates location with all values set to zeroes.
	 */
	public Location()
	{
	}

	/**
	 * Creates location with specified coordinates.
	 * @param x X coordinate.
	 * @param y Y coordinate.
	 * @param z Z coordinate.
	 */
	public Location(double x, double y, double z)
	{
		this.x = x;
		this.y = y;
		this.z = z;
	}

	/**
	 * Creates location with specified planar coordinates. Sets z to zero.
	 * @param x X coordinate.
	 * @param y Y coordinate.
	 */
	public Location(double x, double y)
	{
		this.x = x;
		this.y = y;
	}

	/**
	 * Creates location from array of three doubles.
	 * Sets x = d[0], y = d[1] and z = d[2].
	 * @param d Array of (at least) three doubles to be used for creation.
	 */
	public Location(double d[])
	{
		this.x = d[0];
		this.y = d[1];
		this.z = d[2];
	}

	/**
	 * Creates location from specified 3D point.
	 * @param p Point in space to be used for creation.
	 */
	public Location(Tuple3d p)
	{
		this.x = p.x;
		this.y = p.y;
		this.z = p.z;
	}
	
	/**
	 * Converts Location into Rotation.
	 * Since location is only a single vector, roll is ommited.
	 * TODO roll pitch yaw (as far as i remember
	 */
	public Rotation getRotation()
	{
		Location XYprojection = new Location(this.x, this.y, 0);
		Location XZprojection = new Location(this.x, 0, this.z);
		// simple eh?
		XYprojection = XYprojection.getNormalized();
		double yaw = Math.acos(XYprojection.x);
		// turn left or right?
		yaw = (XYprojection.y >= 0)?yaw:-yaw;
				
		XZprojection = XZprojection.getNormalized();
		double pitch = Math.acos(XYprojection.x);
		// turn left or right?
		pitch = (XYprojection.y >= 0)?pitch:-pitch;
		
		return new Rotation(yaw, 0, pitch);
	}
	
	public Location getNormalized()
	{
		return this.scale(this.getLength());
	}
	
	public double getLength()
	{
		return Math.sqrt(x*x + y*y + z*z);
	}

	/* ********************************************************************** */

	@Override
	public String toString()
	{
		return String.format ("[%.2f, %.2f, %.2f]", x, y, z);
	}

}
