XFEEDToolBox

using XFEEDToolBox.Core.Navigation;
using XFEExtension.NetCore.XUnit.Attributes;

namespace XFEEDToolBox.Test;

[TestFixture]
public sealed class AdaptiveAttitudeTests
{
    [Test] public void EachAxisFitsItsOwnTurnSpeedResponseTimeAndInputDelay()
    {
        foreach(var rate in new[]{3d,45,180,420})foreach(var tau in new[]{.06,.25,.75})foreach(var delay in new[]{0d,.1,.2})
        {
            var model=new AxisResponseEstimator();var history=new List<(double Time,double Input)>();double v=0;
            const double dt=.025;
            for(var i=0;i<400;i++)
            {
                var t=i*dt;model.Observe(t,v);
                var input=Math.Sin(t*2.4)*.4+Math.Sin(t*.7)*.2;history.Add((t,input));model.Acknowledge(t,input);
                var u=history.LastOrDefault(h=>h.Time<=t+dt*.5-delay).Input;
                v=v*Math.Exp(-dt/tau)+rate*u*(1-Math.Exp(-dt/tau));
            }
            var fit=model.Response;
            Check.That(fit.Calibrated,$"No independent fit: {rate}/{tau}/{delay}");
            Check.That(Math.Abs(fit.MaximumRate/rate-1)<.12,$"Rate: {rate}/{tau}/{delay}, fitted {fit}");
            Check.That(Math.Abs(fit.TimeConstant-tau)<.11,$"Response time: {rate}/{tau}/{delay}, fitted {fit}");
            Check.That(Math.Abs(fit.InputDelay-delay)<.055,$"Input delay: {rate}/{tau}/{delay}, fitted {fit}");
        }
    }

    [Test] public void HighAndLowAgilityShipsSettleOnAllThreeAxesDespiteLatencyAndFrameJitter()
    {
        var cases=new[]{(Rates:new[]{3d,22,50},Taus:new[]{.4,.25,.15},Delay:0d),
            (Rates:new[]{60d,160,420},Taus:new[]{.06,.12,.08},Delay:.1),
            (Rates:new[]{110d,240,380},Taus:new[]{.18,.06,.25},Delay:.2)};
        foreach(var ship in cases)foreach(var hz in new[]{30,60,144})foreach(var goal in new[]{-20d,20})for(var axis=0;axis<3;axis++)
        {
            var controller=new BankedAttitudeController();var start=DateTimeOffset.Parse("2026-10-23T00:00:00Z");
            double t=0,angle=0,rate=0,tailSquare=0,tailRate=0,maxOvershoot=0,lastSign=0;int count=0,flips=0;
            var history=new List<(double Time,double Input)>();
            var rotationAxis=axis==0?new Vector3d(0,1,0):axis==1?new(1,0,0):new Vector3d(0,0,1);
            var targetForward=Rotate(new(0,0,1),rotationAxis,goal);var targetUp=Rotate(new(0,1,0),rotationAxis,goal);
            for(var i=0;t<25;i++)
            {
                var dt=(1d/hz)*(i%4==0?1.3:i%4==1?.7:1);
                var f=Rotate(new(0,0,1),rotationAxis,angle);var up=Rotate(new(0,1,0),rotationAxis,angle);
                var measured=new Vector3d(axis==0?rate:0,axis==1?rate:0,axis==2?rate:0);
                var frame=new FlightFrame{Sequence=i+1,SystemAddress=1,LoadoutFingerprint="independent-ship",InSupercruise=true,ObservedAt=start.AddSeconds(t),Forward=f,Up=up};
                var input=controller.Update(frame,targetForward,targetUp,measured);
                controller.ConfirmApplied(frame.ObservedAt,input);
                var command=axis==0?input.Yaw:axis==1?input.Pitch:input.Roll;
                history.Add((t,command));var u=history.LastOrDefault(h=>h.Time<=t+dt*.5-ship.Delay).Input;
                var next=rate*Math.Exp(-dt/ship.Taus[axis])+ship.Rates[axis]*u*(1-Math.Exp(-dt/ship.Taus[axis]));
                angle+=(rate+next)*.5*dt;rate=next;t+=dt;maxOvershoot=Math.Max(maxOvershoot,(angle-goal)*Math.Sign(goal));
                if(t>20)
                {
                    tailSquare+=(angle-goal)*(angle-goal);tailRate+=Math.Abs(rate);count++;
                    var sign=Math.Abs(command)>.002?Math.Sign(command):0;
                    if(sign!=0){if(lastSign!=0&&lastSign!=sign)flips++;lastSign=sign;}
                }
            }
            var label=$"{ship.Rates[axis]} deg/s, axis {axis}, {hz} Hz, delay {ship.Delay}; angle {angle}, rate {rate}; fit {controller.Responses[axis]}";
            Check.That(Math.Sqrt(tailSquare/count)<.3,"Steady heading: "+label);
            Check.That(tailRate/count<.4,"Steady rotation: "+label);
            Check.That(maxOvershoot<1.2,"Overshoot: "+label);
            Check.That(flips<=6,"Repeated opposite inputs: "+label);
        }
    }

