AutoMiningScript

using System;
using System.Collections.Generic;
using System.Reflection;
using System.Runtime.Remoting;
using Sandbox.ModAPI.Ingame;
using VRage.Game.ModAPI.Ingame;
using VRageMath;
using Xunit;
using P=AutoMiningScript.Program;

namespace AutoMiningScript.Tests
{
    public class DockingNavigationTests
    {
        static readonly BindingFlags Private=BindingFlags.Instance|BindingFlags.NonPublic;
        static T Read<T>(object instance,string name) {return (T)instance.GetType().GetField(name,Private).GetValue(instance);}
        static void Write(object instance,string name,object value) {instance.GetType().GetField(name,Private).SetValue(instance,value);}
        static void Call(object instance,string name,params object[] args) {instance.GetType().GetMethod(name,Private).Invoke(instance,args);}

        sealed class DockRig
        {
            public readonly MinerStateTests.MinerRig R=new MinerStateTests.MinerRig();
            public readonly P.FlightController Flight;
            public readonly P.DockFrame Dock;
            public MatrixD Orientation=MatrixD.Identity,Mount=MatrixD.CreateWorld(Vector3D.Zero,Vector3D.Backward,Vector3D.Up);
            public Vector3D Velocity,AngularVelocity=Vector3D.Zero;
            public bool CamerasReady=true,Connected,Magnetized,LockSucceeds=true;
            public long PeerId=51;
            public int ConnectCalls,Rays;
            public double Mass=1000,ContactSpacing=1;
            public readonly List<P.FlightState> States=new List<P.FlightState>();
            public Func<Vector3D,MyDetectedEntityInfo> Hit=p=>default(MyDetectedEntityInfo);
            public Func<bool> Capture;
            public DockRig(double finalSpeed=.2)
            {
                R.PrepareDeparture("valid-lease");R.Connector.Set("Status",MyShipConnectorStatus.Unconnected);
                R.Program.Config=new P.Settings("[Dock]\nFinalSpeed="+finalSpeed.ToString(System.Globalization.CultureInfo.InvariantCulture),"miner",R.Program.Config.Id);
                Write(R.Miner,"laneKind","dock");R.State(P.FlightState.DockAlign);
                Dock=Read<P.DockFrame>(R.Miner,"dock");Dock.GridId=50;Dock.ConnectorId=51;Dock.WaitingLocal=new Vector3D(-30,20,-100);
                Flight=Read<P.FlightController>(R.Miner,"flight");
                R.Controller.Method("get_WorldMatrix",a=>MatrixD.CreateWorld(R.Position,Orientation.Forward,Orientation.Up))
                    .Method("CalculateShipMass",a=>new MyShipMass((float)Mass,(float)Mass,(float)Mass))
                    .Method("GetShipVelocities",a=>new MyShipVelocities(Velocity,AngularVelocity));
                foreach(var thrust in R.Hardware.Thrusters)
                {
                    MatrixD local=thrust.WorldMatrix;var proxy=(Stub<IMyThrust>)RemotingServices.GetRealProxy(thrust);
                    proxy.Values.Remove("WorldMatrix");proxy.Method("get_WorldMatrix",a=>local*Orientation);
                }
                var gyro=(Stub<IMyGyro>)RemotingServices.GetRealProxy(R.Hardware.Gyros[0]);
                gyro.Values.Remove("WorldMatrix");gyro.Method("get_WorldMatrix",a=>Orientation);
                var peer=new Stub<IMyShipConnector>().Method("get_EntityId",a=>PeerId).Set("CubeGrid",new Stub<IMyCubeGrid>().Set("EntityId",50L).Value)
                    .Method("get_WorldMatrix",a=>MatrixD.CreateWorld(Dock.Position,Dock.Forward,Dock.Up)).Method("GetPosition",a=>Dock.Position);
                R.Connector.Values.Remove("Status");
                R.Connector.Method("GetPosition",a=>PortPosition).Method("get_WorldMatrix",a=>PortPose)
                    .Method("get_Status",a=>Connected?MyShipConnectorStatus.Connected:Magnetized||CanConnect?MyShipConnectorStatus.Connectable:MyShipConnectorStatus.Unconnected)
                    .Method("get_OtherConnector",a=>Connected?peer.Value:null)
                    .Method("Connect",a=>{ConnectCalls++;Connected=LockSucceeds&&(Magnetized||CanConnect);return null;});
                MountPort(Mount,new Vector3D(0,0,2),MatrixD.Identity,Vector3D.Zero);
                R.Hardware.Cameras.Clear();
                foreach(Vector3D direction in new[]{Vector3D.Forward,Vector3D.Backward})AddCamera(direction);
                R.Position=Contact+new Vector3D(-30,20,-80);
            }
            public void MountPort(MatrixD mounting,Vector3D offset,MatrixD saved,Vector3D calibrationError)
            {
                Mount=mounting;R.Hardware.ConnectorLocal=offset;
                R.Hardware.ConnectorForwardLocal=Mount.Forward;R.Hardware.ConnectorUpLocal=Mount.Up;
                Write(R.Miner,"calibratedPosition",Vector3D.Forward*ContactSpacing-Vector3D.TransformNormal(offset,saved)+calibrationError);
                Write(R.Miner,"calibratedForward",saved.Forward);Write(R.Miner,"calibratedUp",saved.Up);
            }
            public void AddCamera(Vector3D direction)
            {
                MatrixD mount=MatrixD.CreateWorld(Vector3D.Zero,direction,Math.Abs(direction.Y)>.9?Vector3D.Forward:Vector3D.Up);
                Func<MatrixD> pose=()=>mount*MatrixD.CreateWorld(R.Position,Orientation.Forward,Orientation.Up);
                R.Hardware.Cameras.Add(new Stub<IMyCameraBlock>().Set("CubeGrid",R.Program.Me.CubeGrid).Set("EntityId",100L+R.Hardware.Cameras.Count)
                    .Set("IsFunctional",true).Set("IsWorking",true).Set("Enabled",true).Set("EnableRaycast",true)
                    .Set("AvailableScanRange",100000d).Set("RaycastConeLimit",45f).Set("RaycastDistanceLimit",-1d)
                    .Method("GetPosition",a=>R.Position).Method("get_WorldMatrix",a=>pose())
                    .Method("CanScan",a=>{
                        var ray=Vector3D.TransformNormal((Vector3D)a[0]-R.Position,MatrixD.Transpose(pose()));
                        return CamerasReady && -ray.Z>0 && Math.Abs(Math.Atan2(ray.X,-ray.Z))<=Math.PI/4 &&
                            Math.Abs(Math.Atan2(ray.Y,Math.Sqrt(ray.X*ray.X+ray.Z*ray.Z)))<=Math.PI/4;
                    }).Method("Raycast",a=>{Rays++;return Hit((Vector3D)a[0]);}).Value);
            }
            public string Reason {get{return Read<string>(R.Miner,"reason");}}
            public MatrixD Desired {get{return MatrixD.Transpose(Mount)*MatrixD.CreateWorld(Vector3D.Zero,-Dock.Forward,Dock.Up);}}
            public Vector3D TargetPort {get{return Dock.Position+Dock.Forward*ContactSpacing;}}
            public Vector3D Contact {get{return TargetPort-Vector3D.TransformNormal(R.Hardware.ConnectorLocal,Desired);}}
            public Vector3D PortPosition {get{return R.Position+Vector3D.TransformNormal(R.Hardware.ConnectorLocal,Orientation);}}
            public MatrixD PortPose {get{return Mount*Orientation;}}
            public double Gap {get{return Vector3D.Dot(PortPosition-TargetPort,Dock.Forward);}}
            bool CanConnect {get{return (Capture!=null?Capture():Vector3D.Distance(PortPosition,TargetPort)<.15) && Vector3D.Dot(PortPose.Forward,-Dock.Forward)>.9998 && Vector3D.Dot(PortPose.Up,Dock.Up)>.9998;}}
            public void Tick(double dt=.05,bool integrate=false)
            {
                R.Program.Now+=dt;Dock.Position+=Dock.Velocity*dt;Dock.Center+=Dock.Velocity*dt;Dock.ReceivedAt=R.Program.Now;
                string method=R.StateNow==P.FlightState.Departing?"Departure":R.StateNow==P.FlightState.Holding?"Waiting":R.StateNow==P.FlightState.Returning?"ReturnHome":R.StateNow==P.FlightState.DockAlign?"DockAlign":R.StateNow==P.FlightState.DockApproach?"DockApproach":R.StateNow==P.FlightState.DockRetreat?"DockRetreat":null;
                if(method==null)return;
                Call(R.Miner,method,dt);States.Add(R.StateNow);
                if(!integrate || Connected)return;
                Integrate(dt);
            }
            public void Integrate(double dt)
            {
                Vector3D acceleration=Vector3D.Zero;
                foreach(var thrust in R.Hardware.Thrusters)acceleration+=thrust.WorldMatrix.Backward*thrust.MaxEffectiveThrust*thrust.ThrustOverridePercentage/Mass;
                Velocity+=acceleration*dt;R.Position+=Velocity*dt;
                // Ideal response integrates actual force commands and the requested pose.
                // Connector capture checks point/normal/up geometry; this is not game physics.
                if(Read<bool>(Flight,"orientationKnown"))Orientation=Read<MatrixD>(Flight,"previousDesired").GetOrientation();
            }
            public void Finish(int steps=3600)
            {
                for(int n=0;n<steps && !Connected && R.StateNow!=P.FlightState.DockRetreat;n++)Tick(.05,true);
                Assert.True(Connected,R.StateNow+" / gap="+Gap+" / "+Reason);
                Assert.Equal(P.FlightState.Servicing,R.StateNow);Assert.Equal(1,ConnectCalls);
                Assert.Single(R.Sent,p=>p.Kind=="LANE_RELEASE");
            }
        }

