using UnityEngine;

public static class MathExtensions{
    public static Vector3 Flatten(this Vector3 origin, float? x = null, float? y = null, float? z = null){
        return new Vector3(x ?? origin.x, y ?? origin.y, z ?? origin.z);
    }

    public static Quaternion Flatten(this Quaternion input, float? x = null, float? y = null, float? z = null){
        return Quaternion.Euler(input.eulerAngles.Flatten(x, y, z));
    }

    public static Vector3 DirectionTo(this Vector3 origin, Vector3 to){
        return (to - origin).normalized;
    }

    public static Vector3 ToVector3(this Vector2 vector2){
        return new Vector3(vector2.x, 0f, vector2.y);
    }

    public static Vector2 ToVector2(this Vector3 vector3){
        return new Vector2(vector3.x, vector3.z);
    } 

    public static Color Alpha(this Color input, float newAlpha){
        return new Color(input.r, input.g, input.b, newAlpha);
    }
    
    public static Quaternion SmoothDamp(Quaternion rot, Quaternion target, ref Quaternion deriv, float time) {
        if (Time.deltaTime < Mathf.Epsilon) return rot;
        // account for double-cover
        var Dot = Quaternion.Dot(rot, target);
        var Multi = Dot > 0f ? 1f : -1f;
        target.x *= Multi;
        target.y *= Multi;
        target.z *= Multi;
        target.w *= Multi;
        // smooth damp (nlerp approx)
        var Result = new Vector4(
            Mathf.SmoothDamp(rot.x, target.x, ref deriv.x, time),
            Mathf.SmoothDamp(rot.y, target.y, ref deriv.y, time),
            Mathf.SmoothDamp(rot.z, target.z, ref deriv.z, time),
            Mathf.SmoothDamp(rot.w, target.w, ref deriv.w, time)
        ).normalized;
		
        // ensure deriv is tangent
        var derivError = Vector4.Project(new Vector4(deriv.x, deriv.y, deriv.z, deriv.w), Result);
        deriv.x -= derivError.x;
        deriv.y -= derivError.y;
        deriv.z -= derivError.z;
        deriv.w -= derivError.w;		
		
        return new Quaternion(Result.x, Result.y, Result.z, Result.w);
    }
    
    public static Vector2 Rotate(this Vector2 v, float delta) {
        return new Vector2(
            v.x * Mathf.Cos(delta) - v.y * Mathf.Sin(delta),
            v.x * Mathf.Sin(delta) + v.y * Mathf.Cos(delta)
        );
    }
}