XFEEDToolBox

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

namespace XFEEDToolBox.Test;

[TestFixture]
public class AttitudeTests
{
    [Test] public void NativeAxisSignsAimRightAndUpAndRemainBoundedBehindTheShip()
    {
        var forward=new Vector3d(0,0,1);var up=new Vector3d(0,1,0);
        var right=AttitudeController.Aim(forward,up,new(1,0,1));
        var above=AttitudeController.Aim(forward,up,new(0,1,1));
        var behind=AttitudeController.Aim(forward,up,new(0,0,-1));
        Check.That(right.Yaw>0&&right.Pitch==0);
        Check.That(above.Pitch<0&&above.Yaw==0);
        Check.That(Math.Abs(behind.Yaw)==.12&&double.IsFinite(behind.ErrorDegrees));
        Check.Equal(0.0,AttitudeController.Aim(forward,up,forward).ErrorDegrees);
    }

    [Test] public void SteeringUsesShipCoordinatesAndRejectsDegenerateAttitude()
    {
        var local=AttitudeController.Aim(new(0,0,1),new(0,1,0),new(.1,.2,1));
        var rotated=AttitudeController.Aim(new(1,0,0),new(0,1,0),new(1,.2,-.1));
        Check.That(Math.Abs(local.Yaw-rotated.Yaw)<1e-10&&Math.Abs(local.Pitch-rotated.Pitch)<1e-10);
        Check.Throws<InvalidOperationException>(()=>AttitudeController.Aim(new(0,0,1),new(0,0,1),new(1,0,0)));
        Check.Throws<ArgumentException>(()=>AttitudeController.Aim(new(0,0,1),new(0,1,0),new(double.NaN,0,1)));
    }
    [Test] public void BankRollsTheStarAboveTheCockpitAndHandlesFacingTheStar()
    {
        var forward=new Vector3d(0,0,1);var up=new Vector3d(0,1,0);
        var starRight=AttitudeController.AimBanked(forward,up,forward,new(1,0,0));
        var starLeft=AttitudeController.AimBanked(forward,up,forward,new(-1,0,0));
        Check.That(starRight.Roll<0&&starLeft.Roll>0&&starRight.Yaw==0&&starRight.Pitch==0);
        var facing=AttitudeController.AimBanked(forward,up,new(0,1,0),forward);
        Check.That(double.IsFinite(facing.Roll)&&double.IsFinite(facing.Pitch)&&double.IsFinite(facing.Yaw));
        Check.Throws<ArgumentException>(()=>AttitudeController.AimBanked(forward,up,forward,new(double.NaN,0,0)));
    }
    [Test] public void OriginalAngularRateBrakesRotationBeforeQuantizedPoseReportsAnOvershoot()
    {
        var frame=new FlightFrame{Sequence=1,SystemAddress=1,ObservedAt=DateTimeOffset.UtcNow,Forward=new(0,0,1),Up=new(0,1,0)};
        var measured=new BankedAttitudeController().Update(frame,frame.Forward,frame.Up,new(3,-3,0));
        Check.That(measured.Yaw<0&&measured.Pitch>0);
        var stationary=new BankedAttitudeController().Update(frame,frame.Forward,frame.Up,Vector3d.Zero);
        Check.Equal(0d,stationary.Yaw);Check.Equal(0d,stationary.Pitch);
    }
    [Test] public void FacingTheStarDoesNotChaseAnUnstableBankProjection()
    {
        var controller=new BankedAttitudeController();var start=DateTimeOffset.Parse("2026-10-23T00:00:00Z");
        for(var i=0;i<200;i++)
        {
            var epsilon=i%2==0?.0001:-.0001;
            var frame=new FlightFrame{Sequence=i+1,SystemAddress=1,ObservedAt=start.AddSeconds(i*.02),Forward=new(0,0,1),Up=new(0,1,0)};
            var input=controller.Update(frame,new(0,1,0),new(epsilon,0,1),Vector3d.Zero);
            Check.Equal(0d,input.Roll);
        }
    }
    [Test] public void SmallSettledBankNoiseDoesNotProduceAlternatingRollInputs()
    {
        var controller=new BankedAttitudeController();var start=DateTimeOffset.Parse("2026-10-23T00:00:00Z");
        for(var i=0;i<200;i++)
        {
            var radians=(i%2==0?.2:-.2)*Math.PI/180;
            var frame=new FlightFrame{Sequence=i+1,SystemAddress=1,ObservedAt=start.AddSeconds(i*.02),Forward=new(0,0,1),Up=new(0,1,0)};
            var input=controller.Update(frame,frame.Forward,new(Math.Sin(radians),Math.Cos(radians),0),new(0,0,i%2==0?.1:-.1));
            Check.Equal(0d,input.Roll);
        }
    }
    [Test] public void AnOverheadTargetDoesNotTurnTinyCoordinateNoiseIntoFullYawCommands()
    {
        var controller=new BankedAttitudeController();var start=DateTimeOffset.Parse("2026-10-23T00:00:00Z");
        for(var i=0;i<100;i++)
        {
            var e=i%2==0?.000001:-.000001;
            var frame=new FlightFrame{Sequence=i+1,SystemAddress=1,ObservedAt=start.AddSeconds(i*.025),Forward=new(0,0,1),Up=new(0,1,0)};
            var input=controller.Update(frame,new(e,1,-e),frame.Up,Vector3d.Zero,false);
            Check.That(Math.Abs(input.Yaw)<.001&&input.Pitch<0);
        }
    }
    [Test] public void ATargetDirectlyBehindKeepsOneTurnDirectionUntilItsPlaneIsObservable()
    {
        var controller=new BankedAttitudeController();var start=DateTimeOffset.Parse("2026-10-23T00:00:00Z");
        for(var i=0;i<100;i++)
        {
            var e=i%2==0?.000001:-.000001;
            var frame=new FlightFrame{Sequence=i+1,SystemAddress=1,ObservedAt=start.AddSeconds(i*.025),Forward=new(0,0,1),Up=new(0,1,0)};
            var input=controller.Update(frame,new(e,-e,-1),frame.Up,Vector3d.Zero,false);
            Check.That(Math.Abs(input.Yaw)<.001&&input.Pitch>0);
        }
    }
    [Test] public void AnOppositeBankDoesNotFlipRollSignWithTinyProjectionNoise()
    {
        var controller=new BankedAttitudeController();var start=DateTimeOffset.Parse("2026-10-23T00:00:00Z");
        for(var i=0;i<100;i++)
        {
            var e=i%2==0?.000001:-.000001;
            var frame=new FlightFrame{Sequence=i+1,SystemAddress=1,ObservedAt=start.AddSeconds(i*.025),Forward=new(0,0,1),Up=new(0,1,0)};
            var input=controller.Update(frame,frame.Forward,new(e,-1,0),Vector3d.Zero);
            Check.That(input.Roll<0&&input.Yaw==0&&input.Pitch==0);
        }
    }
    [Test] public void BankedOrbitUsesPitchAndKeepsTheStarAboveInIndependentThreeDimensionalDynamics()
    {
        foreach(var hz in new[]{30,60,144})foreach(var response in new[]{.15,.4})
        {
            var controller=new BankedAttitudeController();var forward=new Vector3d(0,1,0);var up=new Vector3d(0,0,1);var rates=Vector3d.Zero;
            var start=DateTimeOffset.Parse("2026-10-22T00:00:00Z");var dt=1d/hz;var maximumRoll=0d;var meanYaw=0d;var meanPitch=0d;var samples=0;
            for(var i=0;i<25*hz;i++)
            {
                // A prescribed orbital path is independent of the attitude
                // controller. Pitch is much faster than yaw in this fixture.
                var angle=i*dt*3*Math.PI/180;var radial=new Vector3d(Math.Cos(angle),Math.Sin(angle),0);var tangent=new Vector3d(-Math.Sin(angle),Math.Cos(angle),0);
                var frame=new FlightFrame{Sequence=i+1,SystemAddress=1,ObservedAt=start.AddSeconds(i*dt),Position=radial*150,Forward=forward,Up=up};
                var input=controller.Update(frame,tangent,radial*-1);
                controller.ConfirmApplied(frame.ObservedAt,input);
                maximumRoll=Math.Max(maximumRoll,Math.Abs(input.Roll));
                Check.That(new[]{input.Yaw,input.Pitch,input.Roll}.All(a=>double.IsFinite(a)&&Math.Abs(a)<=1));
                var desired=new Vector3d(input.Yaw*3,input.Pitch*22,input.Roll*50);
                rates+=(desired-rates)*(1-Math.Exp(-dt/response));
                var right=Vector3d.Cross(up,forward).Unit();var omega=up*rates.X+right*rates.Y+forward*rates.Z;
                var speed=omega.Length;
                if(speed>1e-9){var axis=omega/speed;forward=Rotate(forward,axis,speed*dt*Math.PI/180).Unit();up=Rotate(up,axis,speed*dt*Math.PI/180).Unit();}
                if(i>15*hz)
                {
                    Check.That(Vector3d.Dot(up,radial*-1)>Math.Cos(5*Math.PI/180),$"Star left the upper cockpit: {hz} Hz / {response} s");
                    Check.That(Vector3d.AngleDegrees(forward,tangent)<5,$"Orbit heading failed: {hz} Hz / {response} s");
                    meanYaw+=Math.Abs(input.Yaw);meanPitch+=Math.Abs(input.Pitch);samples++;
                }
            }
            Check.That(maximumRoll>.01);Check.That(meanYaw/samples<.08&&meanPitch/samples>.08,"The orbit must use pitch rather than slow yaw.");
        }
        static Vector3d Rotate(Vector3d value,Vector3d axis,double radians)=>value*Math.Cos(radians)+Vector3d.Cross(axis,value)*Math.Sin(radians)+axis*(Vector3d.Dot(axis,value)*(1-Math.Cos(radians)));
    }
}