using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using UnityEngine;

	public static class Trajectory
	{

		// Given an object with maximum scalar acceleration, current vector position currentPosition, target vector position targetPosition, and target vector finalVelocity
		// Return the acceleration vector to maximum acceleration that moves P to T without overshooting if possible.
		public static Vector3 GetThrustInterceptVectorIndependentAxis(float maxAcceleration, Vector3 currentPosition, Vector3 targetPosition, Vector3 currentVelocity, Vector3 finalVelocity)
		{
			if (maxAcceleration <= .0001f)
				return Vector3.zero;

			// What initial velocity at distance s with acceleration aMax will result in a final velocity of 0?
			// Let u be the initial velocity in the frame of reference of the source
			// v*v = u*u + 2 * a * s
			// sqrt(-2as) = u
			Vector3 s = targetPosition - currentPosition;
			Vector3 aMax = new Vector3(maxAcceleration * Mathf.Sign(s.x), maxAcceleration * Mathf.Sign(s.y), maxAcceleration * Mathf.Sign(s.z));
			Vector3 deacceleration = -aMax;
			Vector3 uSq = -2.0f * Vector3.Scale(deacceleration, s);
			Vector3 u = new Vector3(Mathf.Sqrt(uSq.x) * Mathf.Sign(s.x), Mathf.Sqrt(uSq.y) * Mathf.Sign(s.y), Mathf.Sqrt(uSq.z) * Mathf.Sign(s.z));
			// Put u in the global frame of reference
			Vector3 maxGlobalVelocityTowardsTarget = u + finalVelocity;
			Vector3 velocityDeltaFromCurrentToIntended = maxGlobalVelocityTowardsTarget - currentVelocity;

			if (Mathf.Abs(velocityDeltaFromCurrentToIntended.x) < .5f)
				velocityDeltaFromCurrentToIntended.x = 0.0f;
			if (Mathf.Abs(velocityDeltaFromCurrentToIntended.y) < .5f)
				velocityDeltaFromCurrentToIntended.y = 0.0f;
			if (Mathf.Abs(velocityDeltaFromCurrentToIntended.z) < .5f)
				velocityDeltaFromCurrentToIntended.z = 0.0f;

			Vector3 accelerationDelta = velocityDeltaFromCurrentToIntended / Time.fixedDeltaTime;
			accelerationDelta.x = Mathf.Min(maxAcceleration, Mathf.Abs(accelerationDelta.x)) * Mathf.Sign(accelerationDelta.x);
			accelerationDelta.y = Mathf.Min(maxAcceleration, Mathf.Abs(accelerationDelta.y)) * Mathf.Sign(accelerationDelta.y);
			accelerationDelta.z = Mathf.Min(maxAcceleration, Mathf.Abs(accelerationDelta.z)) * Mathf.Sign(accelerationDelta.z);

			return accelerationDelta;
		}

		// Given an object with maximum scalar acceleration, current vector position currentPosition, target vector position targetPosition, and target vector finalVelocity
		// Return the acceleration vector to maximum acceleration that moves P to T without overshooting if possible.
		public static Vector3 GetThrustInterceptVector(float maxAcceleration, Vector3 currentPosition, Vector3 targetPosition, Vector3 currentVelocity, Vector3 finalVelocity)
		{
			if (maxAcceleration <= .0001f)
				return Vector3.zero;

			// What initial velocity at distance s with acceleration aMax will result in a final velocity of 0?
			// Let u be the initial velocity in the frame of reference of the source
			// v*v = u*u + 2 * a * s
			// sqrt(-2as) = u
			Vector3 s = targetPosition - currentPosition;
			float sMagnitude = s.magnitude;
			float u = Mathf.Sqrt(-2 * -maxAcceleration * sMagnitude);
			Vector3 velocityTowardsTarget;
			if (sMagnitude > .0001f)
				velocityTowardsTarget = u * s / sMagnitude + finalVelocity;
			else
				velocityTowardsTarget = finalVelocity;
			// If the change in velocity for the given distance would result in overshoot with the fixed timestamp, reduce it
			float velocityTowardsTargetMagnitude = velocityTowardsTarget.magnitude;
			if (velocityTowardsTargetMagnitude > .0001f && velocityTowardsTargetMagnitude * Time.fixedDeltaTime > sMagnitude)
				velocityTowardsTarget = (sMagnitude / Time.fixedDeltaTime) * ( velocityTowardsTarget / velocityTowardsTargetMagnitude );
			Vector3 thrustVelocity = velocityTowardsTarget - currentVelocity;
			Vector3 accelerationDelta = thrustVelocity / Time.fixedDeltaTime;
			return Vector3.ClampMagnitude(accelerationDelta, maxAcceleration);
		}

	}