using System.Collections.Generic;
using UnityEngine;

static class Ballistics
{
	// Get Position & Velocity of thrown object

	public static Vector2 GetPositionAtTime(Vector2 startVelocity, float gravity, float fullTime) =>
		new(startVelocity.x * fullTime, startVelocity.y * fullTime - 0.5f * gravity * fullTime * fullTime);

	public static Vector2 GetVelocityAtTime(Vector2 startVelocity, float gravity, float fullTime) =>
		new(startVelocity.x, startVelocity.y - gravity * fullTime);

	public static Vector2 GetPositionAtTime(Vector2 startPosition, Vector2 startVelocity, float gravity, float fullTime) =>
		GetPositionAtTime(startVelocity, gravity, fullTime) + startPosition;

	public static Vector3 GetPositionAtTime(Vector3 startVelocity, float gravity, float fullTime)
	{
		Vector2 statVelocity2d = startVelocity.FlattenTo2D();
		Vector2 position2d = GetPositionAtTime(statVelocity2d, gravity, fullTime);
		return position2d.ExtendTo3D(startVelocity);
	}

	public static Vector3 GetVelocityAtTime(Vector3 startVelocity, float gravity, float fullTime)
	{ 
		Vector2 statVelocity2d = startVelocity.FlattenTo2D();
		Vector2 position2d = GetVelocityAtTime(statVelocity2d, gravity, fullTime);
		return position2d.ExtendTo3D(startVelocity);
	}

	public static Vector3 GetPositionAtTime(Vector3 startPosition, Vector3 startVelocity,  float gravity, float fullTime)
	{
		Vector2 statVelocity2d = startVelocity.FlattenTo2D();
		Vector2 position2d = GetPositionAtTime(statVelocity2d, gravity, fullTime);
		return position2d.ExtendTo3D(startVelocity) + startPosition;
	}


	// Get Position & Velocity by simulation
	public static void GetPositionAndVelocitySimulated(Vector2 startPosition, Vector2 startVelocity, float gravity, float drag, float fullTime, float timeStep,
	out Vector2 position, out Vector2 velocity)
	{
		position = startPosition;
		velocity = startVelocity;
		if (timeStep <= 0) return;
		float currentTime = 0;
		for (int i = 0; currentTime < fullTime; i++)
		{
			float timeLeft = fullTime - currentTime;
			if (timeLeft < timeStep)
				timeStep = timeLeft;

			position += velocity * timeStep;
			velocity += gravity * timeStep * Vector2.down;
			velocity *= 1 - drag * timeStep;
			currentTime += timeStep;
		}
	}

	public static void GetPositionAndVelocitySimulated(Vector2 startVelocity, float gravity, float drag, float fullTime, float timeStep,
	out Vector2 position, out Vector2 velocity) =>
		GetPositionAndVelocitySimulated(Vector2.zero, startVelocity, gravity, drag, fullTime, timeStep, out position, out velocity);

	public static void GetPositionAndVelocitySimulated(Vector2 startPosition, Vector2 startVelocity, float gravity, float fullTime, float timeStep,
	out Vector2 position, out Vector2 velocity) =>
		GetPositionAndVelocitySimulated(startPosition, startVelocity, gravity, 0, fullTime, timeStep, out position, out velocity);

	public static void GetPositionAndVelocitySimulated(Vector2 startVelocity, float gravity, float fullTime, float timeStep,
		out Vector2 position, out Vector2 velocity) =>
		GetPositionAndVelocitySimulated(Vector2.zero, startVelocity, gravity, 0, fullTime, timeStep, out position, out velocity);


	public static void GetPositionAndVelocitySimulated(Vector3 startPosition, Vector3 startVelocity, float gravity, float drag, float fullTime, float timeStep,
	out Vector3 position, out Vector3 velocity)
	{ 
		Vector2 velocity2d = FlattenTo2D(startVelocity);

		GetPositionAndVelocitySimulated(Vector2.zero, velocity2d, gravity, drag, fullTime, timeStep, out Vector2 position2dOut, out Vector2 velocity2dOut);

		position = ExtendTo3D(position2dOut, startVelocity) + startPosition;
		velocity = ExtendTo3D(velocity2dOut, startVelocity);
	}