    [Test] public void AnotherShipAndNormalSpaceNeverInheritThePreviousShipsCalibration()
    {
        var controller=new BankedAttitudeController();var start=DateTimeOffset.Parse("2026-10-23T00:00:00Z");
        double rate=0;
        for(var i=0;i<150;i++)
        {
            var frame=new FlightFrame{Sequence=i+1,SystemAddress=1,LoadoutFingerprint="ship-A/engineered",InSupercruise=true,ObservedAt=start.AddSeconds(i*.025),Forward=new(0,0,1),Up=new(0,1,0)};
            var input=controller.Update(frame,new(.3,0,1),frame.Up,new(rate,0,0));controller.ConfirmApplied(frame.ObservedAt,input);
            rate=rate*Math.Exp(-.025/.12)+60*input.Yaw*(1-Math.Exp(-.025/.12));
        }
        Check.That(controller.Responses[0].Calibrated);
        var another=new FlightFrame{Sequence=1000,SystemAddress=1,LoadoutFingerprint="ship-B/stock",InSupercruise=true,ObservedAt=start.AddSeconds(4),Forward=new(0,0,1),Up=new(0,1,0)};
        controller.Update(another,new(.3,0,1),another.Up,Vector3d.Zero);
        Check.That(controller.Responses.All(r=>!r.Calibrated));
        controller.Update(another with{Sequence=1001,LoadoutFingerprint="ship-A/engineered",ObservedAt=start.AddSeconds(4.05)},new(.3,0,1),another.Up,Vector3d.Zero);
        Check.That(controller.Responses[0].Calibrated);
        controller.Update(another with{Sequence=1002,LoadoutFingerprint="ship-A/engineered",InSupercruise=false,ObservedAt=start.AddSeconds(4.1)},new(.3,0,1),another.Up,Vector3d.Zero);
        Check.That(controller.Responses.All(r=>!r.Calibrated));
    }

    [Test] public void RejectedOrExpiredCommandsAreExcludedFromCalibration()
    {
        var controller=new BankedAttitudeController();var start=DateTimeOffset.Parse("2026-10-23T00:00:00Z");
        for(var i=0;i<200;i++)
        {
            var frame=new FlightFrame{Sequence=i+1,SystemAddress=1,LoadoutFingerprint="ship",ObservedAt=start.AddSeconds(i*.025),Forward=new(0,0,1),Up=new(0,1,0)};
            controller.Update(frame,new(.3,0,1),frame.Up,new(Math.Sin(i*.025),0,0));
        }
        Check.That(controller.Responses.All(r=>!r.Calibrated));
    }
    [Test] public void EnginePipAndSpeedChangesCannotReuseAnIncompatibleResponseFit()
    {
        var controller=new BankedAttitudeController();var start=DateTimeOffset.Parse("2026-10-23T00:00:00Z");
        double rate=0;
        var frame=new FlightFrame{SystemAddress=1,LoadoutFingerprint="same-ship",EnginePips=4,SpeedFraction=.5,Forward=new(0,0,1),Up=new(0,1,0)};
        for(var i=0;i<150;i++)
        {
            var sample=frame with{Sequence=i+1,ObservedAt=start.AddSeconds(i*.025)};
            var input=controller.Update(sample,new(.3,0,1),sample.Up,new(rate,0,0));controller.ConfirmApplied(sample.ObservedAt,input);
            rate=rate*Math.Exp(-.025/.12)+60*input.Yaw*(1-Math.Exp(-.025/.12));
        }
        Check.That(controller.Responses[0].Calibrated);
        controller.Update(frame with{Sequence=151,ObservedAt=start.AddSeconds(3.75),EnginePips=8},new(.3,0,1),frame.Up,Vector3d.Zero);
        Check.That(controller.Responses.All(r=>!r.Calibrated));
        controller.Update(frame with{Sequence=152,ObservedAt=start.AddSeconds(3.8)},new(.3,0,1),frame.Up,Vector3d.Zero);
        Check.That(controller.Responses[0].Calibrated);
        controller.Update(frame with{Sequence=153,ObservedAt=start.AddSeconds(3.85),SpeedFraction=.9},new(.3,0,1),frame.Up,Vector3d.Zero);
        Check.That(controller.Responses.All(r=>!r.Calibrated));
    }
    private static Vector3d Rotate(Vector3d value,Vector3d axis,double degrees)
    {var r=degrees*Math.PI/180;return value*Math.Cos(r)+Vector3d.Cross(axis,value)*Math.Sin(r)+axis*(Vector3d.Dot(axis,value)*(1-Math.Cos(r)));}
}