        static MyDetectedEntityInfo SphereHit(DockRig r,Vector3D center,double radius,Vector3D endpoint)
        {
            var delta=endpoint-r.R.Position;double length=delta.Length();if(length<.001)return default(MyDetectedEntityInfo);
            Vector3D direction=delta/length,offset=r.R.Position-center;double along=Vector3D.Dot(offset,direction);
            double discriminant=along*along-offset.LengthSquared()+radius*radius;
            if(discriminant<0)return default(MyDetectedEntityInfo);
            double distance=-along-Math.Sqrt(discriminant);if(distance<0 || distance>length)return default(MyDetectedEntityInfo);
            return new MyDetectedEntityInfo(96,"Stationary fault miner",MyDetectedEntityType.SmallGrid,r.R.Position+direction*distance,
                MatrixD.Identity,Vector3.Zero,VRage.Game.MyRelationsBetweenPlayerAndBlock.NoOwnership,
                new BoundingBoxD(center-new Vector3D(radius),center+new Vector3D(radius)),1);
        }

        [Fact]
        public void OccupiedOuterGuideSelectsACameraCheckedAxisEntryAndCompletesDocking()
        {
            var r=new DockRig();r.Dock.ApproachDistance=100;r.R.Hardware.Radius=4.63146;
            r.R.Hardware.BodyHalfSize=new Vector3D(2.25,1.75,2.25);
            r.R.Hardware.BodyMin=-r.R.Hardware.BodyHalfSize;r.R.Hardware.BodyMax=r.R.Hardware.BodyHalfSize;
            foreach(var direction in new[]{Vector3D.Right,Vector3D.Left,Vector3D.Up,Vector3D.Down})r.AddCamera(direction);
            var obstacle=r.Contact+r.Dock.Forward*100;double radius=2.25;
            r.R.Position=r.Contact+r.Dock.Forward*93+Vector3D.Right*11;
            r.Hit=end=>SphereHit(r,obstacle,radius,end);
            double nearest=double.MaxValue;bool alternate=false;
            for(int n=0;n<3600 && !r.Connected && r.R.StateNow!=P.FlightState.DockRetreat;n++)
            {
                r.Tick(.05,true);nearest=Math.Min(nearest,Vector3D.Distance(r.R.Position,obstacle));
                alternate|=Read<double>(r.R.Miner,"dockJoinDepth")>0 && Read<double>(r.R.Miner,"dockJoinDepth")<99;
            }
            Assert.True(alternate,"The occupied nominal entry was never replaced.");
            Assert.True(r.Rays>0);Assert.True(r.Connected,r.Reason);Assert.Equal(P.FlightState.Servicing,r.R.StateNow);
            Assert.True(nearest>=radius+r.R.Hardware.Radius,"Hull clearance: "+(nearest-radius));
            Assert.Equal(1,r.ConnectCalls);
        }

        [Fact]
        public void FreeGuideKeepsItsGeometryAndAChosenAlternativeStaysStableUntilCapture()
        {
            var r=new DockRig();r.Dock.ApproachDistance=30;r.R.Position=r.Contact+r.Dock.Forward*23+Vector3D.Right*11;
            var nominal=r.Contact+r.Dock.Forward*30;r.Tick();Assert.Equal(nominal,r.Flight.RouteTarget);
            r.Hit=end=>SphereHit(r,nominal,2,end);
            for(int n=0;n<160 && Read<double>(r.R.Miner,"dockJoinDepth")==30;n++)r.Tick(.05,true);
            double selected=Read<double>(r.R.Miner,"dockJoinDepth");Assert.InRange(selected,5,29.99);
            // New nominal clearance does not cancel a still-advancing alternate route.
            r.Hit=end=>default(MyDetectedEntityInfo);
            for(int n=0;n<8 && r.R.StateNow==P.FlightState.DockAlign;n++)
            {r.Tick(.05,true);Assert.Equal(selected,Read<double>(r.R.Miner,"dockJoinDepth"));}
            r.Finish();Assert.True(r.Rays>0);
        }

        [Fact]
        public void UnobservedAlternateEntriesNeverAuthorizeMovementAndHaveABoundedSearch()
        {
            var r=new DockRig();r.Dock.ApproachDistance=100;r.CamerasReady=false;
            r.R.Position=r.Contact+r.Dock.Forward*93+Vector3D.Right*11;var start=r.R.Position;
            for(int n=0;n<300 && r.R.StateNow==P.FlightState.DockAlign;n++)r.Tick(.5,true);
            Assert.Equal(P.FlightState.Holding,r.R.StateNow);Assert.InRange(Read<int>(r.R.Miner,"dockJoinAttempt"),1,7);
            Assert.Equal(0,r.Rays);Assert.Equal(0,r.ConnectCalls);Assert.InRange(Vector3D.Distance(start,r.R.Position),0,.001);
            Assert.Contains(r.R.Sent,p=>p.Kind=="LANE_RELEASE");
        }

        [Fact]
        public void TrafficYieldsGetShortGraceButCannotRenewTheDockingProgressWatchdogForever()
        {
            var r=new DockRig();r.CamerasReady=false;r.R.Position=r.Contact+r.Dock.Forward*30+Vector3D.Right*10;
            Write(r.R.Miner,"progressAt",r.R.Program.Now-59.5);
            for(int n=0;n<500 && r.R.StateNow==P.FlightState.DockAlign;n++)
            {
                r.R.Program.Now+=.05;r.Dock.ReceivedAt=r.R.Program.Now;r.Flight.TrafficWait=true;r.Flight.TrafficAt=r.R.Program.Now;
                Call(r.R.Miner,"YieldAtPort",.05);
                if(n==119)Assert.Equal(P.FlightState.DockAlign,r.R.StateNow);
            }
            Assert.Equal(P.FlightState.Holding,r.R.StateNow);Assert.Contains(r.R.Sent,p=>p.Kind=="LANE_RELEASE");
            Assert.Equal(0,r.ConnectCalls);
        }

        static void TrafficSurvey(DockRig r)
        {
            Write(r.R.Miner,"job",new P.Job {Id="observation",Kind=P.JobKind.Survey,Entry=r.R.Position+Vector3D.Forward*100,Direction=Vector3D.Forward,Up=Vector3D.Up});
            r.R.State(P.FlightState.Transit);r.Flight.TrafficWait=true;
        }
        [Fact]
        public void SustainedTrafficCannotBypassTheSurveyEntryDeadlineOrLeaveItIdle()
        {
            var r=new DockRig();TrafficSurvey(r);double start=r.R.Program.Now;
            for(int n=0;n<1200 && r.R.StateNow==P.FlightState.Transit;n++)
            {r.R.Program.Now+=.05;r.Flight.TrafficAt=r.R.Program.Now;Call(r.R.Miner,"YieldAtPort",.05);}
            var packet=Assert.Single(r.R.Sent,p=>p.Kind=="RESULT");var report=P.JobReport.FromIni(packet.Body);
            Assert.Equal("Blocked",report.Outcome);Assert.True(report.EntryUnreachable);
            Assert.InRange(r.R.Program.Now-start,55,56);Assert.Equal(P.FlightState.Survey,r.R.StateNow);
            Assert.Equal("observation",Read<P.JobReport>(r.R.Miner,"nextReport").JobId);Assert.True(Read<bool>(r.R.Miner,"continuing"));
        }
        [Fact]
        public void ActualSurveyProgressDuringTrafficKeepsItsDeadlineAlive()
        {
            var r=new DockRig();TrafficSurvey(r);
            for(int n=0;n<1200;n++)
            {
                r.R.Program.Now+=.05;if(n%40==0)r.R.Position+=Vector3D.Forward*2.5;
                r.Flight.TrafficAt=r.R.Program.Now;Call(r.R.Miner,"YieldAtPort",.05);
            }
            Assert.Equal(P.FlightState.Transit,r.R.StateNow);Assert.DoesNotContain(r.R.Sent,p=>p.Kind=="RESULT");
        }
        [Fact]
        public void CameraSurfaceFoundWhileYieldingTakesPriorityOverAnExpiredSurveyDeadline()
        {
            var r=new DockRig();TrafficSurvey(r);Write(r.R.Miner,"lastProgress",100d);Write(r.R.Miner,"progressAt",r.R.Program.Now-60);
            r.Flight.SurfaceHit=new P.JobReport {Outcome="SurveyHit",EntityId=42,Position=r.R.Position+Vector3D.Forward*10,Direction=Vector3D.Forward};
            r.Flight.TrafficAt=r.R.Program.Now;Call(r.R.Miner,"YieldAtPort",.05);
            var report=P.JobReport.FromIni(Assert.Single(r.R.Sent,p=>p.Kind=="RESULT").Body);
            Assert.Equal("SurveyHit",report.Outcome);Assert.False(report.EntryUnreachable);Assert.Equal(42,report.EntityId);
        }
        [Fact]
        public void HullTelemetryUsesControllerRelativeBoundsAndControllerPointVelocity()
        {
            var r=new DockRig();r.R.Position=new Vector3D(10,20,30);r.R.Controller.Set("CenterOfMass",new Vector3D(8,20,30));
            r.Velocity=new Vector3D(1,2,3);r.AngularVelocity=new Vector3D(0,0,.5);
            var snapshot=(P.Telemetry)r.R.Miner.GetType().GetMethod("Snapshot",Private).Invoke(r.R.Miner,null);
            Assert.True(snapshot.HasBodyBounds);Assert.Equal(r.R.Hardware.BodyMin,snapshot.BodyMin);Assert.Equal(r.R.Hardware.BodyMax,snapshot.BodyMax);
            Assert.Equal(new Vector3D(1,3,3),snapshot.Velocity);Assert.Equal(r.AngularVelocity,snapshot.AngularVelocity);Assert.Equal(2,snapshot.RotationRadius);
        }