	public static void GetPositionAndVelocitySimulated(Vector3 startVelocity, float gravity, float fullTime, float timeStep,
		out Vector3 position, out Vector3 velocity) =>
		GetPositionAndVelocitySimulated(Vector3.zero, startVelocity, gravity, 0, fullTime, timeStep, out position, out velocity);
	public static void GetPositionAndVelocitySimulated(Vector3 startVelocity, float gravity, float drag, float fullTime, float timeStep,
		out Vector3 position, out Vector3 velocity) =>
		GetPositionAndVelocitySimulated(Vector3.zero, startVelocity, gravity, drag, fullTime, timeStep, out position, out velocity);
	public static void GetPositionAndVelocitySimulated(Vector3 startPosition, Vector3 startVelocity, float gravity, float fullTime, float timeStep,
		out Vector3 position, out Vector3 velocity) =>
		GetPositionAndVelocitySimulated(startPosition, startVelocity, gravity, 0, fullTime, timeStep, out position, out velocity);


	// Full Path Simulation, No Allocation  -------------------------------------------


	public static void GetPathSimulated(Vector2 startPoint, Vector2 startVelocity, float gravity, float drag, float fullTime, float timeStep, List<Vector2> path)
	{
		if (timeStep <= 0) return;
		path.Clear();
		path.Add(startPoint);
		Vector2 currentPoint = startPoint;
		Vector2 currentVelocity = startVelocity;
		float currentTime = 0;
		for (int i = 0; currentTime < fullTime; i++)
		{
			float timeLeft = fullTime - currentTime;
			if (timeLeft < timeStep)
				timeStep = timeLeft;

			currentPoint += currentVelocity * timeStep;
			path.Add(currentPoint);
			currentVelocity += gravity * timeStep * Vector2.down;
			currentVelocity *= 1 - drag * timeStep;
			currentTime += timeStep;
		}
	}

	public static void GetPathSimulated(Vector3 startPoint, Vector3 startVelocity, float gravity, float drag, float fullTime, float timeStep, List<Vector3> path)
	{
		if (timeStep <= 0) return;
		path.Clear();
		path.Add(startPoint);
		Vector3 currentPoint = startPoint;
		Vector3 currentVelocity = startVelocity;
		float currentTime = 0;
		for (int i = 0; currentTime < fullTime; i++)
		{
			float timeLeft = fullTime - currentTime;
			if (timeLeft < timeStep)
				timeStep = timeLeft;

			currentPoint += currentVelocity * timeStep;
			path.Add(currentPoint);
			currentVelocity += gravity * timeStep * Vector3.down;
			currentVelocity *= 1 - drag * timeStep;
			currentTime += timeStep;
		}
	}

	public static void GetPathSimulated(Vector2 startPoint, Vector2 startVelocity, float gravity, float drag, float fullTime, float timeStep, List<Ray> path)
	{
		if (timeStep <= 0) return;
		path.Clear();
		path.Add(new(startPoint, startVelocity));
		Vector2 currentPoint = startPoint;
		Vector2 currentVelocity = startVelocity;
		float currentTime = 0;
		for (int i = 0; currentTime < fullTime; i++)
		{
			float timeLeft = fullTime - currentTime;
			if (timeLeft < timeStep)
				timeStep = timeLeft;

			currentPoint += currentVelocity * timeStep;
			currentVelocity += gravity * timeStep * Vector2.down;
			currentVelocity *= 1 - drag * timeStep;
			currentTime += timeStep;
			path.Add(new(currentPoint, currentVelocity));
		}
	}

	public static void GetPathSimulated(Vector3 startPoint, Vector3 startVelocity, float gravity, float drag, float fullTime, float timeStep, List<Ray> path)
	{
		if (timeStep <= 0) return;
		path.Clear();
		path.Add(new(startPoint, startVelocity));
		Vector3 currentPoint = startPoint;
		Vector3 currentVelocity = startVelocity;
		float currentTime = 0;
		for (int i = 0; currentTime < fullTime; i++)
		{
			float timeLeft = fullTime - currentTime;
			if (timeLeft < timeStep)
				timeStep = timeLeft;

			currentPoint += currentVelocity * timeStep;
			currentVelocity += gravity * timeStep * Vector3.down;
			currentVelocity *= 1 - drag * timeStep;
			currentTime += timeStep;
			path.Add(new(currentPoint, currentVelocity));
		}
	}

	public static void GetPathSimulated(Vector2 startPoint, Vector2 startVelocity, float gravity, float fullTime, float timeStep, List<Vector2> path)
	=> GetPathSimulated(startPoint, startVelocity, gravity, 0, fullTime, timeStep, path);

	public static void GetPathSimulated(Vector3 startPoint, Vector3 startVelocity, float gravity, float fullTime, float timeStep, List<Vector3> path)
	=> GetPathSimulated(startPoint, startVelocity, gravity, 0, fullTime, timeStep, path);

