using System;
using Godot;

namespace Cirno.Scripts.Utils;

public static class MathFunctions
{
    public static Vector2? PredictInterceptPosition(Vector2 shooterPos, Vector2 targetPos, Vector2 targetVel, float projectileSpeed)
    {
        Vector2 displacement = targetPos - shooterPos;
        float a = targetVel.LengthSquared() - projectileSpeed * projectileSpeed;
        float b = 2 * displacement.Dot(targetVel);
        float c = displacement.LengthSquared();

        float discriminant = b * b - 4 * a * c;

        if (discriminant < 0 || Mathf.Abs(a) < 0.001f)
            return null; // No solution or projectile too slow

        float sqrtDisc = Mathf.Sqrt(discriminant);
        float t1 = (-b - sqrtDisc) / (2 * a);
        float t2 = (-b + sqrtDisc) / (2 * a);

        float t = Mathf.Min(t1, t2);
        if (t < 0)
            t = Mathf.Max(t1, t2);

        if (t < 0)
            return null; // No valid positive time

        return targetPos + targetVel * t;
    }
    
    // Critically damped spring, based on Game Programming Gems 4 Chapter 1.10. https://archive.org/details/game-programming-gems-4/page/95/mode/2up
    // Returns a 2-tuple of [next_position, next_velocity].
    public static Tuple<float, float> SmoothDamp(float current, float target, float currentVelocity, float smoothTime, float maxSpeed, float delta)
    {
        smoothTime = MathF.Max(smoothTime, 0.0001f);
        var omega = 2.0f / smoothTime;

        var x = omega * delta;
        var xExp = 1.0f / (1.0f + x + 0.48f * x * x + 0.235f * x * x * x);
        var change = current - target;
        var originalTarget = target;
        
        // Clamp max speed
        var maxChange = maxSpeed * smoothTime;
        change = Math.Clamp(change, -maxChange, maxChange);
        target = current - change;
        
        var temp = (currentVelocity + omega * change) * delta;
        currentVelocity = (currentVelocity - omega * temp) * xExp;
        var output = target + (change + temp) * xExp;
        
        // Prevent Overshooting
        if ((originalTarget - current > 0.0) == (output > originalTarget))
        {
            output = originalTarget;
            currentVelocity = (output - originalTarget) / delta;
        }
        
        return new Tuple<float, float>(output, currentVelocity);
    }
}