XFEEDToolBox

namespace XFEEDToolBox.Core.Navigation;

public readonly record struct OriginalCurveKnot(double X,double Value,double InverseWidth,double Curvature);
public readonly record struct OriginalCurveRange(double Minimum,double Maximum);

/// <summary>Bounds the original constant, linear or natural-cubic curve, including extrema between knots and float-rounding headroom.</summary>
public static class OriginalCurveBounds
{
    public static bool TryBound(ReadOnlySpan<OriginalCurveKnot> points,int mode,double low,double high,out OriginalCurveRange range)
    {
        range=default;
        if(points.Length is <1 or >256||mode is <0 or >2||!double.IsFinite(low)||!double.IsFinite(high)||low<0||high>1||low>high)return false;
        for(var i=0;i<points.Length;i++)
        {
            var p=points[i];
            if(!double.IsFinite(p.X)||!double.IsFinite(p.Value)||!double.IsFinite(p.InverseWidth)||!double.IsFinite(p.Curvature)
                ||Math.Abs(p.X)>1024||Math.Abs(p.Value)>1e9||Math.Abs(p.InverseWidth)>1e12||Math.Abs(p.Curvature)>1e12)return false;
            if(i==points.Length-1)continue;
            var width=points[i+1].X-p.X;
            if(!double.IsFinite(width)||width<1e-9||mode==1&&Math.Abs(width*p.InverseWidth-1)>1e-3)return false;
        }
        var minimum=double.PositiveInfinity;var maximum=double.NegativeInfinity;
        void Include(double value,double error)
        {minimum=Math.Min(minimum,value-error);maximum=Math.Max(maximum,value+error);}
        if(points.Length==1||high>=points[^1].X)
        {var y=points[^1].Value;Include(y,(Math.Abs(y)+1)*1e-5);}
        for(var i=0;i<points.Length-1;i++)
        {
            var p=points[i];var next=points[i+1];var left=i==0?low:Math.Max(low,p.X);var right=Math.Min(high,next.X);
            if(left>right)continue;
            var width=next.X-p.X;var s0=left-p.X;var s1=right-p.X;
            double c0=p.Value,c1=0,c2=0,c3=0;
            if(mode==1)c1=(next.Value-p.Value)*p.InverseWidth;
            if(mode==2)
            {
                // Original E2FF20 uses second derivatives, h/6 and m0/2.
                // Treating these as first-derivative Hermite tangents loses
                // the interior maximum and can understate fuel consumption.
                c1=(next.Value-p.Value)/width-width*(2*p.Curvature+next.Curvature)/6;
                c2=p.Curvature*.5;c3=(next.Curvature-p.Curvature)/(6*width);
            }
            var radius=Math.Max(Math.Abs(s0),Math.Abs(s1));
            var error=(Math.Abs(c0)+Math.Abs(c1)*radius+Math.Abs(c2)*radius*radius+Math.Abs(c3)*radius*radius*radius+1)*1e-5;
            double Value(double s)=>((c3*s+c2)*s+c1)*s+c0;
            void Candidate(double s){if(double.IsFinite(s)&&s>=s0&&s<=s1)Include(Value(s),error);}
            Candidate(s0);Candidate(s1);
            var a=3*c3;var b=2*c2;
            if(a==0){if(b!=0)Candidate(-c1/b);}
            else
            {
                var discriminant=b*b-4*a*c1;
                if(!double.IsFinite(discriminant))return false;
                if(discriminant>=0)
                {
                    var root=Math.Sqrt(discriminant);var q=-.5*(b+Math.CopySign(root,b));
                    if(q==0)Candidate(-b/(2*a));else{Candidate(q/a);Candidate(c1/q);}
                }
            }
        }
        if(!double.IsFinite(minimum)||!double.IsFinite(maximum)||minimum>maximum)return false;
        range=new(minimum,maximum);return true;
    }
}