	public static void GetPathSimulated(Vector2 startPoint, Vector2 startVelocity, float gravity, float fullTime, float timeStep, List<Ray> path)
	=> GetPathSimulated(startPoint, startVelocity, gravity, 0, fullTime, timeStep, path);

	public static void GetPathSimulated(Vector3 startPoint, Vector3 startVelocity, float gravity, float fullTime, float timeStep, List<Ray> path)
	=> GetPathSimulated(startPoint, startVelocity, gravity, 0, fullTime, timeStep, path);


	// Full Path Calculation, No Allocation  -------------------------------------------

	public static void GetPathExact(Vector2 startPoint, Vector2 startVelocity, float gravity, float fullTime, float timeStep, List<Vector2> path)
	{
		if (timeStep <= 0) return;
		path.Clear();
		path.Add(startPoint);
		float currentTime = 0;
		for (int i = 0; currentTime < fullTime; i++)
		{
			float timeLeft = fullTime - currentTime;
			if (timeLeft < timeStep)
				timeStep = timeLeft;

			currentTime += timeStep;
			Vector2 currentPoint = GetPositionAtTime(startPoint, startVelocity, gravity, currentTime);
			path.Add(currentPoint);
		}
	}

	public static void GetPathExact(Vector3 startPoint, Vector3 startVelocity, float gravity, float fullTime, float timeStep, List<Vector3> path)
	{
		if (timeStep <= 0) return;
		path.Clear();
		path.Add(startPoint);
		float currentTime = 0;
		for (int i = 0; currentTime < fullTime; i++)
		{
			float timeLeft = fullTime - currentTime;
			if (timeLeft < timeStep)
				timeStep = timeLeft;

			currentTime += timeStep;
			Vector3 currentPoint = GetPositionAtTime(startPoint, startVelocity, gravity, currentTime);
			path.Add(currentPoint);
		}
	}

	public static void GetPathExact(Vector2 startPoint, Vector2 startVelocity, float gravity, float fullTime, float timeStep, List<Ray> path)
	{
		if (timeStep <= 0) return;
		path.Clear();
		path.Add(new(startPoint, startVelocity));
		float currentTime = 0;
		for (int i = 0; currentTime < fullTime; i++)
		{
			float timeLeft = fullTime - currentTime;
			if (timeLeft < timeStep)
				timeStep = timeLeft;

			currentTime += timeStep;
			Vector2 currentPoint = GetPositionAtTime(startPoint, startVelocity, gravity, currentTime);
			Vector2 currentVelocity = GetVelocityAtTime(startVelocity, gravity, currentTime);
			path.Add(new(currentPoint, currentVelocity));
		}
	}

	public static void GetPathExact(Vector3 startPoint, Vector3 startVelocity, float gravity, float fullTime, float timeStep, List<Ray> path)
	{
		if (timeStep <= 0) return;
		path.Clear();
		path.Add(new(startPoint, startVelocity));
		float currentTime = 0;
		for (int i = 0; currentTime < fullTime; i++)
		{
			float timeLeft = fullTime - currentTime;
			if (timeLeft < timeStep)
				timeStep = timeLeft;

			currentTime += timeStep;
			Vector3 currentPoint = GetPositionAtTime(startPoint, startVelocity, gravity, currentTime);
			Vector3 currentVelocity = GetVelocityAtTime(startVelocity, gravity, currentTime);
			path.Add(new(currentPoint, currentVelocity));
		}
	}


	// Get Start Speed or Angle -------------------------------------------
	public static bool TryGetStartSpeed(Vector2 startPoint, Vector2 targetPoint, float gravity, float startAngleRad, out float startSpeed) =>
			TryGetStartSpeed(targetPoint - startPoint, gravity, startAngleRad, out startSpeed);
	public static bool TryGetStartSpeed(Vector3 startPoint, Vector3 targetPoint, float gravity, float startAngleRad, out float startSpeed) =>
		TryGetStartSpeed(targetPoint - startPoint, gravity, startAngleRad, out startSpeed);
	public static bool TryGetStartSpeed(Vector3 distanceVector, float gravity, float startAngleRad, out float startSpeed) =>
		TryGetStartSpeed(distanceVector.FlattenTo2D(), gravity, startAngleRad, out startSpeed);

	public static bool TryGetStartSpeed(Vector2 distanceVector, float gravity, float startAngleRad, out float startSpeed)
	{
		float dy = distanceVector.y;
		float dx = distanceVector.x;

		float cosAlpha = Mathf.Cos(startAngleRad);
		float tanAlpha = Mathf.Tan(startAngleRad);

		float root = gravity * dx * dx / (2 * cosAlpha * cosAlpha * (dx * tanAlpha - dy));
		if (root < 0)
		{
			startSpeed = 0;
			return false;
		}

		startSpeed = Mathf.Sqrt(root);
		return root > 0;
	}