        [Theory]
        [InlineData(0d)]
        [InlineData(1d)]
        public void AssignedPortIsReachedAndConnectedWithoutVisitingTheDistantWaitingPoint(double motherSpeed)
        {
            var r=new DockRig();r.Dock.Velocity=Vector3D.Right*motherSpeed;r.R.State(P.FlightState.Holding);
            double start=r.Gap;r.Tick(.05,true);Assert.NotEqual(P.FlightState.Holding,r.R.StateNow);
            r.Finish();Assert.True(r.Gap<start);Assert.Contains(P.FlightState.DockAlign,r.States);Assert.Contains(P.FlightState.DockApproach,r.States);
        }

        [Theory]
        [InlineData(P.FlightState.Returning)]
        [InlineData(P.FlightState.Holding)]
        [InlineData(P.FlightState.DockAlign)]
        [InlineData(P.FlightState.DockApproach)]
        [InlineData(P.FlightState.DockRetreat)]
        public void MagneticCaptureLocksBeforeGeometricCorrectionsOrTelemetryWaits(P.FlightState state)
        {
            var r=new DockRig();r.R.State(state);r.Magnetized=true;
            r.R.Position=r.Contact+Vector3D.Right*.4;r.Orientation=MatrixD.CreateFromAxisAngle(Vector3D.Forward,.1);
            r.Velocity=Vector3D.Right*.4;r.AngularVelocity=Vector3D.Forward*.02;r.Dock.ReceivedAt=r.R.Program.Now-10;
            r.R.Hardware.Thrusters[0].ThrustOverridePercentage=.5f;r.R.Hardware.Gyros[0].GyroOverride=true;
            r.R.Miner.Tick(.05);
            Assert.True(r.Connected);Assert.Equal(1,r.ConnectCalls);Assert.Equal(P.FlightState.Servicing,r.R.StateNow);
            Assert.All(r.R.Hardware.Thrusters,t=>Assert.Equal(0f,t.ThrustOverridePercentage));
            Assert.All(r.R.Hardware.Gyros,g=>Assert.False(g.GyroOverride));Assert.False(r.Flight.HasRoute);
            Assert.Contains(r.R.Sent,p=>p.Kind=="TELEMETRY" && P.Telemetry.FromIni(p.Body).Connected);
        }

        [Fact]
        public void PendingMagneticLockStopsCorrectionAndRetriesUntilActuallyConnected()
        {
            var r=new DockRig();r.R.State(P.FlightState.DockApproach);r.Magnetized=true;r.LockSucceeds=false;
            r.Tick();Assert.False(r.Connected);Assert.Equal(1,r.ConnectCalls);
            Assert.Equal(P.FlightState.DockApproach,r.R.StateNow);Assert.Equal(P.L.MinerDockLockPending,r.Reason);
            Assert.False(r.Flight.HasRoute);Assert.Equal(0,r.Rays);
            r.LockSucceeds=true;r.Tick();Assert.True(r.Connected);Assert.Equal(2,r.ConnectCalls);
            Assert.Equal(P.FlightState.Servicing,r.R.StateNow);
        }

        [Theory]
        [InlineData("pause")]
        [InlineData("release")]
        public void MagneticCaptureDoesNotOverrideAnOperatorStop(string command)
        {
            var r=new DockRig();r.Magnetized=true;r.R.Miner.Command(command);r.R.Miner.Tick(.05);
            Assert.False(r.Connected);Assert.Equal(0,r.ConnectCalls);
        }

        [Fact]
        public void DepartureDoesNotRelockWhileConnectorsAreStillMagneticallyInRange()
        {
            var r=new DockRig();r.Magnetized=true;r.R.State(P.FlightState.Departing);Write(r.R.Miner,"laneKind","depart");
            for(int n=0;n<3;n++)r.R.Miner.Tick(.05);
            Assert.False(r.Connected);Assert.Equal(0,r.ConnectCalls);Assert.Equal(P.FlightState.Departing,r.R.StateNow);
        }

        [Fact]
        public void APhysicallyConnectedUnexpectedPortIsReportedWithoutContinuingThrust()
        {
            var r=new DockRig();r.Magnetized=true;r.PeerId=999;r.R.State(P.FlightState.DockApproach);r.R.Miner.Tick(.05);
            Assert.True(r.Connected);Assert.Equal(P.FlightState.Fault,r.R.StateNow);Assert.Equal(P.L.MinerWrongDock,r.Reason);
            Assert.False(r.Flight.HasRoute);Assert.All(r.R.Hardware.Thrusters,t=>Assert.Equal(0f,t.ThrustOverridePercentage));
        }

        [Theory]
        [InlineData(5d)]
        [InlineData(20d)]
        [InlineData(30d)]
        public void AShipAlreadyInFrontOfItsPortNeverFliesBackOutToTheGuideOrWaitingPoint(double gap)
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*gap;r.R.State(P.FlightState.Returning);
            Read<List<Vector3D>>(r.R.Miner,"breadcrumbs").Add(r.Contact+r.Dock.Forward*150);Write(r.R.Miner,"returnIndex",0);
            for(int n=0;n<2400 && !r.Connected && r.R.StateNow!=P.FlightState.DockRetreat;n++)
            {r.Tick(.05,true);Assert.True(r.Gap<=gap+.02,r.Reason);}
            Assert.True(r.Connected,r.Reason);Assert.Equal(-1,Read<int>(r.R.Miner,"returnIndex"));
        }

        [Theory]
        [InlineData(false, 20d, 1d)]
        [InlineData(false, 130d, 1d)]
        [InlineData(true, 130d, 1d)]
        [InlineData(true, 130d, 2.8d)]
        [InlineData(false, 130d, 5d)]
        [InlineData(true, 130d, 5d)]
        public void FirstReturnWithoutStoredCalibrationReachesPhysicalConnectorAndCalibrates(bool side,double distance,double spacing)
        {
            var r=new DockRig();r.ContactSpacing=spacing;
            var mount=MatrixD.CreateWorld(Vector3D.Zero,side?Vector3D.Right:Vector3D.Backward,Vector3D.Up);
            r.MountPort(mount,new Vector3D(1,.5,2),MatrixD.Identity,Vector3D.Zero);
            Write(r.R.Miner,"calibrated",false);Write(r.R.Miner,"calibratedPosition",Vector3D.Zero);
            Write(r.R.Miner,"calibratedForward",Vector3D.Zero);Write(r.R.Miner,"calibratedUp",Vector3D.Zero);
            r.AddCamera(Vector3D.Right);r.AddCamera(Vector3D.Left);
            r.R.Position=r.Contact+r.Dock.Forward*distance+Vector3D.Right*10;r.R.State(P.FlightState.Returning);
            for(int n=0;n<3400 && !r.Connected && r.R.StateNow!=P.FlightState.Fault;n++)
            {
                r.Tick(.05,true);
                Assert.False(Read<bool>(r.R.Miner,"calibrated"));
                Assert.True(r.Gap>-.2,"Passed the actual mating plane: "+r.Gap+" / "+r.Reason);
            }
            Assert.True(r.Connected,r.Reason);Assert.Equal(1,r.ConnectCalls);
            Assert.Contains(P.FlightState.DockAlign,r.States);Assert.Contains(P.FlightState.DockApproach,r.States);
            r.R.Miner.Tick(.1);Assert.True(Read<bool>(r.R.Miner,"calibrated"));
            Assert.Equal(P.FlightState.Servicing,r.R.StateNow);
        }

