using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace NikRadford.Xna.Extensions
{
	public static class VectorExtensions
	{
		/// <summary>
		/// Given an end point, movement speed and elapsed time, returns a resultant position vector 
		/// that has moved towards the end point
		/// </summary>
		/// <param name="position">The current position</param>
		/// <param name="endPoint">The point to move towards</param>
		/// <param name="speed">Amount of units per second</param>
		/// <param name="elapsedTime">The amount of time passed</param>
		/// <returns>A position that has moved towards endPoint at a given speed over a period of time. </returns>
		public static Vector2 MoveToPoint(this Vector2 position, Vector2 endPoint, float speed, double elapsedTime)
		{
			if (position == endPoint)
				return position;

			// Work out the direction and how far we have to go to the end point
			Vector2 posToEnd = endPoint - position;
		 
			// Get the unit vector for this
			Vector2 normal = posToEnd;
			normal.Normalize();

			// based on the amount of time passed, and the speed work out how far we would move
			// in that time
			float distanceModifier = speed * (float)elapsedTime;

			// if the required amount of movement to reach the end of the line
			// is less than the distanceModifier, then change the distance modifier to only
			// be the required amount
			// (We also avoid doing a square root, in the condition as square roots eat processor time)
			if (posToEnd.LengthSquared() < (distanceModifier * distanceModifier))
				distanceModifier = posToEnd.Length();

			// Okay so we now how much we are going to move. Either as many units as the speed and time calculated
			// or the exact amount to reach the end of the line
			Vector2 velocity = normal * distanceModifier;

			return position + velocity;
		}

		/// <summary>
		/// Given an end point, movement speed and elapsed time, returns a resultant position vector 
		/// that has moved towards the end point
		/// </summary>
		/// <param name="position">The current position</param>
		/// <param name="endPoint">The point to move towards</param>
		/// <param name="speed">Amount of units per second</param>
		/// <param name="elapsedTime">The amount of time passed</param>
		/// <returns>A position that has moved towards endPoint at a given speed over a period of time. </returns>
		public static Vector3 MoveToPoint(this Vector3 position, Vector3 endPoint, float speed, double elapsedTime)
		{
			if (position == endPoint)
				return position;

			// Work out the direction and how far we have to go to the end point
			Vector3 posToEnd = endPoint - position;

			// Get the unit vector for this
			Vector3 normal = posToEnd;
			normal.Normalize();

			// based on the amount of time passed, and the speed work out how far we would move
			// in that time
			float distanceModifier = speed * (float)elapsedTime;

			// if the required amount of movement to reach the end of the line
			// is less than the distanceModifier, then change the distance modifier to only
			// be the required amount
			// (We also avoid doing a square root, in the condition as square roots eat processor time)
			if (posToEnd.LengthSquared() < (distanceModifier * distanceModifier))
				distanceModifier = posToEnd.Length();

			// Okay so we now how much we are going to move. Either as many units as the speed and time calculated
			// or the exact amount to reach the end of the line
			Vector3 velocity = normal * distanceModifier;

			return position + velocity;
		}

		/// <summary>
		/// Given an end point, movement speed and elapsed time, returns a resultant position vector 
		/// that has moved towards the end point
		/// </summary>
		/// <param name="position">The current position</param>
		/// <param name="endPoint">The point to move towards</param>
		/// <param name="speed">Amount of units per second</param>
		/// <param name="elapsedTime">The amount of time passed</param>
		/// <returns>A position that has moved towards endPoint at a given speed over a period of time. </returns>
		public static Vector4 MoveToPoint(this Vector4 position, Vector4 endPoint, float speed, double elapsedTime)
		{
			if (position == endPoint)
				return position;

			// Work out the direction and how far we have to go to the end point
			Vector4 posToEnd = endPoint - position;

			// Get the unit vector for this
			Vector4 normal = posToEnd;
			normal.Normalize();

			// based on the amount of time passed, and the speed work out how far we would move
			// in that time
			float distanceModifier = speed * (float)elapsedTime;

			// if the required amount of movement to reach the end of the line
			// is less than the distanceModifier, then change the distance modifier to only
			// be the required amount
			// (We also avoid doing a square root, in the condition as square roots eat processor time)
			if (posToEnd.LengthSquared() < (distanceModifier * distanceModifier))
				distanceModifier = posToEnd.Length();

			// Okay so we now how much we are going to move. Either as many units as the speed and time calculated
			// or the exact amount to reach the end of the line
			Vector4 velocity = normal * distanceModifier;

			return position + velocity;
		}
	}
}