	public static bool TryGetStartAngle(Vector2 startPoint, Vector2 targetPoint, float gravity, float startSpeed, bool low, out float startAngleRad) =>
		TryGetStartAngle(targetPoint - startPoint, gravity, startSpeed, low, out startAngleRad);
	public static bool TryGetStartAngle(Vector3 startPoint, Vector3 targetPoint, float gravity, float startSpeed, bool low, out float startAngleRad) =>
		TryGetStartAngle(targetPoint - startPoint, gravity, startSpeed, low, out startAngleRad);
	public static bool TryGetStartAngle(Vector3 distanceVector, float gravity, float startSpeed, bool low, out float startAngleRad) =>
		TryGetStartAngle(distanceVector.FlattenTo2D(), gravity, startSpeed, low, out startAngleRad);

	public static bool TryGetStartAngle(Vector2 distanceVector, float gravity, float startSpeed, bool low, out float startAngleRad)
	{
		float dy = distanceVector.y;
		float dx = distanceVector.x;

		float v2 = startSpeed * startSpeed;
		float v4 = v2 * v2;
		float x2 = dx * dx;
		float underRoot = v4 - gravity * (gravity * x2 + 2 * dy * v2);

		if (underRoot < 0)
		{
			startAngleRad = 0;
			return false;
		}

		float root = Mathf.Sqrt(underRoot);

		if (low)
			startAngleRad = Mathf.Atan((v2 - root) / (gravity * dx));
		else
			startAngleRad = Mathf.Atan((v2 + root) / (gravity * dx));

		return true;
	}


	// Get StartVelocity Vector -------------------------------------------

	public static bool TryGetStartVelocity(Vector2 startPoint, Vector2 targetPoint, float gravity, float startAngleRad, out Vector2 startVelocity) =>
		TryGetStartVelocity(targetPoint - startPoint, gravity, startAngleRad, out startVelocity);

	public static bool TryGetStartVelocity(Vector2 distanceVector, float gravity, float startAngleRad, out Vector2 startVelocity)
	{
		if (!TryGetStartSpeed(distanceVector, gravity, startAngleRad, out float startSpeed))
		{
			startVelocity = Vector2.zero;
			return false;
		}

		startVelocity = GetStartVelocity(startSpeed, startAngleRad);
		return true;
	}

	public static bool TryGetStartVelocity(Vector2 startPoint, Vector2 targetPoint, float gravity, float startSpeed, bool low, out Vector2 startVelocity) =>
		TryGetStartVelocity(targetPoint - startPoint, gravity, startSpeed, low, out startVelocity);

	public static bool TryGetStartVelocity(Vector2 distanceVector, float gravity, float startSpeed, bool low, out Vector2 startVelocity)
	{
		if (!TryGetStartAngle(distanceVector, gravity, startSpeed, low, out float startAngleRad))
		{
			startVelocity = Vector2.zero;
			return false;
		}

		startVelocity = GetStartVelocity(startSpeed, startAngleRad);
		return true;
	}

	public static Vector2 GetStartVelocity(float speed, float startAngleRad)
	{
		float vx = speed * Mathf.Cos(startAngleRad);
		float vy = speed * Mathf.Sin(startAngleRad);
		return new(vx, vy);
	}

	// Get StartVelocity Vector 3D -------------------------------------------

	public static bool TryGetStartVelocity(Vector3 startPoint, Vector3 targetPoint, float gravity, float startAngleRad, out Vector3 startVelocity) =>
			TryGetStartVelocity(targetPoint - startPoint, gravity, startAngleRad, out startVelocity);

	public static bool TryGetStartVelocity(Vector3 distanceVector, float gravity, float startAngleRad, out Vector3 startVelocity)
	{
		Vector2 distanceVector2D = distanceVector.FlattenTo2D();
		if (!TryGetStartVelocity(distanceVector2D, gravity, startAngleRad, out Vector2 v2D))
		{
			startVelocity = Vector3.zero;
			return false;
		}

		Vector3 horizontalDirection = new Vector3(distanceVector.x, 0, distanceVector.z).normalized;
		startVelocity = new(horizontalDirection.x * v2D.x, v2D.y, horizontalDirection.z * v2D.x);
		return true;
	}