        [Fact]
        public void LegacyOneTenthFinalSpeedIsRaisedToHalfAMetrePerSecond()
        {
            var r=new DockRig(.1);r.R.Position=r.Contact+r.Dock.Forward*30+Vector3D.Right*.25;r.R.State(P.FlightState.DockApproach);
            double start=r.R.Program.Now;
            for(int n=0;n<100;n++)r.Tick(.05,true);
            Assert.True(r.Gap<25,r.Gap+" / "+r.Reason);Assert.True(r.Velocity.Length()>1);
            bool sawFinal=false;
            for(int n=0;n<2800 && !r.Connected && r.R.StateNow!=P.FlightState.DockRetreat;n++)
            {
                r.Tick(.05,true);
                if(r.Gap<.6 && r.Gap>.3) {sawFinal=true;Assert.True(r.Flight.CommandSpeed>=.5);Assert.InRange(r.Velocity.Length(),.45,.6);}
            }
            Assert.True(r.Connected,r.Reason);Assert.True(sawFinal);Assert.True(r.R.Program.Now-start<140);
        }

        [Theory]
        [InlineData(false,.05)]
        [InlineData(false,.1)]
        [InlineData(false,.2)]
        [InlineData(false,.5)]
        [InlineData(false,1)]
        [InlineData(true,.5)]
        public void CalibratedFinalThreeMetresFinishPromptlyWithLegacyAndNewDefaults(bool newTemplate,double configuredSpeed)
        {
            var r=new DockRig(configuredSpeed);var finalSpeed=Math.Max(.5,configuredSpeed);
            if(newTemplate)r.R.Program.Config=new P.Settings(P.Settings.Template("miner",r.R.Program.Config.Id),"miner",r.R.Program.Config.Id);
            r.R.Position=r.Contact+r.Dock.Forward*2.8;r.R.State(P.FlightState.DockApproach);
            double start=r.R.Program.Now;bool accelerated=false,captured=false;
            for(int n=0;n<320 && !r.Connected && r.R.StateNow==P.FlightState.DockApproach;n++)
            {
                r.Tick(.05,true);
                if(r.Gap>1.5 && r.Gap<2.7 && r.Flight.CommandSpeed>.65)accelerated=true;
                if(r.Gap>.25 && r.Gap<.6)
                {
                    captured=true;Assert.InRange(r.Flight.CommandSpeed,finalSpeed,finalSpeed+.001);
                    Assert.InRange((r.Velocity-r.Dock.Velocity).Length(),finalSpeed-.01,finalSpeed+.1);
                }
                Assert.True(r.Gap>-.2,"Passed the mating plane: "+r.Gap);
            }
            Assert.True(r.Connected,r.Gap+" / "+r.Reason);Assert.True(accelerated);Assert.True(captured);
            Assert.True(r.R.Program.Now-start<14);Assert.Equal(1,r.ConnectCalls);
        }

        [Theory]
        [InlineData(.1,.16)]
        [InlineData(.2,.3)]
        [InlineData(.5,.6)]
        public void StableCaptureDoesNotCrawlBelowHalfAMetrePerSecondBecauseOfPositionGain(double configuredSpeed,double gap)
        {
            var r=new DockRig(configuredSpeed);
            r.R.Program.Config=new P.Settings("[Flight]\nPositionGain=0.1\n[Dock]\nFinalSpeed="+configuredSpeed.ToString(System.Globalization.CultureInfo.InvariantCulture),"miner",r.R.Program.Config.Id);
            r.R.Position=r.Contact+r.Dock.Forward*gap;r.R.State(P.FlightState.DockApproach);Write(r.R.Miner,"stableSince",r.R.Program.Now-3);
            r.Velocity=-r.Dock.Forward*.5;r.CamerasReady=false;
            for(int n=0;n<80 && !r.Connected;n++)
            {
                r.Tick(.05,true);
                if(!r.Connected){Assert.InRange(r.Flight.CommandSpeed,.5,.501);Assert.InRange(r.Velocity.Length(),.49,.51);}
                Assert.True(r.Gap>-.1,"Crossed the mating plane: "+r.Gap);
            }
            Assert.True(r.Connected,r.Gap+" / "+r.Reason);Assert.Equal(1,r.ConnectCalls);Assert.Equal(0,r.Rays);
        }

        static DockRig MeasuredCaptureRig(bool side=false)
        {
            var r=new DockRig(.5);r.Mass=10999.4;r.ContactSpacing=2.226945938;
            if(side)r.Mount=MatrixD.CreateWorld(Vector3D.Zero,Vector3D.Right,Vector3D.Up);
            r.MountPort(r.Mount,side?Vector3D.Right*1.75:Vector3D.Backward*1.75,r.Desired,Vector3D.Zero);
            r.Orientation=r.Desired;r.R.Position=r.Contact+r.Dock.Forward*.00075;
            r.R.State(P.FlightState.DockApproach);Write(r.R.Miner,"stableSince",r.R.Program.Now-3);
            return r;
        }

        [Theory]
        [InlineData(false)]
        [InlineData(true)]
        public void SavedContactOutsideTheObservedCapturePointKeepsClosingInsteadOfReversing(bool side)
        {
            var r=MeasuredCaptureRig(side);var nominal=r.Contact;
            // The real return stopped at centre spacing 2.226945938. Another observed
            // return first reported Connected at 1.993233; use distinct geometry so the
            // test cannot declare capture merely because the old navigation target arrived.
            r.Capture=()=>Vector3D.Distance(r.PortPosition,r.Dock.Position)<=1.993233;
            for(int n=0;n<300 && !r.Connected && r.R.StateNow==P.FlightState.DockApproach;n++)
            {
                r.Tick(1.0/60,true);
                if(!r.Connected)
                {
                    Assert.InRange(r.Flight.CommandSpeed,.5,.501);
                    Assert.InRange(Vector3D.Dot(r.Velocity,r.Dock.Forward),-.501,0);
                    Assert.True(Vector3D.Dot(r.Flight.RouteTarget-r.R.Position,r.Dock.Forward)<-.24);
                }
            }
            Assert.True(r.Connected,r.Gap+" / "+r.Reason);Assert.Equal(1,r.ConnectCalls);
            Assert.InRange(Vector3D.Dot(r.R.Position-nominal,r.Dock.Forward),-.35,-.23);
            Assert.Equal(P.FlightState.Servicing,r.R.StateNow);
        }

        [Fact]
        public void MissingMagneticCaptureBrakesAtTheBoundAndThenMovesOutward()
        {
            var r=MeasuredCaptureRig();r.Capture=()=>false;double previousVelocity=0;
            for(int n=0;n<300 && r.R.StateNow==P.FlightState.DockApproach;n++)
            {previousVelocity=Vector3D.Dot(r.Velocity,r.Dock.Forward);r.Tick(1.0/60,true);}
            Assert.Equal(P.FlightState.DockRetreat,r.R.StateNow);Assert.Equal(P.L.MinerDockCaptureLimit,r.Reason);
            Assert.True(Vector3D.Dot(r.Velocity,r.Dock.Forward)>previousVelocity,"The limit must brake immediately.");
            var retreatDepth=r.Gap;var minimum=retreatDepth;Assert.True(retreatDepth<-.35);
            for(int n=0;n<180;n++){r.Tick(1.0/60,true);minimum=Math.Min(minimum,r.Gap);}
            Assert.True(r.Gap>retreatDepth+.5,"The craft kept chasing the inward capture target.");
            Assert.True(minimum>retreatDepth-.2,"Retreat did not arrest the remaining inward motion.");
            Assert.True(Vector3D.Dot(r.Velocity,r.Dock.Forward)>.25);Assert.Equal(0,r.ConnectCalls);
        }

        [Fact]
        public void RealMagneticCaptureStillLocksBeforeTheInwardLimitCheck()
        {
            var r=MeasuredCaptureRig();r.R.Position=r.Contact-r.Dock.Forward*.36;r.Magnetized=true;
            r.Tick();Assert.True(r.Connected);Assert.Equal(P.FlightState.Servicing,r.R.StateNow);
            Assert.Equal(1,r.ConnectCalls);Assert.DoesNotContain(P.FlightState.DockRetreat,r.States);
        }

        [Theory]
        [InlineData(.4,true)]
        [InlineData(.05,false)]
        public void FortyFrameMagneticPollingEitherCapturesOrTriggersBoundedRetreat(double phase,bool expectedCapture)
        {
            var r=MeasuredCaptureRig();double next=r.R.Program.Now+phase;bool available=false;
            r.Capture=()=>{
                if(r.R.Program.Now>=next)
                {next+=40.0/60;available=Vector3D.Distance(r.PortPosition,r.Dock.Position)<=1.993233;}
                return available;
            };
            for(int n=0;n<300 && !r.Connected && r.R.StateNow==P.FlightState.DockApproach;n++)r.Tick(1.0/60,true);
            Assert.Equal(expectedCapture,r.Connected);
            Assert.Equal(expectedCapture?P.FlightState.Servicing:P.FlightState.DockRetreat,r.R.StateNow);
            Assert.Equal(expectedCapture?1:0,r.ConnectCalls);
            if(!expectedCapture)Assert.Equal(P.L.MinerDockCaptureLimit,r.Reason);
        }

