XFEEDToolBox

using XFEEDToolBox.Core.Navigation;
using XFEExtension.NetCore.XUnit.Attributes;
using System.Text.Json;

namespace XFEEDToolBox.Test;

[TestFixture]
public sealed class ScoBrakingPredictorTests
{
    private static ScoExitEstimateInput ExitInput=>new()
    {
        OriginalParametersVerified=true,ForwardServoDomainVerified=true,OrdinaryArrivalControllerVerified=true,
        OriginalBrakeScalingMode=2,RemainingDistanceMetres=10000,CurrentSpeedMetresPerSecond=400,
        CurrentOrdinarySpeedLimitMetresPerSecond=40,OriginalBaseMaximumSpeedMetresPerSecond=1000,
        OriginalBaseBrakeAcceleration=150,OriginalResponseTimeSeconds=1,OriginalJerkMetresPerSecondCubed=100,
        OriginalForwardTargetBiasMetresPerSecond=10,ForwardThrottle=.75,StopResponseSecondsUpper=0,
        ActiveAccelerationUpper=0,EstimatedCoolingAndAssistSettleSeconds=1.5,MaximumPhysicsStepSeconds=.01,
        AdditionalResponseMarginSeconds=.1,MinimumOrdinaryHandoffSpeedMetresPerSecond=5,
        OriginalArrivalWindowSeconds=6,RelativeDistanceMargin=.01,DistanceUncertaintyMetres=2,SafeDropDistanceMetres=1
    };
    private static ScoServoParameters Servo=>new()
    {
        PinnedSourceDomainVerified=true,BaseMaximumSpeedMetresPerSecond=1000,BaseForwardAcceleration=75,
        BaseBrakeAcceleration=150,ResponseTimeSeconds=1,JerkMetresPerSecondCubed=10000,
        ForwardTargetBiasMetresPerSecond=10,ForwardThrottle=.75
    };
    private static ScoServoStepInput ServoStep=>new()
    {
        SpeedMetresPerSecond=400,OrdinaryPhysicsMaximumSpeedMetresPerSecond=40,PreviousServoAccelerationNorm=1000,
        FlightAssistBlend=1,Cooling=false,CoolingDragPerSecond=.36,NativeStepSeconds=.01
    };
    [Test]public void EstimatedBudgetIsExplicitlyUnqualifiedAndGivesCoolingNoBrakeCredit()
    {
        var first=ScoBrakingPredictor.EstimateExit(ExitInput);
        var longer=ScoBrakingPredictor.EstimateExit(ExitInput with{EstimatedCoolingAndAssistSettleSeconds=2});
        Check.That(first.Known&&first.Estimated&&!first.QualifiedForControl&&!first.CoolingDragUsed&&first.RequiresActualOrdinaryHandoff);
        Check.That(Math.Abs(longer.EstimatedStopDistanceMetres-first.EstimatedStopDistanceMetres-200)<1e-9);
        Check.That(first.EstimatedStopDistanceMetres<400*13);
        Check.That(!ScoBrakingPredictor.EstimateExit(ExitInput with{RemainingDistanceMetres=first.EstimatedStopDistanceMetres}).RemainingDistanceExceedsEstimate);
    }
    [Test]public void NativeModeTwoLowerLawDoesNotUseAnUnconditionalQuarterBrake()
    {
        var result=ScoBrakingPredictor.StepOriginalServo(Servo,ServoStep);
        var clamp=150*.4*Math.Sqrt(.4/.04);
        Check.That(result.Known&&!result.QualifiedForControl);
        Check.That(Math.Abs(result.AppliedAcceleration+clamp)<1e-10);
        Check.Equal(32.5d,result.TargetSpeedMetresPerSecond);
        Check.Equal(.5d,result.EffectiveResponseTimeSeconds);
        Check.That(Math.Abs(result.NextSpeedMetresPerSecond-(400-clamp*.01))<1e-10);
    }
    [Test]public void OriginalJerkRampsFromTheActualPriorNormPerPhysicsStep()
    {
        var parameters=Servo with{JerkMetresPerSecondCubed=100};
        var first=ScoBrakingPredictor.StepOriginalServo(parameters,ServoStep with{PreviousServoAccelerationNorm=0,NativeStepSeconds=.1});
        var next=ScoBrakingPredictor.StepOriginalServo(parameters,ServoStep with{PreviousServoAccelerationNorm=first.NextServoAccelerationNorm,SpeedMetresPerSecond=first.NextSpeedMetresPerSecond,NativeStepSeconds=.1});
        Check.Equal(10d,first.NextServoAccelerationNorm);Check.Equal(399d,first.NextSpeedMetresPerSecond);
        Check.Equal(20d,next.NextServoAccelerationNorm);Check.Equal(397d,next.NextSpeedMetresPerSecond);
    }
    [Test]public void CoolingLambdaIsNeitherMultipliedByBlendNorPresentWhenBlendIsZero()
    {
        var original=ScoBrakingPredictor.StepOriginalServo(Servo,ServoStep);
        var mixed=ScoBrakingPredictor.StepOriginalServo(Servo,ServoStep with{FlightAssistBlend=.5,Cooling=true});
        var zero=ScoBrakingPredictor.StepOriginalServo(Servo,ServoStep with{FlightAssistBlend=0,Cooling=true});
        Check.That(Math.Abs(mixed.AppliedAcceleration-(original.AppliedAcceleration*.5-.36*400))<1e-10);
        Check.Equal(0d,zero.AppliedAcceleration);Check.Equal(400d,zero.NextSpeedMetresPerSecond);
    }
    [Test]public void BudgetMaximisesOverHandoffsAndStopsTheOverspeedDomainAtItsFloor()
    {
        foreach(var window in new[]{2d,6d,10d})
        {
            var value=ExitInput with{OriginalArrivalWindowSeconds=window};
            var result=ScoBrakingPredictor.EstimateExit(value);Check.That(result.Known);
            // Independent small-step lower-law integration; no predictor roots
            // or path primitive are reused. Evaluate every possible handoff.
            var v=value.CurrentSpeedMetresPerSecond;var distance=0d;var t=0d;var maximum=window*v;
            const double dt=.0001;
            while(v>result.ServoDomainFloorMetresPerSecond)
            {
                var a=Math.Min(result.DecayRatePerSecond*v,value.OriginalJerkMetresPerSecondCubed*t);
                var next=Math.Max(result.ServoDomainFloorMetresPerSecond,v-a*dt);
                distance+=(v+next)*.5*dt;t+=dt;v=next;
                maximum=Math.Max(maximum,distance+window*v);
            }
            var analytic=result.OverspeedBudgetDistanceMetres+result.OrdinaryArrivalEnvelopeMetres;
            Check.That(Math.Abs(analytic-maximum)<.03);
            Check.That(result.BudgetHandoffSpeedMetresPerSecond>=result.ServoDomainFloorMetresPerSecond-1e-9);
            Check.That(result.BudgetHandoffSeconds<=result.DomainFloorSeconds);
        }
    }
    [Test]public void BiasFloorAndServoLimitedRateAreDerivedRatherThanFitted()
    {
        var original=ScoBrakingPredictor.EstimateExit(ExitInput);Check.That(original.Known);
        Check.That(Math.Abs(original.DecayRatePerSecond-.15)<1e-12);
        Check.That(Math.Abs(original.ServoDomainFloorMetresPerSecond-25)<1e-12);
        var stronger=ScoBrakingPredictor.EstimateExit(ExitInput with{OriginalBaseBrakeAcceleration=500});
        Check.That(stronger.Known);Check.Equal(.125d,stronger.DecayRatePerSecond);Check.Equal(20d,stronger.ServoDomainFloorMetresPerSecond);
    }
    [Test]public void MissingExitSourceGatesAndInvalidParametersNeverAuthoriseAnEstimate()
    {
        foreach(var input in new[]{ExitInput with{OriginalParametersVerified=false},ExitInput with{ForwardServoDomainVerified=false},
            ExitInput with{OrdinaryArrivalControllerVerified=false},ExitInput with{OriginalBrakeScalingMode=1},
            ExitInput with{CurrentSpeedMetresPerSecond=1000},ExitInput with{CurrentOrdinarySpeedLimitMetresPerSecond=400},
            ExitInput with{OriginalJerkMetresPerSecondCubed=double.NaN},
            ExitInput with{CurrentOrdinarySpeedLimitMetresPerSecond=1},ExitInput with{ForwardThrottle=1},
            ExitInput with{EstimatedCoolingAndAssistSettleSeconds=double.PositiveInfinity}})
        {
            var result=ScoBrakingPredictor.EstimateExit(input);
            Check.That(!result.Known&&!result.QualifiedForControl&&!result.RemainingDistanceExceedsEstimate);
        }
        Check.That(!ScoBrakingPredictor.StepOriginalServo(Servo with{PinnedSourceDomainVerified=false},ServoStep).Known);
        Check.That(!ScoBrakingPredictor.StepOriginalServo(Servo,ServoStep with{SpeedMetresPerSecond=20,FlightAssistBlend=.5}).Known);
    }
    private static ScoExitEstimateObservation Observation=>new()
    {
        FreshBindingAndSourceGatesVerified=true,ElapsedSeconds=2,SpeedMetresPerSecond=350,OrdinarySpeedLimitMetresPerSecond=40,
        NativeStepSeconds=.005,FlightAssistBlend=1,ScoState=0,AllowedSpeedResidualFraction=0,AllowedSpeedResidualMetresPerSecond=0
    };
    [Test]public void ActualOrdinaryHandoffEndsTheModelEvenWhenTheCapHasRisen()
    {
        var forecast=ScoBrakingPredictor.EstimateExit(ExitInput);
        var check=ScoBrakingPredictor.CheckExitEstimate(ExitInput,forecast,Observation with{SpeedMetresPerSecond=100,OrdinarySpeedLimitMetresPerSecond=110});
        Check.That(check.WithinEstimatedDomain&&check.OrdinaryHandoffReady&&!check.QualifiedForControl);
        Check.Equal("actual-ordinary-cap-handoff",check.Reason);
    }
    [Test]public void SettleDeadlineIdentityDomainAndBrakeResidualAreOnlineFallbackReasons()
    {
        var forecast=ScoBrakingPredictor.EstimateExit(ExitInput);
        foreach(var observation in new[]{Observation with{FreshBindingAndSourceGatesVerified=false},
            Observation with{ScoState=3},Observation with{FlightAssistBlend=.9},Observation with{NativeStepSeconds=.02},
            Observation with{ElapsedSeconds=5,SpeedMetresPerSecond=400},
            Observation with{OrdinarySpeedLimitMetresPerSecond=20},Observation with{OrdinarySpeedLimitMetresPerSecond=1001},
            Observation with{ElapsedSeconds=1000}})
        {
            var check=ScoBrakingPredictor.CheckExitEstimate(ExitInput,forecast,observation);
            Check.That(!check.WithinEstimatedDomain&&!check.OrdinaryHandoffReady);
        }
    }
    [Test]public void RecordedCurveKeepsSparsePhysicsStepsSeparateFromElapsedTime()
    {
        using var doc=ReadExitFixture();var frames=doc.RootElement.GetProperty("frames").EnumerateArray().Select(x=>x.EnumerateArray().Select(v=>v.GetDouble()).ToArray()).ToArray();
        const double c=299792458;var integral=0d;
        for(var i=1;i<frames.Length;i++)integral+=(frames[i-1][1]+frames[i][1])*.5*(frames[i][0]-frames[i-1][0]);
        var actual=frames[0][6]-frames[^1][6];
        Check.Equal(130,frames.Length);Check.That(Math.Abs((frames[^1][0]-frames[0][0])-7.935)<1e-9);
        Check.That(frames.Sum(x=>x[8])<1); // NOT 7.935 seconds of simulated history.
        Check.That(Math.Abs(integral-actual)/actual<.002);
        Check.That(Math.Abs(actual/c-215.34685571495913)<1e-9);
        Check.That(!doc.RootElement.GetProperty("qualifiedForControl").GetBoolean());
    }
    [Test]public void OriginalScalarOffReplayAgreesWithTheIndependentObservedCurveWithinItsStatedLimits()
    {
        using var doc=ReadExitFixture();var root=doc.RootElement;
        var frames=root.GetProperty("frames").EnumerateArray().Select(x=>x.EnumerateArray().Select(v=>v.GetDouble()).ToArray()).ToArray();
        var first=Array.FindIndex(frames,x=>x[5]==0);const double c=299792458;
        var parameters=new ScoServoParameters
        {
            PinnedSourceDomainVerified=true,BaseMaximumSpeedMetresPerSecond=root.GetProperty("baseMaximumSpeed").GetDouble(),
            BaseBrakeAcceleration=root.GetProperty("baseBrake").GetDouble(),BaseForwardAcceleration=root.GetProperty("baseForward").GetDouble(),
            ResponseTimeSeconds=root.GetProperty("baseResponseTime").GetDouble(),JerkMetresPerSecondCubed=root.GetProperty("laterObservedJerk").GetDouble(),
            ForwardTargetBiasMetresPerSecond=root.GetProperty("laterObservedBias").GetDouble(),ForwardThrottle=root.GetProperty("throttle").GetDouble()
        };
        // Supplied FUTURE cap, and an initially saturated servo norm hypothesis.
        // These are a retrospective source-equation check, not live calibration.
        var speed=frames[first][1];var norm=frames[first][7];var distance=0d;var error=0d;
        for(var i=first+1;i<frames.Length;i++)
        {
            var left=frames[i-1];var right=frames[i];var t=left[0];
            while(t<right[0]-1e-12)
            {
                var dt=Math.Min(.001,right[0]-t);var fraction=(t+.5*dt-left[0])/(right[0]-left[0]);
                var result=ScoBrakingPredictor.StepOriginalServo(parameters,new()
                {
                    SpeedMetresPerSecond=speed,PreviousServoAccelerationNorm=norm,NativeStepSeconds=dt,FlightAssistBlend=1,
                    OrdinaryPhysicsMaximumSpeedMetresPerSecond=left[2]+(right[2]-left[2])*fraction,Cooling=false,CoolingDragPerSecond=0
                });
                Check.That(result.Known);speed=result.NextSpeedMetresPerSecond;norm=result.NextServoAccelerationNorm;
                distance+=result.ApproximateDistanceMetres;t+=dt;
            }
            error=Math.Max(error,Math.Abs(speed-right[1]));
        }
        var actual=frames[first][6]-frames[^1][6];
        Check.That(error/c<.05);Check.That(Math.Abs(distance-actual)/actual<.003);
        Check.That(Math.Abs(speed-frames[^1][1])/c<.005);
    }
    private static JsonDocument ReadExitFixture()=>JsonDocument.Parse(File.ReadAllText(Path.Combine(AppContext.BaseDirectory,"Fixtures","sco-exit-observed.json")));
    // Isolated mathematical fixture, not a claimed calibration of any live ship.
    private static ScoBrakingInput Input=>new()
    {
        OriginalParametersVerified=true,OrdinaryArrivalControllerVerified=true,OverspeedBrakeBoundVerified=true,
        RemainingDistanceMetres=10000,CurrentSpeedMetresPerSecond=400,OriginalBaseMaximumSpeedMetresPerSecond=1000,
        OrdinarySpeedLimitUpperMetresPerSecond=40,OriginalBaseOverspeedBrakeAcceleration=200,
        OriginalBrakeScalingMode=2,OriginalOrdinaryBrakeScaleLower=.04,OriginalExtraBrakeFactorLower=.25,OriginalArrivalWindowSeconds=6,
        StopResponseSecondsUpper=0,ActiveAccelerationMetresPerSecondSquaredUpper=0,PositionUncertaintyMetres=0,SafeDropDistanceMetres=0
    };
    [Test]public void ExactOverspeedIntegralAgreesWithIndependentNumericalOriginalLaw()
    {
        var value=Input;var result=ScoBrakingPredictor.Predict(value);Check.That(result.Known);
        // Simpson integration in velocity space uses the actual original
        // scale formula, rather than the predictor's analytic primitive.
        var intervals=10000;var h=(value.CurrentSpeedMetresPerSecond-value.OrdinarySpeedLimitUpperMetresPerSecond)/intervals;
        var distance=0d;var seconds=0d;var scale=value.OrdinarySpeedLimitUpperMetresPerSecond/value.OriginalBaseMaximumSpeedMetresPerSecond;
        for(var i=0;i<=intervals;i++)
        {
            var speed=value.OrdinarySpeedLimitUpperMetresPerSecond+i*h;
            var ratio=speed/value.OriginalBaseMaximumSpeedMetresPerSecond;
            var originalScaled=value.OriginalBaseOverspeedBrakeAcceleration*Math.Max(scale,ratio*Math.Max(1,Math.Sqrt(ratio/scale)));
            var deceleration=value.OriginalExtraBrakeFactorLower*originalScaled;
            var weight=i==0||i==intervals?1:i%2==0?2:4;
            distance+=weight*speed/deceleration;seconds+=weight/deceleration;
        }
        distance*=h/3;seconds*=h/3;
        Check.That(Math.Abs(distance-result.OverspeedDistanceMetres)<distance*1e-9);
        Check.That(Math.Abs(seconds-result.OverspeedSeconds)<seconds*1e-9);
        Check.Equal(240d,result.OrdinaryArrivalEnvelopeMetres);
        Check.That(result.StopDistanceMetres<value.CurrentSpeedMetresPerSecond*13);
        Check.That(!result.CoolingDragUsed);
    }
    [Test]public void OrdinaryEntryUsesThePostScoLimitRatherThanTheCurrentOverchargeSpeed()
    {
        var first=ScoBrakingPredictor.Predict(Input);
        var faster=ScoBrakingPredictor.Predict(Input with{CurrentSpeedMetresPerSecond=1600});
        Check.Equal(40d,first.OrdinaryEntrySpeedMetresPerSecond);Check.Equal(40d,faster.OrdinaryEntrySpeedMetresPerSecond);
        Check.Equal(240d,faster.OrdinaryArrivalEnvelopeMetres);Check.That(faster.OverspeedDistanceMetres>first.OverspeedDistanceMetres);
        var slow=ScoBrakingPredictor.Predict(Input with{CurrentSpeedMetresPerSecond=20});
        Check.Equal(0d,slow.OverspeedDistanceMetres);Check.Equal(120d,slow.OrdinaryArrivalEnvelopeMetres);
    }
    [Test]public void RealResponseDelayAccountsForDistanceAndFurtherScoAccelerationOnlyDuringThatDelay()
    {
        var baseline=ScoBrakingPredictor.Predict(Input);
        var delayed=ScoBrakingPredictor.Predict(Input with{StopResponseSecondsUpper=.25,ActiveAccelerationMetresPerSecondSquaredUpper=80,PositionUncertaintyMetres=3,SafeDropDistanceMetres=2});
        Check.Equal(420d,delayed.SpeedAtOriginalStopMetresPerSecond);Check.Equal(102.5d,delayed.ResponseDistanceMetres);
        Check.That(delayed.StopDistanceMetres>baseline.StopDistanceMetres+107.5);
        Check.That(ScoBrakingPredictor.Predict(Input with{RemainingDistanceMetres=delayed.StopDistanceMetres-1,StopResponseSecondsUpper=.25,ActiveAccelerationMetresPerSecondSquaredUpper=80,PositionUncertaintyMetres=3,SafeDropDistanceMetres=2}).ContinueOvercharge==false);
    }
    [Test]public void SourceOrBrakeResponseGapsCannotAuthorizeALaterStop()
    {
        foreach(var value in new[]{Input with{OriginalParametersVerified=false},Input with{OrdinaryArrivalControllerVerified=false},Input with{OverspeedBrakeBoundVerified=false},
            Input with{OriginalBrakeScalingMode=3},Input with{OrdinarySpeedLimitUpperMetresPerSecond=1001},Input with{OriginalBaseOverspeedBrakeAcceleration=0},
            Input with{OriginalExtraBrakeFactorLower=.251},Input with{CurrentSpeedMetresPerSecond=double.NaN},Input with{StopResponseSecondsUpper=double.PositiveInfinity}})
        {var result=ScoBrakingPredictor.Predict(value);Check.That(!result.Known&&!result.ContinueOvercharge&&double.IsNaN(result.StopDistanceMetres));}
    }
    [Test]public void ConstantAndSquareRootNativeScalingModesRespectTheirDifferentBrakeStrength()
    {
        var linear=ScoBrakingPredictor.Predict(Input with{OriginalBrakeScalingMode=0});
        var root=ScoBrakingPredictor.Predict(Input with{OriginalBrakeScalingMode=1});
        Check.That(linear.Known&&root.Known);Check.Equal(39600d,linear.OverspeedDistanceMetres);Check.Equal(7920d,root.OverspeedDistanceMetres);
    }
    [Test]public void AProviderSnapshotCannotReplaceALowerScaleBoundForModesThatLoseBrakingNearGravity()
    {
        var current=ScoBrakingPredictor.Predict(Input with{OriginalBrakeScalingMode=0});
        var weaker=ScoBrakingPredictor.Predict(Input with{OriginalBrakeScalingMode=0,OriginalOrdinaryBrakeScaleLower=.01});
        Check.Equal(current.OverspeedDistanceMetres*4,weaker.OverspeedDistanceMetres);
        Check.That(!ScoBrakingPredictor.Predict(Input with{OriginalBrakeScalingMode=0,OriginalOrdinaryBrakeScaleLower=double.NaN}).Known);
        Check.That(!ScoBrakingPredictor.Predict(Input with{OriginalBrakeScalingMode=1,OriginalOrdinaryBrakeScaleLower=.041}).Known);
        // Mode 2 gets stronger when the ordinary limit falls. Its upper
        // speed limit is already the conservative scale for the integral.
        Check.That(ScoBrakingPredictor.Predict(Input with{OriginalOrdinaryBrakeScaleLower=double.NaN}).Known);
    }
    [Test]public void LightSecondScaleAndTinyOverspeedDoNotLoseTheFiniteBrakeDistance()
    {
        const double c=299792458;
        var large=ScoBrakingPredictor.Predict(Input with{CurrentSpeedMetresPerSecond=500*c,OriginalBaseMaximumSpeedMetresPerSecond=2000*c,
            OrdinarySpeedLimitUpperMetresPerSecond=100*c,OriginalBaseOverspeedBrakeAcceleration=400*c});
        Check.That(large.Known&&double.IsFinite(large.StopDistanceMetres)&&large.OverspeedDistanceMetres>0);
        var small=ScoBrakingPredictor.Predict(Input with{CurrentSpeedMetresPerSecond=40.000000001});
        Check.That(small.Known&&small.OverspeedDistanceMetres>0&&small.OverspeedDistanceMetres<1e-6);
    }
}