	public static bool TryGetStartVelocity(Vector3 startPoint, Vector3 targetPoint, float gravity, float startSpeed, bool low, out Vector3 startVelocity) =>
		TryGetStartVelocity(targetPoint - startPoint, gravity, startSpeed, low, out startVelocity);

	public static bool TryGetStartVelocity(Vector3 distanceVector, float gravity, float startSpeed, bool low, out Vector3 startVelocity)
	{
		Vector2 distanceVector2D = distanceVector.FlattenTo2D();

		if (!TryGetStartVelocity(distanceVector2D, gravity, startSpeed, low, out Vector2 v2D))
		{
			startVelocity = Vector3.zero;
			return false;
		}

		Vector3 horizontalDirection = new Vector3(distanceVector.x, 0, distanceVector.z).normalized;
		startVelocity = new(horizontalDirection.x * v2D.x, v2D.y, horizontalDirection.z * v2D.x);
		return true;
	}

	// Utility Functions: Vectors, Vertical Angles -------------------------------------------

	public static float GetVerticalAngleRad(this Vector2 velocity) => Mathf.Atan(velocity.y / velocity.x);
	public static float GetVerticalAngleRad(this Vector3 velocity) => Mathf.Atan(velocity.y / velocity.GetHorizontalSize());
	public static Vector2 FlattenTo2D(this Vector3 v3) => new(v3.GetHorizontalSize(), v3.y);
	public static Vector3 ExtendTo3D(this Vector2 v2, Vector3 horizontalDirection)
	{
		Vector2 distanceVectorH = new(horizontalDirection.x, horizontalDirection.z);
		Vector2 directionH = distanceVectorH.normalized;
		return new(directionH.x * v2.x, v2.y, directionH.y * v2.x);
	}
	public static float GetHorizontalSize(this Vector3 v) => new Vector2(v.x, v.z).magnitude;

	// Get FullTime -----------------------------------------------

	public static float GetFullTime(float distanceX, float startAngleRad, float startSpeed)
	{
		float vx = startSpeed * Mathf.Cos(startAngleRad);
		return Mathf.Abs(distanceX) / vx;
	}

	public static float GetFullTime(Vector2 distanceVector, Vector2 startVelocity) =>
		GetFullTime(distanceVector.x, startVelocity.GetVerticalAngleRad(), startVelocity.y);

	public static float GetFullTime(Vector3 distanceVector, Vector3 startVelocity)
	{
		float dx = distanceVector.GetHorizontalSize();
		float angleRad = Mathf.Atan(startVelocity.y / dx);
		return GetFullTime(dx, angleRad, startVelocity.y);
	}

	// Get MaxPointInfo -----------------------------------------------

	public static void GetMaxPoint(Vector2 startVelocity, float gravity, out Vector2 positionAtTop, out Vector2 velocityAtTop, out float timeAtTop)
	{
		timeAtTop = startVelocity.y / gravity;
		float height = startVelocity.y * timeAtTop - 0.5f * gravity * timeAtTop * timeAtTop;
		positionAtTop = new(startVelocity.x * timeAtTop, height);
		velocityAtTop = new(startVelocity.x, 0);
	}

	public static void GetMaxPoint(Vector3 startVelocity, float gravity, out Vector3 positionAtTop, out Vector3 velocityAtTop, out float timeAtTop)
	{
		Vector2 startVelocity2D = startVelocity.FlattenTo2D();
		GetMaxPoint(startVelocity2D, gravity, out Vector2 positionAtTop2D, out Vector2 _, out timeAtTop);
		positionAtTop = positionAtTop2D.ExtendTo3D(startVelocity);
		velocityAtTop = startVelocity2D.ExtendTo3D(startVelocity);
	}

	public static void GetMaxPoint(Vector2 startPosition, Vector3 startVelocity, float gravity, out Vector2 positionAtTop, out Vector2 velocityAtTop, out float timeAtTop)
	{
		GetMaxPoint(Vector2.zero, startVelocity, gravity, out positionAtTop, out velocityAtTop, out timeAtTop);
		positionAtTop += startPosition;
	}

	public static void GetMaxPoint(Vector3 startPosition, Vector3 startVelocity, float gravity, out Vector3 positionAtTop, out Vector3 velocityAtTop, out float timeAtTop)
	{
		Vector2 startVelocity2D = startVelocity.FlattenTo2D();
		GetMaxPoint(startVelocity2D, gravity, out Vector2 positionAtTop2d, out Vector2 _, out timeAtTop);
		positionAtTop = positionAtTop2d.ExtendTo3D(startVelocity) + startPosition;
		velocityAtTop = startVelocity2D.ExtendTo3D(startVelocity);
	}
}