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;
            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 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>().Set("EntityId",51L).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:CanConnect?MyShipConnectorStatus.Connectable:MyShipConnectorStatus.Unconnected)
                    .Method("get_OtherConnector",a=>Connected?peer.Value:null)
                    .Method("Connect",a=>{ConnectCalls++;Connected=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 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.Holding?"Waiting":R.StateNow==P.FlightState.Returning?"ReturnHome":R.StateNow==P.FlightState.DockAlign?"DockAlign":R.StateNow==P.FlightState.DockApproach?"DockApproach":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");
            }
        }

        [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(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 OneTenthFinalSpeedIsOnlyUsedNearContactAndThirtyMetresStillCompletesPromptly()
        {
            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<2 && r.Gap>.5) {sawFinal=true;Assert.InRange(r.Velocity.Length(),.05,.12);}
            }
            Assert.True(r.Connected,r.Reason);Assert.True(sawFinal);Assert.True(r.R.Program.Now-start<140);
        }

        [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);
        }

        [Fact]
        public void ObstacleOnMotherStillBlocksTravelToTheEntryPoint()
        {
            var r=new DockRig();r.R.Position=r.Contact+Vector3D.Forward*30+Vector3D.Right*10;
            r.AddCamera(Vector3D.Left);
            r.Hit=p=>new MyDetectedEntityInfo(50,"mother wall",MyDetectedEntityType.LargeGrid,r.R.Position+Vector3D.Normalize(p-r.R.Position)*1.1,
                MatrixD.Identity,Vector3.Zero,VRage.Game.MyRelationsBetweenPlayerAndBlock.NoOwnership,new BoundingBoxD(new Vector3D(-1),new Vector3D(1)),1);
            var start=r.R.Position;
            for(int n=0;n<40 && !r.Flight.Blocked;n++)r.Tick(.05,true);
            Assert.True(r.Flight.Blocked);Assert.InRange(Vector3D.Distance(start,r.R.Position),0,.1);Assert.False(Read<bool>(r.Flight,"hasDetour"));
            Assert.Contains(P.L.MinerObstacleBraking,r.Reason);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.Equal(.2,r.Flight.CommandSpeed,5);
            Assert.Contains(P.L.F(P.L.MinerDockDirectControl,.2),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);
        }

        [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 && Read<int>(r.R.Miner,"returnIndex")>0;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"));
        }

        [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"));
        }

        [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);
        }
    }
}