        [Fact]
        public void StraightApproachKeepsMovingAcrossTheFormerSixtyAndThirtyMetreStops()
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*90;
            for(int n=0;n<3200 && !r.Connected && r.R.StateNow!=P.FlightState.DockRetreat;n++)
            {
                r.Tick(.05,true);
                if(r.Gap<75 && r.Gap>10)Assert.True((r.Velocity-r.Dock.Velocity).Length()>.5,"gap="+r.Gap+" / "+r.Reason);
            }
            Assert.True(r.Connected,r.Reason);
        }

        [Theory]
        [InlineData(false,0d)]
        [InlineData(false,90d)]
        [InlineData(true,25d)]
        public void PhysicalConnectorNormalsAndUpAlignDespiteOffsetMountAndOldManualPoseError(bool side,double rollDegrees)
        {
            var r=new DockRig();var mount=MatrixD.CreateWorld(Vector3D.Zero,side?Vector3D.Right:Vector3D.Backward,Vector3D.Up);
            var initial=MatrixD.Transpose(mount)*MatrixD.CreateWorld(Vector3D.Zero,Vector3D.Backward,Vector3D.Up);
            var saved=initial*MatrixD.CreateFromAxisAngle(Vector3D.Forward,rollDegrees*Math.PI/180)*MatrixD.CreateFromAxisAngle(Vector3D.Up,.003);
            r.MountPort(mount,new Vector3D(1,.5,2),saved,new Vector3D(.12,-.07,0));
            r.Orientation=initial;r.R.Position=r.Contact+Vector3D.Forward*30;r.R.State(P.FlightState.DockApproach);
            r.AddCamera(Vector3D.Right);r.AddCamera(Vector3D.Left);r.Finish();
            Assert.True(Vector3D.Dot(r.PortPose.Forward,-r.Dock.Forward)>.99999);
            Assert.True(Vector3D.Dot(r.PortPose.Up,r.Dock.Up)>.99999);
            Vector3D error=r.PortPosition-r.TargetPort;Assert.True((error-r.Dock.Forward*Vector3D.Dot(error,r.Dock.Forward)).Length()<.05);
        }

        [Fact]
        public void UnalignedPoseHoldsTheNormalGapAndReportsTheActualRemainingDistance()
        {
            var r=new DockRig();r.R.Position=r.Contact+Vector3D.Forward*30;r.R.State(P.FlightState.DockApproach);
            r.Orientation=MatrixD.CreateFromAxisAngle(Vector3D.Forward,.2);r.Tick();
            Assert.Contains(P.L.MinerDockSyncPose,r.Reason);Assert.Contains("30.0",r.Reason);
            Assert.True(Vector3D.Dot(Read<MatrixD>(r.Flight,"previousDesired").Up,r.Dock.Up)>.99999);
            Assert.True(Vector3D.Distance(Read<Vector3D>(r.Flight,"routeTarget"),r.R.Position)<.01);
        }

        [Fact]
        public void FinalMisalignmentCannotCloseTheRemainingGap()
        {
            var r=new DockRig();r.R.Position=r.Contact+Vector3D.Forward*2+Vector3D.Right*.3;r.R.State(P.FlightState.DockApproach);r.Tick();
            Assert.Contains(P.L.MinerDockSyncPose,r.Reason);
            Assert.InRange(Vector3D.Dot(Read<Vector3D>(r.Flight,"routeTarget")-r.R.Position,r.Dock.Forward),-.001,.001);
            Assert.Equal(0,r.ConnectCalls);
        }

        [Fact]
        public void ProgressingApproachCanExceedTheOldFourMinuteTimeout()
        {
            var r=new DockRig();r.R.State(P.FlightState.DockApproach);
            for(int n=0;n<30;n++) {r.R.Position=r.Contact+Vector3D.Forward*(500-n*5);r.Tick(10);}
            Assert.Equal(P.FlightState.DockApproach,r.R.StateNow);
        }

        [Fact]
        public void NoPhysicalProgressTimesOutAndKeepsTheLaneUntilRetreat()
        {
            var r=new DockRig();r.R.Position=r.Contact+Vector3D.Forward*30;r.R.State(P.FlightState.DockApproach);r.CamerasReady=false;
            for(int n=0;n<65 && r.R.StateNow==P.FlightState.DockApproach;n++)r.Tick(1);
            Assert.Equal(P.FlightState.DockRetreat,r.R.StateNow);Assert.Contains(P.L.MinerDockTimeout,r.Reason);
            Assert.Contains(P.L.MinerDockFollowAxis,r.Reason);Assert.Contains("30.0",r.Reason);
            Assert.DoesNotContain(P.L.MinerEnvelopeScanPending,r.Reason);Assert.Equal(0,r.Rays);
            Assert.DoesNotContain(r.R.Sent,p=>p.Kind=="LANE_RELEASE");
        }

        [Theory]
        [InlineData(91.1)] [InlineData(126.7)] [InlineData(31.2)]
        public void ReturnAlongPortAxisUsesTheSingleForwardCameraUntilEntry(double distance)
        {
            var r=new DockRig();r.R.Hardware.Cameras.Clear();r.AddCamera(Vector3D.Forward);
            r.Orientation=r.Desired;r.R.Position=r.Contact+r.Dock.Forward*distance;
            r.R.State(P.FlightState.Transit);r.R.Miner.Command("return");
            r.Finish();Assert.True(r.Rays>0);Assert.DoesNotContain(P.FlightState.Fault,r.States);
        }

        [Fact]
        public void WaitingForEntryScanYieldsTheLaneWithoutCountingADockingFailure()
        {
            var r=new DockRig();r.CamerasReady=false;r.R.Position=r.Contact+r.Dock.Forward*100;r.R.State(P.FlightState.DockAlign);
            for(int n=0;n<65 && r.R.StateNow==P.FlightState.DockAlign;n++)r.Tick(1);
            Assert.Equal(P.FlightState.Holding,r.R.StateNow);Assert.Equal(0,Read<int>(r.R.Miner,"retry"));
            Assert.Contains(r.R.Sent,p=>p.Kind=="LANE_RELEASE");Assert.Contains(P.L.MinerDockAlignTimeout,r.Reason);
        }

        [Fact]
        public void ExplicitReturnFromFaultStartsAFreshDockingAttempt()
        {
            var r=new DockRig();Write(r.R.Miner,"retry",3);r.R.State(P.FlightState.Fault);r.R.Miner.Command("return");
            Assert.Equal(P.FlightState.Returning,r.R.StateNow);Assert.Equal(0,Read<int>(r.R.Miner,"retry"));
            r.R.State(P.FlightState.DockApproach);Write(r.R.Miner,"retry",1);r.R.Miner.Command("return");
            Assert.Equal(P.FlightState.DockApproach,r.R.StateNow);Assert.Equal(1,Read<int>(r.R.Miner,"retry"));
        }

        [Theory]
        [InlineData(P.FlightState.DockAlign,0)] [InlineData(P.FlightState.DockApproach,1)]
        public void OnlyAnActualFinalApproachConsumesTheDockFailureBudget(P.FlightState state,int failures)
        {
            var r=new DockRig();r.R.State(state);Call(r.R.Miner,"AbortDock","pose unavailable");
            Assert.Equal(failures,Read<int>(r.R.Miner,"retry"));Assert.Equal(P.FlightState.DockRetreat,r.R.StateNow);
        }

        [Theory]
        [InlineData(false)] [InlineData(true)]
        public void RepeatedDockMotionOrStaleTelemetryDoesNotBecomeThreeCaptureFailures(bool stale)
        {
            var r=new DockRig();
            for(int n=0;n<4;n++)
            {
                r.R.Program.Now+=1;r.R.State(P.FlightState.DockApproach);
                r.Dock.ReceivedAt=r.R.Program.Now-(stale?1:0);r.Dock.Acceleration=stale?Vector3D.Zero:Vector3D.Right*.12;
                Call(r.R.Miner,"DockApproach",.05);
                Assert.Equal(P.FlightState.DockRetreat,r.R.StateNow);Assert.Equal(0,Read<int>(r.R.Miner,"retry"));
            }
        }

        [Fact]
        public void ObstacleOnMotherStillBlocksUnverifiedEntryWithDetoursEnabled()
        {
            var r=new DockRig();r.R.Position=r.Contact+Vector3D.Forward*30+Vector3D.Right*10;
            r.AddCamera(Vector3D.Left);
            var wall=new BoundingBoxD(new Vector3D(7,-1000,-1000),new Vector3D(8,1000,1000));
            r.Hit=end=>{
                var ray=end-r.R.Position;double length=ray.Length();var direction=Vector3D.Normalize(ray);
                var distance=wall.Intersects(new RayD(r.R.Position,direction));
                if(!distance.HasValue || distance.Value>length)return default(MyDetectedEntityInfo);
                return new MyDetectedEntityInfo(50,"mother wall",MyDetectedEntityType.LargeGrid,r.R.Position+direction*distance.Value,
                    MatrixD.Identity,Vector3.Zero,VRage.Game.MyRelationsBetweenPlayerAndBlock.NoOwnership,wall,1);
            };
            double clearance=double.MaxValue;
            for(int n=0;n<160;n++){r.Tick(.05,true);clearance=Math.Min(clearance,r.R.Position.X-wall.Max.X-r.R.Hardware.BodyHalfSize.X);}
            Assert.True(r.Rays>0);Assert.True(clearance>=.2,"Crossed the observed wall: "+clearance);Assert.Equal(0,r.ConnectCalls);
        }

        [Fact]
        public void ScannedNavigationStillUsesClearSpaceAndStopsBeforeHullContact()
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*14;r.R.State(P.FlightState.DockApproach);
            var wall=r.Contact+r.Dock.Forward*5;double closest=double.MaxValue;
            r.Hit=end=>{
                Vector3D ray=end-r.R.Position;
                double fraction=Vector3D.Dot(wall-r.R.Position,r.Dock.Forward)/Vector3D.Dot(ray,r.Dock.Forward);
                if(fraction<0 || fraction>1)return default(MyDetectedEntityInfo);
                Vector3D point=r.R.Position+ray*fraction;
                return new MyDetectedEntityInfo(50,"Berth railing",MyDetectedEntityType.LargeGrid,point,MatrixD.Identity,Vector3.Zero,
                    VRage.Game.MyRelationsBetweenPlayerAndBlock.NoOwnership,new BoundingBoxD(point-new Vector3D(.1),point+new Vector3D(.1)),1);
            };
            for(int n=0;n<500;n++)
            {
                r.R.Program.Now+=.05;
                r.Flight.Move(r.Contact,Vector3D.Zero,r.Desired.Forward,r.Desired.Up,2,.1,true,.05,false,false);
                r.Integrate(.05);
                closest=Math.Min(closest,Vector3D.Dot(r.R.Position-wall,r.Dock.Forward)-r.R.Hardware.BodyHalfSize.Z);
            }
            Assert.True(r.Gap<8,r.Reason);Assert.True(closest>=.2,"Hull passed its stopping boundary: "+closest);
            Assert.True(r.Velocity.Length()<.02,r.Reason);Assert.False(r.Connected);Assert.Equal(0,r.ConnectCalls);
            Assert.Contains("Berth railing",r.Flight.Problem);Assert.Contains(P.L.MinerObstacleBraking,r.Flight.Problem);
        }

        [Theory]
        [InlineData(2.8,false)]
        [InlineData(14,false)]
        [InlineData(30,true)]
        public void FormalDockingCompletesWithoutCameraChargeOrObstacleQueries(double gap,bool obstruction)
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*gap;r.R.State(P.FlightState.DockApproach);
            r.CamerasReady=obstruction;
            r.Hit=end=>new MyDetectedEntityInfo(50,"Berth railing",MyDetectedEntityType.LargeGrid,r.R.Position-r.Dock.Forward,
                MatrixD.Identity,Vector3.Zero,VRage.Game.MyRelationsBetweenPlayerAndBlock.NoOwnership,new BoundingBoxD(new Vector3D(-1),new Vector3D(1)),1);
            r.Finish();Assert.Equal(0,r.Rays);
        }

        [Fact]
        public void EnteringFormalDockingClearsOldDetourAndScanConstraints()
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*14;
            Write(r.Flight,"routeKnown",true);Write(r.Flight,"routeTarget",r.Contact);
            Write(r.Flight,"hasDetour",true);Write(r.Flight,"detour",r.R.Position+Vector3D.Right*20);
            Write(r.Flight,"scanBlocked",true);r.Flight.Problem="old scan";r.CamerasReady=false;
            r.Tick();Assert.Equal(P.FlightState.DockApproach,r.R.StateNow);
            Assert.False(Read<bool>(r.Flight,"hasDetour"));Assert.Equal(r.Contact,Read<Vector3D>(r.Flight,"routeTarget"));
            Assert.Equal("",r.Flight.Problem);Assert.Equal(0,r.Rays);r.Finish();
        }

        [Theory]
        [InlineData(true)]
        [InlineData(false)]
        public void CamerasStillGateTheTripToTheDockEntryButNotFormalDocking(bool onAxis)
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*80+(onAxis?Vector3D.Zero:Vector3D.Right*10);
            r.CamerasReady=false;r.Tick();
            Assert.Equal(P.FlightState.DockAlign,r.R.StateNow);Assert.NotEqual("",r.Flight.Problem);
            Assert.Equal(0,r.Flight.CommandSpeed);Assert.Equal(0,r.ConnectCalls);
        }

        [Fact]
        public void FormalDockingDiagnosticsShowActualCommandInsteadOfStaleCameraClearance()
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*2.8;r.R.State(P.FlightState.DockApproach);r.CamerasReady=false;
            r.Tick();Assert.Equal(0,r.Flight.CommandSpeed);
            Assert.Contains(P.L.F(P.L.MinerDockDirectControl,0d),r.R.Miner.Diagnostics);
            for(int n=0;n<45;n++)r.Tick();
            Assert.InRange(r.Flight.CommandSpeed,1.05,1.1);
            Assert.Contains(P.L.F(P.L.MinerDockDirectControl,r.Flight.CommandSpeed),r.R.Miner.Diagnostics);
        }

        [Theory]
        [InlineData(.05)]
        [InlineData(1d/60)]
        public void FinalApproachAtTwoPointEightMetresToleratesSmallAngularNoiseAfterSynchronizing(double dt)
        {
            var r=new DockRig();r.Mass=10999.4;r.R.Position=r.Contact+r.Dock.Forward*2.8;r.R.State(P.FlightState.DockApproach);
            foreach(var thrust in r.R.Hardware.Thrusters)((Stub<IMyThrust>)RemotingServices.GetRealProxy(thrust)).Set("MaxEffectiveThrust",28800f);
            double began=r.R.Program.Now;
            while(r.R.Program.Now-began<40 && !r.Connected && r.R.StateNow==P.FlightState.DockApproach)
            {
                double elapsed=r.R.Program.Now-began;
                // The first window is stable; brief 0.11-degree/s readings then recur
                // more often than two seconds. Pose and lateral motion remain aligned.
                double rate=elapsed>2.2 && (elapsed-2.2)%1.8<.1?.11:.05;
                r.AngularVelocity=r.Dock.Forward*(rate*Math.PI/180);r.Tick(dt,true);
                if(elapsed>4 && r.Gap>.3)Assert.DoesNotContain(P.L.MinerDockSyncPose,r.Reason);
            }
            Assert.True(r.Connected,r.Gap+" / "+r.Reason);Assert.Equal(1,r.ConnectCalls);
        }

        [Theory]
        [InlineData("lateral")]
        [InlineData("angle")]
        [InlineData("angular")]
        [InlineData("speed")]
        public void FinalApproachHysteresisStillStopsForSubstantialMisalignment(string fault)
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*2.8;r.R.State(P.FlightState.DockApproach);
            for(int n=0;n<45;n++)r.Tick();
            Assert.DoesNotContain(P.L.MinerDockSyncPose,r.Reason);
            if(fault=="lateral")r.R.Position+=Vector3D.Right*.3;
            if(fault=="angle")r.Orientation=MatrixD.CreateFromAxisAngle(Vector3D.Forward,3*Math.PI/180);
            if(fault=="angular")r.AngularVelocity=Vector3D.Forward*(.3*Math.PI/180);
            if(fault=="speed")r.Velocity=Vector3D.Right*.3;
            r.Tick();Assert.Contains(P.L.MinerDockSyncPose,r.Reason);Assert.Equal(0,r.ConnectCalls);
            Assert.InRange(Vector3D.Dot(Read<Vector3D>(r.Flight,"routeTarget")-r.R.Position,r.Dock.Forward),-.001,.001);
        }

        [Fact]
        public void MotherSurfaceBehindTheMatingPlaneDoesNotBlockContinuousDocking()
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*14;r.R.State(P.FlightState.DockApproach);
            r.Hit=end=>{
                Vector3D ray=end-r.R.Position;
                double fraction=Vector3D.Dot(r.Dock.Position-r.R.Position,r.Dock.Forward)/Vector3D.Dot(ray,r.Dock.Forward);
                if(fraction<0 || fraction>1)return default(MyDetectedEntityInfo);
                Vector3D point=r.R.Position+ray*fraction;
                return new MyDetectedEntityInfo(50,"Mother hull",MyDetectedEntityType.LargeGrid,point,MatrixD.Identity,Vector3.Zero,
                    VRage.Game.MyRelationsBetweenPlayerAndBlock.NoOwnership,new BoundingBoxD(point-new Vector3D(.1),point+new Vector3D(.1)),1);
            };
            r.Finish();
        }

        [Fact]
        public void MissingOrChangedPermissionCannotEnterDirectApproach()
        {
            var r=new DockRig();r.R.Position=r.Contact+Vector3D.Forward*30;r.Dock.Token="changed";r.Tick();
            Assert.Equal(P.FlightState.DockRetreat,r.R.StateNow);Assert.Equal(P.L.MinerDockGrantChanged,r.Reason);
        }

        [Theory]
        [InlineData(false)]
        [InlineData(true)]
        public void NearPortTrafficClipsInwardMotionKeepsPoseAndFollowsTheMovingMother(bool onlyInward)
        {
            var r=new DockRig();r.R.Position=r.Dock.Position+r.Dock.Forward*r.R.Hardware.Radius;r.R.State(P.FlightState.DockApproach);
            r.CamerasReady=false;r.Dock.Velocity=Vector3D.Up*.7;r.Dock.AngularVelocity=Vector3D.Up*.001;
            r.Flight.TrafficAvoid=true;r.Flight.TrafficAt=r.R.Program.Now;
            r.Flight.TrafficPoint=r.R.Position+Vector3D.Right*(onlyInward?0:2)-r.Dock.Forward*10;
            Call(r.R.Miner,"YieldAtPort",.05);
            var point=r.Flight.RouteTarget;
            Assert.True(Vector3D.Dot(point-r.R.Position,r.Dock.Predicted(r.R.Program.Now).Forward)>=-.001);
            Assert.InRange(Vector3D.Distance(point,r.R.Position),.1,2.01);
            var pose=Read<MatrixD>(r.Flight,"previousDesired");Assert.True(Vector3D.Dot(pose.Forward,r.Orientation.Forward)>.9999);
            Assert.True(Vector3D.Dot(pose.Up,r.Orientation.Up)>.9999);Assert.Equal(0,r.Flight.CommandSpeed);
            Vector3D acceleration=Vector3D.Zero;
            foreach(var thrust in r.R.Hardware.Thrusters)acceleration+=thrust.WorldMatrix.Backward*thrust.MaxEffectiveThrust*thrust.ThrustOverridePercentage/r.Mass;
            var following=r.Dock.PointVelocity(point);
            Assert.True(Vector3D.Dot(acceleration,following)>0);
            Assert.True(Vector3D.Dot(Vector3D.Normalize(acceleration),Vector3D.Normalize(following))>.999);
        }

        [Fact]
        public void TrafficFarFromTheMotherDoesNotUseAnOldDockPlane()
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*1000;r.R.State(P.FlightState.Transit);
            r.Flight.TrafficAvoid=true;r.Flight.TrafficAt=r.R.Program.Now;r.Flight.TrafficPoint=r.R.Position-r.Dock.Forward*2;
            Call(r.R.Miner,"YieldAtPort",.05);Assert.Equal(r.Flight.TrafficPoint,r.Flight.RouteTarget);
        }

        [Fact]
        public void AShortInwardYieldKeepsItsDirectionWhenThereIsAmpleMotherHullClearance()
        {
            var r=new DockRig();r.R.Position=r.Dock.Position+r.Dock.Forward*27;r.R.State(P.FlightState.DockAlign);
            r.Flight.TrafficAvoid=true;r.Flight.TrafficAt=r.R.Program.Now;r.Flight.TrafficPoint=r.R.Position-r.Dock.Forward*2;
            Call(r.R.Miner,"YieldAtPort",.05);
            Assert.Equal(r.Flight.TrafficPoint,r.Flight.RouteTarget);
            Assert.InRange(Vector3D.Dot(r.Flight.RouteTarget-r.Dock.Position,r.Dock.Forward),24.99,25.01);
        }

        [Theory]
        [InlineData(.2,5)]
        [InlineData(1.6,.625)]
        [InlineData(5,.625)]
        public void TelemetryIncludesActualFeedbackResponseAndKeepsHardwareBrakingMargin(double gain,double response)
        {
            var r=new DockRig();r.R.Program.Config=new P.Settings("[Flight]\nVelocityGain="+gain.ToString(System.Globalization.CultureInfo.InvariantCulture),"miner",r.R.Program.Config.Id);
            r.Flight.ThrustMargin=100;r.Velocity=Vector3D.Right*10;
            var snapshot=(P.Telemetry)r.R.Miner.GetType().GetMethod("Snapshot",Private).Invoke(r.R.Miner,null);
            Assert.Equal(response,snapshot.BrakeResponse);Assert.Equal(100,snapshot.ThrustMargin);
            Assert.Equal(response,P.Telemetry.FromIni(snapshot.ToIni()).BrakeResponse);
        }

        [Fact]
        public void HoldingRefreshesTheRealAvailableBrakingMargin()
        {
            var r=new DockRig();r.Velocity=Vector3D.Right;r.Flight.ThrustMargin=0;r.Flight.Hold(.05);
            Assert.True(r.Flight.ThrustMargin>1);
        }

        [Theory]
        [InlineData(false,.2)]
        [InlineData(true,.2)]
        [InlineData(false,1.6)]
        [InlineData(true,1.6)]
        public void ConfirmedTrafficStopsRealMomentumWithoutTheOrdinarySoftBrakingTail(bool avoid,double gain)
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*100;r.R.State(P.FlightState.DockAlign);
            r.R.Program.Config=new P.Settings("[Flight]\nVelocityGain="+gain.ToString(System.Globalization.CultureInfo.InvariantCulture),"miner",r.R.Program.Config.Id);
            r.CamerasReady=false;r.Velocity=Vector3D.Right*10;var start=r.R.Position;
            for(int n=0;n<12;n++)
            {
                r.R.Program.Now+=.05;r.Flight.TrafficAt=r.R.Program.Now;r.Flight.TrafficAvoid=avoid;r.Flight.TrafficWait=!avoid;
                r.Flight.TrafficPoint=r.R.Position+Vector3D.Up*2;
                Call(r.R.Miner,"YieldAtPort",.05);r.Integrate(.05);
                Assert.InRange(r.Velocity.X,-.1,10);Assert.InRange(r.Flight.CommandSpeed,0,1.5);
            }
            Assert.True(r.Velocity.Length()<.1,r.Velocity.ToString());
            Assert.True(Vector3D.Distance(start,r.R.Position)<.6,Vector3D.Distance(start,r.R.Position).ToString());
        }

        [Fact]
        public void TrafficWaitingNearTheMovingMotherBrakesRelativeMotionAndFollowsItsPointVelocity()
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*2;r.R.State(P.FlightState.DockApproach);
            r.Dock.Velocity=new Vector3D(.7,.2,.3);r.Dock.AngularVelocity=Vector3D.Up*.001;
            r.Velocity=r.Dock.PointVelocity(r.R.Position);var start=r.R.Position;var offset=start-r.Dock.Position;
            for(int n=0;n<120;n++)
            {
                r.R.Program.Now+=.05;r.Dock.Position+=r.Dock.Velocity*.05;r.Dock.Center+=r.Dock.Velocity*.05;r.Dock.ReceivedAt=r.R.Program.Now;
                r.Flight.TrafficWait=true;r.Flight.TrafficAt=r.R.Program.Now;
                Call(r.R.Miner,"YieldAtPort",.05);r.Integrate(.05);
                Assert.Equal(0,r.Flight.CommandSpeed);
                Assert.True((r.Velocity-r.Dock.PointVelocity(r.R.Position)).Length()<.05);
            }
            Assert.True(Vector3D.Distance(start,r.R.Position)>4);
            Assert.True((r.R.Position-r.Dock.Position-offset).Length()<.1);
            Assert.Equal(0,r.ConnectCalls);
        }

        [Theory]
        [InlineData(true)]
        [InlineData(false)]
        public void TrafficSideStepUsesSideCamerasAndTurnsOnlyIfCoverageIsMissing(bool sideCamera)
        {
            var r=new DockRig();r.R.Position=r.Contact+Vector3D.Forward*1000;r.R.State(P.FlightState.Returning);
            if(sideCamera)r.AddCamera(Vector3D.Right);
            var start=r.R.Position;var target=start+Vector3D.Right*2;double turned=0;
            for(int n=0;n<160 && r.R.Position.X<start.X+1;n++)
            {
                r.R.Program.Now+=.05;r.Flight.TrafficAvoid=true;r.Flight.TrafficAt=r.R.Program.Now;
                r.Flight.TrafficPoint=target;Call(r.R.Miner,"YieldAtPort",.05);r.Integrate(.05);
                turned=Math.Max(turned,(r.Orientation.Forward-Vector3D.Forward).Length());
            }
            Assert.True(r.Rays>=18);Assert.True(r.R.Position.X>start.X+1,r.Reason);
            if(sideCamera)Assert.InRange(turned,0,.001);else Assert.True(turned>.1);
        }

        [Fact]
        public void TrafficSideStepScansTheMotherHullWithoutTheDockContactExemption()
        {
            var r=new DockRig();r.R.Position=r.Contact+r.Dock.Forward*2;r.R.State(P.FlightState.DockApproach);r.AddCamera(Vector3D.Right);
            var wall=r.R.Position+Vector3D.Right*1.1;
            r.Flight.SetDockContact(50,wall,100);
            r.Hit=end=>new MyDetectedEntityInfo(50,"Mother side wall",MyDetectedEntityType.LargeGrid,wall,MatrixD.Identity,Vector3.Zero,
                VRage.Game.MyRelationsBetweenPlayerAndBlock.NoOwnership,new BoundingBoxD(wall-new Vector3D(.1),wall+new Vector3D(.1)),1);
            bool sawWall=false;
            for(int n=0;n<80;n++)
            {
                r.R.Program.Now+=.05;r.Flight.TrafficAvoid=true;r.Flight.TrafficAt=r.R.Program.Now;
                r.Flight.TrafficPoint=r.R.Position+Vector3D.Right*2;Call(r.R.Miner,"YieldAtPort",.05);
                sawWall|=r.Flight.Problem.Contains("Mother side wall");
            }
            Assert.True(r.Rays>0);Assert.True(sawWall);Assert.Equal(0,r.Flight.CommandSpeed);
            Assert.Equal(P.L.MinerFindingRoute,r.Flight.Problem);Assert.Equal(0,r.ConnectCalls);
            Assert.InRange(Vector3D.Distance(r.R.Position,r.Flight.RouteTarget),1.9,3);
        }

        [Fact]
        public void StraightReturnBreadcrumbsAreJoinedWithoutStoppingAtEveryPoint()
        {
            var r=new DockRig();r.R.State(P.FlightState.Returning);Write(r.R.Miner,"lane","");
            var path=Read<List<Vector3D>>(r.R.Miner,"breadcrumbs");
            for(int n=0;n<5;n++)path.Add(r.Contact+Vector3D.Forward*(30+n*15));
            Write(r.R.Miner,"returnIndex",4);r.R.Position=r.Contact+Vector3D.Forward*100;
            r.Orientation=MatrixD.CreateWorld(Vector3D.Zero,Vector3D.Backward,Vector3D.Up);r.Velocity=Vector3D.Backward*4;
            bool passed=false;
            for(int n=0;n<1200 && r.R.Position.Z<r.Contact.Z-70;n++)
            {
                r.Tick(.05,true);
                if(r.R.Position.Z>r.Contact.Z-75) {passed=true;Assert.True(r.Velocity.Length()>1,r.Reason);}
            }
            Assert.True(passed);Assert.Equal(0,Read<int>(r.R.Miner,"returnIndex"));
        }

        [Theory]
        [InlineData(false)]
        [InlineData(true)]
        public void ReturnRejoinsANearbyEarlierTrailButStillScansTheShortcut(bool blocked)
        {
            var r=new DockRig();r.R.State(P.FlightState.Returning);Write(r.R.Miner,"lane","");
            r.R.Hardware.Radius=4.8;r.R.Position=r.Contact+Vector3D.Forward*1000;var start=r.R.Position;
            var path=Read<List<Vector3D>>(r.R.Miner,"breadcrumbs");path.Clear();
            path.AddRange(new[]{start+Vector3D.Forward*40,start+Vector3D.Forward*8,
                start+Vector3D.Forward*8+Vector3D.Right*20,start+Vector3D.Right*20,start+Vector3D.Forward*20});
            Write(r.R.Miner,"returnIndex",4);
            if(blocked)r.Hit=end=>SphereHit(r,start+Vector3D.Forward*5,2,end);
            for(int n=0;n<18;n++)r.Tick();
            Assert.Equal(0,Read<int>(r.R.Miner,"returnIndex"));Assert.True(r.Rays>0);
            if(blocked)Assert.Equal(0,r.Flight.CommandSpeed);else Assert.True(r.Flight.CommandSpeed>1);
        }

        [Fact]
        public void ASharpReturnCornerIsNotCutByWaypointBlending()
        {
            var r=new DockRig();r.R.State(P.FlightState.Returning);Write(r.R.Miner,"lane","");
            var corner=r.Contact+Vector3D.Forward*90;
            Read<List<Vector3D>>(r.R.Miner,"breadcrumbs").AddRange(new[]{corner+Vector3D.Right*50,corner});
            Write(r.R.Miner,"returnIndex",1);r.R.Position=corner+Vector3D.Forward*5;r.Velocity=Vector3D.Backward*4;
            r.Tick();Assert.Equal(1,Read<int>(r.R.Miner,"returnIndex"));
            Assert.Equal(corner,Read<Vector3D>(r.Flight,"routeTarget"));
        }

        static DockRig LongDeparture(double cruise=15)
        {
            var r=new DockRig();r.Mass=10999.4;r.R.Hardware.Radius=4.63146;
            r.R.Hardware.BodyMin=new Vector3D(-2.25,-1.75,-2.25);r.R.Hardware.BodyMax=new Vector3D(2.25,1.25,2.25);
            r.R.Hardware.BodyHalfSize=new Vector3D(2.25,1.75,2.25);
            r.R.Program.Config=new P.Settings("[Flight]\nCruiseSpeed="+cruise.ToString(System.Globalization.CultureInfo.InvariantCulture),"miner",r.R.Program.Config.Id);
            r.Dock.ApproachDistance=100;r.Dock.LaneClearance=130;r.R.Position=r.Contact;
            Write(r.R.Miner,"laneKind","depart");r.R.State(P.FlightState.Departing);
            return r;
        }

        [Theory]
        [InlineData(12)]
        [InlineData(15)]
        public void ALongDepartureUsesCruiseSpeedAfterItsScannedAccelerationRun(double cruise)
        {
            var r=LongDeparture(cruise);double began=r.R.Program.Now,peak=0;
            for(int n=0;n<800 && r.R.StateNow==P.FlightState.Departing;n++)
            {r.Tick(.05,true);peak=Math.Max(peak,r.Velocity.Length());}
            Assert.Equal(P.FlightState.Transit,r.R.StateNow);Assert.True(r.Rays>=18);
            Assert.InRange(peak,cruise*.9,cruise+.1);
            Assert.True(r.R.Program.Now-began<35,"The 136m departure retained its old 2m/s cap.");
        }

        [Fact]
        public void FasterDepartureStillNeedsFreshCameraClearance()
        {
            var r=LongDeparture();r.CamerasReady=false;var start=r.R.Position;
            for(int n=0;n<150;n++)r.Tick(.05,true);
            Assert.Equal(P.FlightState.Departing,r.R.StateNow);Assert.Equal(0,r.Rays);
            Assert.InRange(Vector3D.Distance(start,r.R.Position),0,.001);Assert.Equal(0,r.Flight.CommandSpeed);
        }

        [Fact]
        public void FasterDepartureBrakesBeforeAnActualObstacleOnItsExitAxis()
        {
            var r=LongDeparture();var start=r.R.Position;var obstacle=start+r.Dock.Forward*30;
            r.Hit=end=>SphereHit(r,obstacle,2,end);
            for(int n=0;n<500;n++)
            {
                r.Tick(.05,true);
                Assert.True(Vector3D.Dot(r.R.Position-start,r.Dock.Forward)<25.75,"The hull passed the obstacle clearance boundary.");
            }
            Assert.True(r.Rays>0);Assert.True(r.Flight.CommandSpeed<.1);Assert.True(r.Velocity.Length()<.1);
            Assert.Equal(P.FlightState.Departing,r.R.StateNow);
        }

        [Theory]
        [InlineData(1d/60)]
        [InlineData(.05)]
        public void TenMetreReturnSamplesAdvanceAcrossAWeakThrusterBrakingHorizon(double dt)
        {
            var r=LongDeparture();r.R.State(P.FlightState.Returning);Write(r.R.Miner,"lane","");
            foreach(var thrust in r.R.Hardware.Thrusters)((Stub<IMyThrust>)RemotingServices.GetRealProxy(thrust)).Set("MaxEffectiveThrust",28800f);
            var path=Read<List<Vector3D>>(r.R.Miner,"breadcrumbs");path.Clear();
            for(int n=0;n<=60;n++)path.Add(r.Contact+Vector3D.Forward*(300+n*10));
            Write(r.R.Miner,"returnIndex",60);r.R.Position=r.Contact+Vector3D.Forward*950;
            r.Orientation=MatrixD.CreateWorld(Vector3D.Zero,Vector3D.Backward,Vector3D.Up);
            r.Flight.BrakeAcceleration=2.618;double lowest=double.MaxValue,peak=0;var trace=new List<string>();
            for(int n=0;n<60/dt && r.R.Position.Z<r.Contact.Z-650;n++)
            {
                r.Tick(dt,true);peak=Math.Max(peak,r.Velocity.Length());
                if(n%(int)Math.Round(1/dt)==0)trace.Add("t="+n*dt+" v="+r.Velocity.Length()+" cmd="+r.Flight.CommandSpeed+" dist="+Vector3D.Distance(r.Flight.RouteTarget,r.R.Position)+" clear="+r.Flight.ScanClearance+" brake="+r.Flight.BrakeAcceleration+" index="+Read<int>(r.R.Miner,"returnIndex"));
                if(r.R.Position.Z>r.Contact.Z-800)lowest=Math.Min(lowest,r.Velocity.Length());
            }
            Assert.True(r.R.Position.Z>=r.Contact.Z-650);Assert.True(r.Rays>=18);
            // A coarse update cadence can correctly brake for an unfinished envelope scan.
            // Neither cadence should retain the old 10m/s return cap or stop at every sample.
            Assert.True(peak>10.5 && lowest>1,"peak="+peak+" lowest="+lowest+"\n"+string.Join("\n",trace));
            Assert.InRange(peak,0,15.1);Assert.InRange(lowest,0,15.1);
        }

        [Fact]
        public void DepartureKeepsItsConnectedRollUntilClearOfTheMother()
        {
            var r=new DockRig();var saved=MatrixD.CreateFromAxisAngle(Vector3D.Forward,.7);
            r.MountPort(r.Mount,new Vector3D(1,0,2),saved,Vector3D.Zero);r.Orientation=saved;
            r.R.Position=Read<Vector3D>(r.R.Miner,"calibratedPosition");r.R.State(P.FlightState.Departing);Write(r.R.Miner,"laneKind","depart");
            Call(r.R.Miner,"Departure",.05);
            Assert.True(Vector3D.Dot(Read<MatrixD>(r.Flight,"previousDesired").Up,saved.Up)>.99999);
        }
    }
}