using UnityEngine;
using System.Collections;

namespace Pathfinding.Util {
	public static class MovementUtilities {
		/** Clamps the velocity to the max speed and optionally the forwards direction.
		 * \param velocity Desired velocity of the character. In world units per second.
		 * \param maxSpeed Max speed of the character. In world units per second.
		 * \param slowdownFactor Value between 0 and 1 which determines how much slower the character should move than normal.
		 *      Normally 1 but should go to 0 when the character approaches the end of the path.
		 * \param slowWhenNotFacingTarget Prevent the velocity from being too far away from the forward direction of the character
		 *      and slow the character down if the desired velocity is not in the same direction as the forward vector.
		 * \param forward Forward direction of the character. Used together with the \a slowWhenNotFacingTarget parameter.
		 *
		 * Note that all vectors are 2D vectors, not 3D vectors.
		 *
		 * \returns The clamped velocity in world units per second.
		 */
		public static Vector2 ClampVelocity (Vector2 velocity, float maxSpeed, float slowdownFactor, bool slowWhenNotFacingTarget, Vector2 forward) {
			// Max speed to use for this frame
			var currentMaxSpeed = maxSpeed * slowdownFactor;

			// Check if the agent should slow down in case it is not facing the direction it wants to move in
			if (slowWhenNotFacingTarget && (forward.x != 0 || forward.y != 0)) {
				float currentSpeed;
				var normalizedVelocity = VectorMath.Normalize(velocity, out currentSpeed);
				float dot = Vector2.Dot(normalizedVelocity, forward);

				// Lower the speed when the character's forward direction is not pointing towards the desired velocity
				// 1 when velocity is in the same direction as forward
				// 0.2 when they point in the opposite directions
				float directionSpeedFactor = Mathf.Clamp(dot+0.707f, 0.2f, 1.0f);
				currentMaxSpeed *= directionSpeedFactor;
				currentSpeed = Mathf.Min(currentSpeed, currentMaxSpeed);

				// Angle between the forwards direction of the character and our desired velocity
				float angle = Mathf.Acos(Mathf.Clamp(dot, -1, 1));

				// Clamp the angle to 20 degrees
				// We cannot keep the velocity exactly in the forwards direction of the character
				// because we use the rotation to determine in which direction to rotate and if
				// the velocity would always be in the forwards direction of the character then
				// the character would never rotate.
				// Allow larger angles when near the end of the path to prevent oscillations.
				angle = Mathf.Min(angle, (20f + 180f*(1 - slowdownFactor*slowdownFactor))*Mathf.Deg2Rad);

				float sin = Mathf.Sin(angle);
				float cos = Mathf.Cos(angle);

				// Determine if we should rotate clockwise or counter-clockwise to move towards the current velocity
				sin *= Mathf.Sign(normalizedVelocity.x*forward.y - normalizedVelocity.y*forward.x);
				// Rotate the #forward vector by #angle radians
				// The rotation is done using an inlined rotation matrix.
				// See https://en.wikipedia.org/wiki/Rotation_matrix
				return new Vector2(forward.x*cos + forward.y*sin, forward.y*cos - forward.x*sin) * currentSpeed;
			} else {
				return Vector2.ClampMagnitude(velocity, currentMaxSpeed);
			}
		}

		/** Calculate an acceleration to move deltaPosition units and get there with approximately a velocity of targetVelocity */
		public static Vector2 CalculateAccelerationToReachPoint (Vector2 deltaPosition, Vector2 targetVelocity, Vector2 currentVelocity, float acceleration, float maxSpeed) {
			// If the target velocity is zero we can use a more fancy approach
			// and calculate a nicer path.
			// In particular, this is the case at the end of the path.
			if (targetVelocity == Vector2.zero) {
				// Run a binary search over the time to get to the target point.
				float mn = 0.01f;
				float mx = 10;
				float sqrAcceleration = acceleration*acceleration;
				while (mx - mn > 0.01f) {
					var time = (mx + mn) * 0.5f;

					// Given that we want to move deltaPosition units from out current position, that our current velocity is given
					// and that when we reach the target we want our velocity to be zero. Also assume that our acceleration will
					// vary linearly during the slowdown. Then we can calculate what our acceleration should be during this frame.

					//{ t = time
					//{ deltaPosition = vt + at^2/2 + qt^3/6
					//{ 0 = v + at + qt^2/2
					//{ solve for a
					// a = acceleration vector
					// q = derivative of the acceleration vector
					var a = (6*deltaPosition - 4*time*currentVelocity)/(time*time);
					var q = 6*(time*currentVelocity - 2*deltaPosition)/(time*time*time);

					// Make sure the acceleration is not greater than our maximum allowed acceleration.
					// If it is we increase the time we want to use to get to the target
					// and if it is not, we decrease the time to get there faster.
					// Since the acceleration is described by acceleration = a + q*t
					// we only need to check at t=0 and t=time.
					if (a.sqrMagnitude > sqrAcceleration || (a + q*time).sqrMagnitude > sqrAcceleration) {
						mn = time;
					} else {
						mx = time;
					}
				}

				var finalAcceleration = (6*deltaPosition - 4*mx*currentVelocity)/(mx*mx);

				// The trajectory calculated above has a tendency to use very wide arcs
				// and that does unfortunately not look particularly good in some cases.
				// Here we amplify the component of the acceleration that is perpendicular
				// to our current velocity. This will make the agent turn towards the
				// target quicker.
				var right = new Vector2(currentVelocity.y, -currentVelocity.x);
				// How much amplification to use. Value is unitless.
				const float Boost = 1f;
				finalAcceleration = Vector3.ClampMagnitude(finalAcceleration + (Boost * Vector3.Dot(finalAcceleration, right) / Mathf.Max(0.0001f, right.sqrMagnitude)) * right, acceleration);
				return finalAcceleration;
			} else {
				// Here we try to move towards the next waypoint which has been modified slightly using our
				// desired velocity at that point so that the agent will more smoothly round the corner.

				// How much to strive for making sure we reach the target point with the target velocity. Unitless.
				const float TargetVelocityWeight = 0.5f;

				// Limit to how much to care about the target velocity. Value is in seconds.
				// This prevents the character from moving away from the path too much when the target point is far away
				const float TargetVelocityWeightLimit = 1.5f;
				float currentSpeed = currentVelocity.magnitude;
				float targetSpeed;
				var normalizedTargetVelocity = VectorMath.Normalize(targetVelocity, out targetSpeed);

				var distance = deltaPosition.magnitude;
				var targetPoint = deltaPosition - normalizedTargetVelocity * System.Math.Min(TargetVelocityWeight * distance * targetSpeed / (currentSpeed + targetSpeed), maxSpeed*TargetVelocityWeightLimit);

				// How quickly the agent will try to reach the velocity that we want it to have.
				// We need this to prevent oscillations and jitter which is what happens if
				// we let the constant go towards zero. Value is in seconds.
				const float TimeToReachDesiredVelocity = 0.1f;
				// TODO: Clamp to ellipse using more accurate acceleration (use rotation speed as well)
				return Vector2.ClampMagnitude((targetPoint.normalized*maxSpeed - currentVelocity) * (1f/TimeToReachDesiredVelocity), acceleration);
			}
		}
	}
}
