using System;
using System.Collections.Generic;
using System.Linq;
using System.Reflection;
using System.Runtime.Remoting;
using Sandbox.ModAPI.Ingame;
using VRage.Game;
using VRage.Game.ModAPI.Ingame;
using VRageMath;
using Xunit;
using P=AutoMiningScript.Program;
namespace AutoMiningScript.Tests
{
public class PortQueueProgressTests
{
static readonly BindingFlags Private=BindingFlags.Instance|BindingFlags.NonPublic;
static T Read<T>(object instance,string field) {return (T)instance.GetType().GetField(field,Private).GetValue(instance);}
static void Write(object instance,string field,object value) {instance.GetType().GetField(field,Private).SetValue(instance,value);}
static Stub<T> Proxy<T>(T instance) where T:class {return (Stub<T>)RemotingServices.GetRealProxy(instance);}
sealed class SentMessage
{
public double At;
public P.Packet Packet;
}
// In-memory IGC still exercises Wire encoding, receiving, acknowledgments,
// message budgets, and the actual controller Receive methods.
sealed class Network
{
public readonly List<Node> Nodes=new List<Node>();
public readonly List<SentMessage> Sent=new List<SentMessage>();
public sealed class Node
{
public P Program;
public long Address;
public Action<P.Packet> Receive;
public readonly Queue<MyIGCMessage> Incoming=new Queue<MyIGCMessage>();
}
public void Add(P program,long address,Action<P.Packet> receive)
{
var node=new Node {Program=program,Address=address,Receive=receive};Nodes.Add(node);
Proxy(program.Me).Set("EntityId",address);
var broadcast=new Stub<IMyBroadcastListener>();
var unicast=new Stub<IMyUnicastListener>().Method("get_HasPendingMessage",a=>node.Incoming.Count>0).Method("AcceptMessage",a=>node.Incoming.Dequeue());
var igc=new Stub<IMyIntergridCommunicationSystem>().Set("UnicastListener",unicast.Value).Method("RegisterBroadcastListener",a=>broadcast.Value)
.Method("SendBroadcastMessage",a=>{Send(node,0,(string)a[0],(string)a[1]);return null;})
.Method("SendUnicastMessage",a=>{Send(node,(long)a[0],(string)a[1],(string)a[2]);return true;});
TestRig.SetBase(program,"IGC",igc.Value);program.Bus=new P.Wire(program);
}
void Send(Node from,long target,string tag,string body)
{
P.Packet decoded;
if(P.Packet.TryDecode(body,from.Program.Config.FleetId,out decoded))Sent.Add(new SentMessage {At=from.Program.Now,Packet=decoded});
foreach(var node in Nodes)if(node!=from && (target==0 || node.Address==target))node.Incoming.Enqueue(new MyIGCMessage(body,tag,from.Address));
}
public void Drain()
{
// One bounded receive pass per simulated programmable-block run.
foreach(var node in Nodes)node.Program.Bus.Drain(node.Receive);
}
}
sealed class MovingMiner
{
readonly PortQueueRig owner;
readonly MatrixD mount=MatrixD.CreateWorld(Vector3D.Zero,Vector3D.Backward,Vector3D.Up);
public readonly MinerStateTests.MinerRig Rig=new MinerStateTests.MinerRig();
public readonly P.FlightController Flight;
public readonly Vector3D InitialPosition;
public MatrixD Orientation=MatrixD.Identity;
public Vector3D Velocity;
public int ConnectedPort=-1,Rays,ConnectCalls;
public double Travelled;
public string Id {get{return Rig.Program.Config.Id;}}
public bool Connected {get{return ConnectedPort>=0;}}
public Vector3D Position {get{return Rig.Position;}}
public Vector3D PortPosition {get{return Position+Vector3D.TransformNormal(Rig.Hardware.ConnectorLocal,Orientation);}}
public MatrixD PortPose {get{return mount*Orientation;}}
public bool Yielding {get{return Rig.Program.Now-Flight.TrafficAt<3 && Flight.TrafficAvoid;}}
public MovingMiner(PortQueueRig owner,int index,Vector3D position)
{
this.owner=owner;InitialPosition=position;
Rig.PrepareDeparture("");
Rig.Program.Config=new P.Settings("[System]\nId=miner-"+index+"\nBaseId=base\nFleetId=mining\n[Flight]\nDepartureMass=1000\n","miner","miner-"+index);
Proxy(Rig.Program.Me.CubeGrid).Set("EntityId",1000L+index);
Rig.Connector.Set("EntityId",2000L+index);
Rig.Hardware.Radius=4;Rig.Hardware.BodyHalfSize=new Vector3D(2.3);Rig.Hardware.BodyMin=new Vector3D(-2.3);Rig.Hardware.BodyMax=new Vector3D(2.3);
Rig.Hardware.ConnectorLocal=new Vector3D(0,0,2);Rig.Hardware.ConnectorForwardLocal=mount.Forward;Rig.Hardware.ConnectorUpLocal=mount.Up;
Write(Rig.Miner,"calibratedPosition",Vector3D.Forward*3);
Write(Rig.Miner,"lane","");Write(Rig.Miner,"laneKind","");Write(Rig.Miner,"dock",null);Write(Rig.Miner,"home",null);
Write(Rig.Miner,"job",null);Write(Rig.Miner,"running",false);Write(Rig.Miner,"nextRequest",0d);Write(Rig.Miner,"nextTelemetry",0d);
Rig.State(P.FlightState.Holding);Rig.Position=position;
Flight=Read<P.FlightController>(Rig.Miner,"flight");
Rig.Controller.Method("get_WorldMatrix",a=>MatrixD.CreateWorld(Position,Orientation.Forward,Orientation.Up))
.Method("GetShipVelocities",a=>new MyShipVelocities(Velocity,Vector3D.Zero));
foreach(var thrust in Rig.Hardware.Thrusters)
{
var local=thrust.WorldMatrix;var stub=Proxy(thrust);stub.Values.Remove("WorldMatrix");stub.Method("get_WorldMatrix",a=>local*Orientation);
}
var gyro=Proxy(Rig.Hardware.Gyros[0]);gyro.Values.Remove("WorldMatrix");gyro.Method("get_WorldMatrix",a=>Orientation);
Rig.Connector.Values.Remove("Status");Rig.Connector.Values.Remove("OtherConnector");
Rig.Connector.Method("GetPosition",a=>PortPosition).Method("get_WorldMatrix",a=>PortPose)
.Method("get_Status",a=>Connected?MyShipConnectorStatus.Connected:ConnectablePort()>=0?MyShipConnectorStatus.Connectable:MyShipConnectorStatus.Unconnected)
.Method("get_OtherConnector",a=>Connected?owner.Ports[ConnectedPort].Value:null)
.Method("Connect",a=>{ConnectCalls++;int port=ConnectablePort();if(port>=0){ConnectedPort=port;Velocity=Vector3D.Zero;}return null;});
Rig.Hardware.Cameras.Clear();AddCamera(Vector3D.Forward);AddCamera(Vector3D.Backward);
}
int ConnectablePort()
{
for(int n=0;n<owner.Ports.Count;n++)
{
var port=owner.Ports[n].Value;
if(owner.Ships.Any(s=>s!=this&&s.ConnectedPort==n))continue;
var target=port.GetPosition()+port.WorldMatrix.Forward;
if(Vector3D.Distance(PortPosition,target)<.15 && Vector3D.Dot(PortPose.Forward,-port.WorldMatrix.Forward)>.9998 && Vector3D.Dot(PortPose.Up,port.WorldMatrix.Up)>.9998)return n;
}
return -1;
}
void AddCamera(Vector3D forward)
{
var cameraMount=MatrixD.CreateWorld(Vector3D.Zero,forward,Vector3D.Up);
Func<MatrixD> pose=()=>cameraMount*Orientation;
Rig.Hardware.Cameras.Add(new Stub<IMyCameraBlock>().Set("CubeGrid",Rig.Program.Me.CubeGrid).Set("EntityId",3000L+Rig.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=>Position).Method("get_WorldMatrix",a=>pose())
.Method("CanScan",a=>
{
var ray=Vector3D.TransformNormal((Vector3D)a[0]-Position,MatrixD.Transpose(pose()));
return -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 Raycast((Vector3D)a[0]);}).Value);
}
MyDetectedEntityInfo Raycast(Vector3D target)
{
var direction=target-Position;double length=direction.Length();if(length<1e-6)return default(MyDetectedEntityInfo);direction/=length;
MovingMiner hit=null;double nearest=length;
foreach(var other in owner.Ships)
{
if(other==this)continue;
var delta=Position-other.Position;double along=Vector3D.Dot(delta,direction),disc=along*along-delta.LengthSquared()+other.Rig.Hardware.Radius*other.Rig.Hardware.Radius;
if(disc<0)continue;double distance=-along-Math.Sqrt(disc);
if(distance>=0 && distance<nearest){nearest=distance;hit=other;}
}
if(hit==null)return default(MyDetectedEntityInfo);
return new MyDetectedEntityInfo(hit.Rig.Program.Me.CubeGrid.EntityId,hit.Id,MyDetectedEntityType.SmallGrid,Position+direction*nearest,
MatrixD.CreateTranslation(hit.Position),(Vector3)hit.Velocity,MyRelationsBetweenPlayerAndBlock.NoOwnership,
new BoundingBoxD(hit.Position-new Vector3D(hit.Rig.Hardware.Radius),hit.Position+new Vector3D(hit.Rig.Hardware.Radius)),1);
}
public void Integrate(double dt)
{
if(Connected)return;
Vector3D acceleration=Vector3D.Zero;
foreach(var thrust in Rig.Hardware.Thrusters)acceleration+=thrust.WorldMatrix.Backward*thrust.MaxEffectiveThrust*thrust.ThrustOverridePercentage/1000;
Velocity+=acceleration*dt;var before=Rig.Position;Rig.Position+=Velocity*dt;Travelled+=Vector3D.Distance(before,Rig.Position);
// As in DockingNavigationTests: real thrust integration with ideal
// attitude response, not a full rigid-body or magnetic physics model.
if(Read<bool>(Flight,"orientationKnown"))Orientation=Read<MatrixD>(Flight,"previousDesired").GetOrientation();
}
}
sealed class PortQueueRig
{
public readonly FleetStateTests.Rig Base;
public readonly Network Network=new Network();
public readonly List<Stub<IMyShipConnector>> Ports;
public readonly List<MovingMiner> Ships=new List<MovingMiner>();
public double Now,MinimumSeparation=double.MaxValue;
readonly Queue<string> closeTrace=new Queue<string>();
string minimumTrace="",firstContactTrace="";
public bool SawSingleYield,SawBothYield;
public double ClosestGroupSpread=double.MaxValue;
public bool SawMutualPairYield;
public readonly HashSet<string> EncounterParticipants=new HashSet<string>();
public readonly HashSet<string> FirstEncounterYielders=new HashSet<string>();
public readonly List<Vector3D> FirstEncounterPoints=new List<Vector3D>();
bool firstEncounterEnded;
public PortQueueRig(int count,int seed,bool sharedEncounter=false,bool nearEncounter=false)
{
Base=new FleetStateTests.Rig(count,false);Ports=Read<List<Stub<IMyShipConnector>>>(Base,"portStubs");
Write(Base.Fleet,"trafficRandom",new Random(seed));
double spread=sharedEncounter?40:25;
Base.SetPort(0,new Vector3D(-spread,0,0),sharedEncounter?Vector3D.Right:Vector3D.Forward);
Base.SetPort(1,new Vector3D(spread,0,0),sharedEncounter?Vector3D.Left:Vector3D.Forward);
if(count==3)Base.SetPort(2,sharedEncounter?new Vector3D(0,40,0):new Vector3D(120,0,0),sharedEncounter?Vector3D.Down:Vector3D.Forward);
Network.Add(Base.Program,5000,Base.Fleet.Receive);
for(int n=0;n<count;n++)
{
var start=n==2&&sharedEncounter?new Vector3D(0,40,-100):new Vector3D(n==0?-spread:n==1?spread:120,0,-100);
if(nearEncounter)start=new Vector3D(n==0?-5:5,0,-100);
var ship=new MovingMiner(this,n,start);Ships.Add(ship);
if(nearEncounter)ship.Velocity=(n==0?Vector3D.Right:Vector3D.Left)*2;
Network.Add(ship.Rig.Program,6000+n,ship.Rig.Miner.Receive);
}
for(int n=0;n<count;n++)
{
int port=n;Ports[n].Values.Remove("Status");Ports[n].Values.Remove("OtherConnector");
Ports[n].Method("get_Status",a=>Ships.Any(s=>s.ConnectedPort==port)?MyShipConnectorStatus.Connected:MyShipConnectorStatus.Unconnected)
.Method("get_OtherConnector",a=>Ships.Where(s=>s.ConnectedPort==port).Select(s=>s.Rig.Connector.Value).FirstOrDefault());
}
// Warm only the stationary base's stability measurements. No ship
// positions change before the actual coupled simulation starts.
for(int n=0;n<30;n++){Now+=.1;Base.Program.Now=Now;Base.Fleet.Tick(.1);}
foreach(var ship in Ships)ship.Rig.Program.Now=Now;
Network.Drain();Network.Drain();
}
public void Step(double dt=.05)
{
Now+=dt;Base.Program.Now=Now;foreach(var ship in Ships)ship.Rig.Program.Now=Now;
Network.Drain();
Base.Program.Bus.Tick();Base.Fleet.Tick(dt);
Network.Drain();
foreach(var ship in Ships){ship.Rig.Program.Bus.Tick();ship.Rig.Miner.Tick(dt);}
Network.Drain();
foreach(var ship in Ships)ship.Integrate(dt);
double spread=0,closest=double.MaxValue;bool newMinimum=false;
for(int a=0;a<Ships.Count;a++)for(int b=a+1;b<Ships.Count;b++)
{
double distance=Vector3D.Distance(Ships[a].Position,Ships[b].Position);
if(distance<MinimumSeparation){MinimumSeparation=distance;newMinimum=true;}
closest=Math.Min(closest,distance);spread=Math.Max(spread,distance);
if(Ships[a].Connected || Ships[b].Connected)continue;
bool aYields=Ships[a].Yielding&&Ships[a].Flight.TrafficPeer==Ships[b].Id;
bool bYields=Ships[b].Yielding&&Ships[b].Flight.TrafficPeer==Ships[a].Id;
SawMutualPairYield|=aYields&&bYields;
if(aYields || bYields){EncounterParticipants.Add(Ships[a].Id);EncounterParticipants.Add(Ships[b].Id);}
}
if(closest<18)
{
closeTrace.Enqueue("t="+Now+" gap="+closest+" "+string.Join(" | ",Ships.Select(ShipTrace)));
while(closeTrace.Count>80)closeTrace.Dequeue();
if(newMinimum)minimumTrace=string.Join("\n",closeTrace);
if(closest<8 && firstContactTrace.Length==0)firstContactTrace=string.Join("\n",closeTrace);
}
if(Ships.All(s=>!s.Connected))ClosestGroupSpread=Math.Min(ClosestGroupSpread,spread);
var first=Ships[0];var second=Ships[1];double gap=Vector3D.Distance(first.Position,second.Position);
// The passing ship may brake briefly while the yielding ship
// clears the route. Only an active lateral maneuver is a yield.
bool one=first.Yielding&&first.Flight.TrafficPeer==second.Id,two=second.Yielding&&second.Flight.TrafficPeer==first.Id;
if(!first.Connected && !second.Connected)
{
SawBothYield|=one&&two;
if(one!=two)
{
SawSingleYield=true;
if(!firstEncounterEnded)
{
FirstEncounterYielders.Add(one?first.Id:second.Id);
FirstEncounterPoints.Add(one?first.Flight.TrafficPoint:second.Flight.TrafficPoint);
}
}
}
if(SawSingleYield && gap>45)firstEncounterEnded=true;
}
string ShipTrace(MovingMiner ship)
{
var observed=Base.Fleet.GetTelemetry().FirstOrDefault(t=>t.Id==ship.Id);
return ship.Id+" "+ship.Rig.StateNow+" pos="+ship.Position+" vel="+ship.Velocity+
" traffic age="+(Now-ship.Flight.TrafficAt)+" wait="+ship.Flight.TrafficWait+" avoid="+ship.Flight.TrafficAvoid+" peer="+ship.Flight.TrafficPeer+" point="+ship.Flight.TrafficPoint+
(observed==null?"":" observed age="+(Now-observed.ReceivedAt)+" pos="+observed.Position+" vel="+observed.Velocity+" route="+observed.HasRoute+" target="+observed.RouteTarget+" margin="+observed.ThrustMargin+" response="+observed.BrakeResponse);
}
public string Diagnostics()
{
return "time="+Now+" / minimum separation="+MinimumSeparation+" / closest group spread="+ClosestGroupSpread+
" / encounter participants="+string.Join(",",EncounterParticipants)+"\nfirst contact:\n"+firstContactTrace+"\nminimum:\n"+minimumTrace+"\n"+Base.Fleet.Diagnostics+"\n"+
string.Join("\n",Ships.Select(s=>s.Id+" / "+s.Position+" / velocity="+s.Velocity+" / "+s.Rig.Miner.Diagnostics));
}
}
static List<P.DockFrame> InitialGrants(PortQueueRig rig,int count)
{
for(int step=0;step<40 && rig.Network.Sent.Count(m=>m.Packet.Kind=="DOCK_GRANT")<count;step++)rig.Step();
var sent=rig.Network.Sent.Where(m=>m.Packet.Kind=="DOCK_GRANT").ToList();
Assert.Equal(count,sent.Select(m=>m.Packet.Target).Distinct().Count());
// Wire drains may deliver different miners' first requests on adjacent
// frames; each request should receive its own immediate allocation.
Assert.InRange(sent.Max(m=>m.At)-sent.Min(m=>m.At),0,.2);
var grants=sent.Select(m=>P.DockFrame.FromIni(m.Packet.Body)).ToList();
Assert.Equal(count,grants.Select(g=>g.ConnectorId).Distinct().Count());
Assert.Equal(Enumerable.Range(0,count).Select(n=>30d+n*10),grants.Select(g=>g.ApproachDistance).OrderBy(d=>d));
return grants;
}
static void StableApproachPoses(PortQueueRig rig,IEnumerable<P.DockFrame> grants)
{
var distances=grants.ToDictionary(g=>g.ConnectorId,g=>g.ApproachDistance);
foreach(var message in rig.Network.Sent.Where(m=>m.Packet.Kind=="DOCKPOSE" || m.Packet.Kind=="DOCK_GRANT"))
{
var pose=P.DockFrame.FromIni(message.Packet.Body);
Assert.Equal(distances[pose.ConnectorId],pose.ApproachDistance);
var port=rig.Ports.Single(p=>p.Value.EntityId==pose.ConnectorId).Value;
Assert.True(Vector3D.Distance(port.WorldMatrix.Forward,pose.Forward)<.000001);
}
}
[Theory]
[InlineData(2,0)]
[InlineData(2,1)]
[InlineData(3,0)]
[InlineData(3,1)]
public void SimultaneousDockGrantsUseStableStaggeredApproachesAndAllShipsPhysicallyConnect(int count,int seed)
{
var rig=new PortQueueRig(count,seed);
var grants=InitialGrants(rig,count);
for(int step=0;step<6000 && rig.Ships.Any(s=>!s.Connected);step++)rig.Step();
Assert.False(rig.SawMutualPairYield,rig.Diagnostics());
StableApproachPoses(rig,grants);
Assert.True(rig.MinimumSeparation>=8,rig.Diagnostics());
Assert.True(rig.Ships.All(s=>s.Connected),rig.Diagnostics());
Assert.Equal(count,rig.Ships.Select(s=>s.ConnectedPort).Distinct().Count());
Assert.All(rig.Ships,s=>
{
Assert.Equal(P.FlightState.Servicing,s.Rig.StateNow);
Assert.True(s.Travelled>40);
Assert.True(s.Rays>0);
Assert.Equal(1,s.ConnectCalls);
});
}
[Fact]
public void DifferentlyFacingBerthsWithStaggeredApproachesAllRemainReachable()
{
// Different approach lengths may avoid an encounter altogether. The
// requirement is safe completion along each real connector axis.
var rig=new PortQueueRig(3,0,true);
var grants=InitialGrants(rig,3);
for(int step=0;step<6000 && rig.Ships.Any(s=>!s.Connected);step++)rig.Step();
Assert.False(rig.SawMutualPairYield,rig.Diagnostics());
StableApproachPoses(rig,grants);
Assert.True(rig.MinimumSeparation>=8,rig.Diagnostics());
Assert.True(rig.Ships.All(s=>s.Connected),rig.Diagnostics());
Assert.Equal(3,rig.Ships.Select(s=>s.ConnectedPort).Distinct().Count());
Assert.All(rig.Ships,s=>
{
Assert.Equal(P.FlightState.Servicing,s.Rig.StateNow);
Assert.True(s.Travelled>40);Assert.True(s.Rays>0);Assert.Equal(1,s.ConnectCalls);
});
}
[Theory]
[InlineData(0)]
[InlineData(1)]
public void ARealNearEncounterStillYieldsAndBothMinersCompleteTheirStaggeredApproaches(int seed)
{
// Unlike the normal arrival layouts, these measured initial velocities
// are already closing a safe 10 m gap. Subsequent positions come only
// from thrust integration, including the side step chosen by traffic.
var rig=new PortQueueRig(2,seed,nearEncounter:true);
var grants=InitialGrants(rig,2);
for(int step=0;step<6000 && rig.Ships.Any(s=>!s.Connected);step++)rig.Step();
Assert.True(rig.SawSingleYield,rig.Diagnostics());
Assert.False(rig.SawMutualPairYield,rig.Diagnostics());
Assert.Single(rig.FirstEncounterYielders);
Assert.NotEmpty(rig.FirstEncounterPoints);
Assert.True(rig.MinimumSeparation>=8,rig.Diagnostics());
Assert.True(rig.Ships.All(s=>s.Connected),rig.Diagnostics());
Assert.Equal(2,rig.Ships.Select(s=>s.ConnectedPort).Distinct().Count());
StableApproachPoses(rig,grants);
Assert.All(rig.Ships,s=>{Assert.Equal(P.FlightState.Servicing,s.Rig.StateNow);Assert.True(s.Travelled>40);Assert.Equal(1,s.ConnectCalls);});
}
[Fact]
public void SixCloseBerthsWithTwoDockedMinersGrantAllFourRemainingArrivalsInOneSchedule()
{
// The stalled save had six berths at about 5 m spacing. The large
// conservative ship radii must not reserve neighboring empty berths.
var rig=new FleetStateTests.Rig(6,true);
var arrivals=new List<P.Telemetry>();
for(int n=0;n<6;n++)
{
var port=new Vector3D(n*5,0,0);rig.SetPort(n,port,Vector3D.Forward);
var miner=rig.Miners[n];miner.Position=port;
if(n==1 || n==5){miner.Radius=4.6;continue;}
rig.SetConnected(n,false);miner.State=P.FlightState.Holding;miner.Radius=4.1;
miner.Position=new Vector3D(n*20-50,20,-100);arrivals.Add(miner);
}
rig.RegisterAll();rig.Tick(30);rig.Sent.Clear();
foreach(var miner in arrivals)rig.Receive("DOCK_REQUEST",miner);
for(int n=0;n<12 && !rig.Sent.Any(p=>p.Kind=="DOCK_GRANT");n++)rig.Tick(1);
var grants=rig.Sent.Where(p=>p.Kind=="DOCK_GRANT").ToList();
Assert.Equal(4,grants.Count);
Assert.Equal(arrivals.Select(t=>t.Id).OrderBy(id=>id),grants.Select(p=>p.Target).OrderBy(id=>id));
Assert.Equal(new long[]{100,102,103,104},grants.Select(p=>P.DockFrame.FromIni(p.Body).ConnectorId).OrderBy(id=>id));
Assert.True(rig.Miners[1].Connected);Assert.True(rig.Miners[5].Connected);
}
}
}
using System;
using System.Collections.Generic;
using System.Linq;
using System.Reflection;
using System.Runtime.Remoting;
using Sandbox.ModAPI.Ingame;
using VRage.Game;
using VRage.Game.ModAPI.Ingame;
using VRageMath;
using Xunit;
using P=AutoMiningScript.Program;
namespace AutoMiningScript.Tests
{
public class PortQueueProgressTests
{
static readonly BindingFlags Private=BindingFlags.Instance|BindingFlags.NonPublic;
static T Read<T>(object instance,string field) {return (T)instance.GetType().GetField(field,Private).GetValue(instance);}
static void Write(object instance,string field,object value) {instance.GetType().GetField(field,Private).SetValue(instance,value);}
static Stub<T> Proxy<T>(T instance) where T:class {return (Stub<T>)RemotingServices.GetRealProxy(instance);}
sealed class SentMessage
{
public double At;
public P.Packet Packet;
}
// In-memory IGC still exercises Wire encoding, receiving, acknowledgments,
// message budgets, and the actual controller Receive methods.
sealed class Network
{
public readonly List<Node> Nodes=new List<Node>();
public readonly List<SentMessage> Sent=new List<SentMessage>();
public sealed class Node
{
public P Program;
public long Address;
public Action<P.Packet> Receive;
public readonly Queue<MyIGCMessage> Incoming=new Queue<MyIGCMessage>();
}
public void Add(P program,long address,Action<P.Packet> receive)
{
var node=new Node {Program=program,Address=address,Receive=receive};Nodes.Add(node);
Proxy(program.Me).Set("EntityId",address);
var broadcast=new Stub<IMyBroadcastListener>();
var unicast=new Stub<IMyUnicastListener>().Method("get_HasPendingMessage",a=>node.Incoming.Count>0).Method("AcceptMessage",a=>node.Incoming.Dequeue());
var igc=new Stub<IMyIntergridCommunicationSystem>().Set("UnicastListener",unicast.Value).Method("RegisterBroadcastListener",a=>broadcast.Value)
.Method("SendBroadcastMessage",a=>{Send(node,0,(string)a[0],(string)a[1]);return null;})
.Method("SendUnicastMessage",a=>{Send(node,(long)a[0],(string)a[1],(string)a[2]);return true;});
TestRig.SetBase(program,"IGC",igc.Value);program.Bus=new P.Wire(program);
}
void Send(Node from,long target,string tag,string body)
{
P.Packet decoded;
if(P.Packet.TryDecode(body,from.Program.Config.FleetId,out decoded))Sent.Add(new SentMessage {At=from.Program.Now,Packet=decoded});
foreach(var node in Nodes)if(node!=from && (target==0 || node.Address==target))node.Incoming.Enqueue(new MyIGCMessage(body,tag,from.Address));
}
public void Drain()
{
// One bounded receive pass per simulated programmable-block run.
foreach(var node in Nodes)node.Program.Bus.Drain(node.Receive);
}
}
sealed class MovingMiner
{
readonly PortQueueRig owner;
readonly MatrixD mount=MatrixD.CreateWorld(Vector3D.Zero,Vector3D.Backward,Vector3D.Up);
public readonly MinerStateTests.MinerRig Rig=new MinerStateTests.MinerRig();
public readonly P.FlightController Flight;
public readonly Vector3D InitialPosition;
public MatrixD Orientation=MatrixD.Identity;
public Vector3D Velocity;
public int ConnectedPort=-1,Rays,ConnectCalls;
public double Travelled;
public string Id {get{return Rig.Program.Config.Id;}}
public bool Connected {get{return ConnectedPort>=0;}}
public Vector3D Position {get{return Rig.Position;}}
public Vector3D PortPosition {get{return Position+Vector3D.TransformNormal(Rig.Hardware.ConnectorLocal,Orientation);}}
public MatrixD PortPose {get{return mount*Orientation;}}
public bool Yielding {get{return Rig.Program.Now-Flight.TrafficAt<3 && Flight.TrafficAvoid;}}
public MovingMiner(PortQueueRig owner,int index,Vector3D position)
{
this.owner=owner;InitialPosition=position;
Rig.PrepareDeparture("");
Rig.Program.Config=new P.Settings("[System]\nId=miner-"+index+"\nBaseId=base\nFleetId=mining\n[Flight]\nDepartureMass=1000\n","miner","miner-"+index);
Proxy(Rig.Program.Me.CubeGrid).Set("EntityId",1000L+index);
Rig.Connector.Set("EntityId",2000L+index);
Rig.Hardware.Radius=4;Rig.Hardware.BodyHalfSize=new Vector3D(2.3);Rig.Hardware.BodyMin=new Vector3D(-2.3);Rig.Hardware.BodyMax=new Vector3D(2.3);
Rig.Hardware.ConnectorLocal=new Vector3D(0,0,2);Rig.Hardware.ConnectorForwardLocal=mount.Forward;Rig.Hardware.ConnectorUpLocal=mount.Up;
Write(Rig.Miner,"calibratedPosition",Vector3D.Forward*3);
Write(Rig.Miner,"lane","");Write(Rig.Miner,"laneKind","");Write(Rig.Miner,"dock",null);Write(Rig.Miner,"home",null);
Write(Rig.Miner,"job",null);Write(Rig.Miner,"running",false);Write(Rig.Miner,"nextRequest",0d);Write(Rig.Miner,"nextTelemetry",0d);
Rig.State(P.FlightState.Holding);Rig.Position=position;
Flight=Read<P.FlightController>(Rig.Miner,"flight");
Rig.Controller.Method("get_WorldMatrix",a=>MatrixD.CreateWorld(Position,Orientation.Forward,Orientation.Up))
.Method("GetShipVelocities",a=>new MyShipVelocities(Velocity,Vector3D.Zero));
foreach(var thrust in Rig.Hardware.Thrusters)
{
var local=thrust.WorldMatrix;var stub=Proxy(thrust);stub.Values.Remove("WorldMatrix");stub.Method("get_WorldMatrix",a=>local*Orientation);
}
var gyro=Proxy(Rig.Hardware.Gyros[0]);gyro.Values.Remove("WorldMatrix");gyro.Method("get_WorldMatrix",a=>Orientation);
Rig.Connector.Values.Remove("Status");Rig.Connector.Values.Remove("OtherConnector");
Rig.Connector.Method("GetPosition",a=>PortPosition).Method("get_WorldMatrix",a=>PortPose)
.Method("get_Status",a=>Connected?MyShipConnectorStatus.Connected:ConnectablePort()>=0?MyShipConnectorStatus.Connectable:MyShipConnectorStatus.Unconnected)
.Method("get_OtherConnector",a=>Connected?owner.Ports[ConnectedPort].Value:null)
.Method("Connect",a=>{ConnectCalls++;int port=ConnectablePort();if(port>=0){ConnectedPort=port;Velocity=Vector3D.Zero;}return null;});
Rig.Hardware.Cameras.Clear();AddCamera(Vector3D.Forward);AddCamera(Vector3D.Backward);
}
int ConnectablePort()
{
for(int n=0;n<owner.Ports.Count;n++)
{
var port=owner.Ports[n].Value;
if(owner.Ships.Any(s=>s!=this&&s.ConnectedPort==n))continue;
var target=port.GetPosition()+port.WorldMatrix.Forward;
if(Vector3D.Distance(PortPosition,target)<.15 && Vector3D.Dot(PortPose.Forward,-port.WorldMatrix.Forward)>.9998 && Vector3D.Dot(PortPose.Up,port.WorldMatrix.Up)>.9998)return n;
}
return -1;
}
void AddCamera(Vector3D forward)
{
var cameraMount=MatrixD.CreateWorld(Vector3D.Zero,forward,Vector3D.Up);
Func<MatrixD> pose=()=>cameraMount*Orientation;
Rig.Hardware.Cameras.Add(new Stub<IMyCameraBlock>().Set("CubeGrid",Rig.Program.Me.CubeGrid).Set("EntityId",3000L+Rig.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=>Position).Method("get_WorldMatrix",a=>pose())
.Method("CanScan",a=>
{
var ray=Vector3D.TransformNormal((Vector3D)a[0]-Position,MatrixD.Transpose(pose()));
return -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 Raycast((Vector3D)a[0]);}).Value);
}
MyDetectedEntityInfo Raycast(Vector3D target)
{
var direction=target-Position;double length=direction.Length();if(length<1e-6)return default(MyDetectedEntityInfo);direction/=length;
MovingMiner hit=null;double nearest=length;
foreach(var other in owner.Ships)
{
if(other==this)continue;
var delta=Position-other.Position;double along=Vector3D.Dot(delta,direction),disc=along*along-delta.LengthSquared()+other.Rig.Hardware.Radius*other.Rig.Hardware.Radius;
if(disc<0)continue;double distance=-along-Math.Sqrt(disc);
if(distance>=0 && distance<nearest){nearest=distance;hit=other;}
}
if(hit==null)return default(MyDetectedEntityInfo);
return new MyDetectedEntityInfo(hit.Rig.Program.Me.CubeGrid.EntityId,hit.Id,MyDetectedEntityType.SmallGrid,Position+direction*nearest,
MatrixD.CreateTranslation(hit.Position),(Vector3)hit.Velocity,MyRelationsBetweenPlayerAndBlock.NoOwnership,
new BoundingBoxD(hit.Position-new Vector3D(hit.Rig.Hardware.Radius),hit.Position+new Vector3D(hit.Rig.Hardware.Radius)),1);
}
public void Integrate(double dt)
{
if(Connected)return;
Vector3D acceleration=Vector3D.Zero;
foreach(var thrust in Rig.Hardware.Thrusters)acceleration+=thrust.WorldMatrix.Backward*thrust.MaxEffectiveThrust*thrust.ThrustOverridePercentage/1000;
Velocity+=acceleration*dt;var before=Rig.Position;Rig.Position+=Velocity*dt;Travelled+=Vector3D.Distance(before,Rig.Position);
// As in DockingNavigationTests: real thrust integration with ideal
// attitude response, not a full rigid-body or magnetic physics model.
if(Read<bool>(Flight,"orientationKnown"))Orientation=Read<MatrixD>(Flight,"previousDesired").GetOrientation();
}
}
sealed class PortQueueRig
{
public readonly FleetStateTests.Rig Base;
public readonly Network Network=new Network();
public readonly List<Stub<IMyShipConnector>> Ports;
public readonly List<MovingMiner> Ships=new List<MovingMiner>();
public double Now,MinimumSeparation=double.MaxValue;
readonly Queue<string> closeTrace=new Queue<string>();
string minimumTrace="",firstContactTrace="";
public bool SawSingleYield,SawBothYield;
public double ClosestGroupSpread=double.MaxValue;
public bool SawMutualPairYield;
public readonly HashSet<string> EncounterParticipants=new HashSet<string>();
public readonly HashSet<string> FirstEncounterYielders=new HashSet<string>();
public readonly List<Vector3D> FirstEncounterPoints=new List<Vector3D>();
bool firstEncounterEnded;
public PortQueueRig(int count,int seed,bool sharedEncounter=false,bool nearEncounter=false)
{
Base=new FleetStateTests.Rig(count,false);Ports=Read<List<Stub<IMyShipConnector>>>(Base,"portStubs");
Write(Base.Fleet,"trafficRandom",new Random(seed));
double spread=sharedEncounter?40:25;
Base.SetPort(0,new Vector3D(-spread,0,0),sharedEncounter?Vector3D.Right:Vector3D.Forward);
Base.SetPort(1,new Vector3D(spread,0,0),sharedEncounter?Vector3D.Left:Vector3D.Forward);
if(count==3)Base.SetPort(2,sharedEncounter?new Vector3D(0,40,0):new Vector3D(120,0,0),sharedEncounter?Vector3D.Down:Vector3D.Forward);
Network.Add(Base.Program,5000,Base.Fleet.Receive);
for(int n=0;n<count;n++)
{
var start=n==2&&sharedEncounter?new Vector3D(0,40,-100):new Vector3D(n==0?-spread:n==1?spread:120,0,-100);
if(nearEncounter)start=new Vector3D(n==0?-5:5,0,-100);
var ship=new MovingMiner(this,n,start);Ships.Add(ship);
if(nearEncounter)ship.Velocity=(n==0?Vector3D.Right:Vector3D.Left)*2;
Network.Add(ship.Rig.Program,6000+n,ship.Rig.Miner.Receive);
}
for(int n=0;n<count;n++)
{
int port=n;Ports[n].Values.Remove("Status");Ports[n].Values.Remove("OtherConnector");
Ports[n].Method("get_Status",a=>Ships.Any(s=>s.ConnectedPort==port)?MyShipConnectorStatus.Connected:MyShipConnectorStatus.Unconnected)
.Method("get_OtherConnector",a=>Ships.Where(s=>s.ConnectedPort==port).Select(s=>s.Rig.Connector.Value).FirstOrDefault());
}
// Warm only the stationary base's stability measurements. No ship
// positions change before the actual coupled simulation starts.
for(int n=0;n<30;n++){Now+=.1;Base.Program.Now=Now;Base.Fleet.Tick(.1);}
foreach(var ship in Ships)ship.Rig.Program.Now=Now;
Network.Drain();Network.Drain();
}
public void Step(double dt=.05)
{
Now+=dt;Base.Program.Now=Now;foreach(var ship in Ships)ship.Rig.Program.Now=Now;
Network.Drain();
Base.Program.Bus.Tick();Base.Fleet.Tick(dt);
Network.Drain();
foreach(var ship in Ships){ship.Rig.Program.Bus.Tick();ship.Rig.Miner.Tick(dt);}
Network.Drain();
foreach(var ship in Ships)ship.Integrate(dt);
double spread=0,closest=double.MaxValue;bool newMinimum=false;
for(int a=0;a<Ships.Count;a++)for(int b=a+1;b<Ships.Count;b++)
{
double distance=Vector3D.Distance(Ships[a].Position,Ships[b].Position);
if(distance<MinimumSeparation){MinimumSeparation=distance;newMinimum=true;}
closest=Math.Min(closest,distance);spread=Math.Max(spread,distance);
if(Ships[a].Connected || Ships[b].Connected)continue;
bool aYields=Ships[a].Yielding&&Ships[a].Flight.TrafficPeer==Ships[b].Id;
bool bYields=Ships[b].Yielding&&Ships[b].Flight.TrafficPeer==Ships[a].Id;
SawMutualPairYield|=aYields&&bYields;
if(aYields || bYields){EncounterParticipants.Add(Ships[a].Id);EncounterParticipants.Add(Ships[b].Id);}
}
if(closest<18)
{
closeTrace.Enqueue("t="+Now+" gap="+closest+" "+string.Join(" | ",Ships.Select(ShipTrace)));
while(closeTrace.Count>80)closeTrace.Dequeue();
if(newMinimum)minimumTrace=string.Join("\n",closeTrace);
if(closest<8 && firstContactTrace.Length==0)firstContactTrace=string.Join("\n",closeTrace);
}
if(Ships.All(s=>!s.Connected))ClosestGroupSpread=Math.Min(ClosestGroupSpread,spread);
var first=Ships[0];var second=Ships[1];double gap=Vector3D.Distance(first.Position,second.Position);
// The passing ship may brake briefly while the yielding ship
// clears the route. Only an active lateral maneuver is a yield.
bool one=first.Yielding&&first.Flight.TrafficPeer==second.Id,two=second.Yielding&&second.Flight.TrafficPeer==first.Id;
if(!first.Connected && !second.Connected)
{
SawBothYield|=one&&two;
if(one!=two)
{
SawSingleYield=true;
if(!firstEncounterEnded)
{
FirstEncounterYielders.Add(one?first.Id:second.Id);
FirstEncounterPoints.Add(one?first.Flight.TrafficPoint:second.Flight.TrafficPoint);
}
}
}
if(SawSingleYield && gap>45)firstEncounterEnded=true;
}
string ShipTrace(MovingMiner ship)
{
var observed=Base.Fleet.GetTelemetry().FirstOrDefault(t=>t.Id==ship.Id);
return ship.Id+" "+ship.Rig.StateNow+" pos="+ship.Position+" vel="+ship.Velocity+
" traffic age="+(Now-ship.Flight.TrafficAt)+" wait="+ship.Flight.TrafficWait+" avoid="+ship.Flight.TrafficAvoid+" peer="+ship.Flight.TrafficPeer+" point="+ship.Flight.TrafficPoint+
(observed==null?"":" observed age="+(Now-observed.ReceivedAt)+" pos="+observed.Position+" vel="+observed.Velocity+" route="+observed.HasRoute+" target="+observed.RouteTarget+" margin="+observed.ThrustMargin+" response="+observed.BrakeResponse);
}
public string Diagnostics()
{
return "time="+Now+" / minimum separation="+MinimumSeparation+" / closest group spread="+ClosestGroupSpread+
" / encounter participants="+string.Join(",",EncounterParticipants)+"\nfirst contact:\n"+firstContactTrace+"\nminimum:\n"+minimumTrace+"\n"+Base.Fleet.Diagnostics+"\n"+
string.Join("\n",Ships.Select(s=>s.Id+" / "+s.Position+" / velocity="+s.Velocity+" / "+s.Rig.Miner.Diagnostics));
}
}
static List<P.DockFrame> InitialGrants(PortQueueRig rig,int count)
{
for(int step=0;step<40 && rig.Network.Sent.Count(m=>m.Packet.Kind=="DOCK_GRANT")<count;step++)rig.Step();
var sent=rig.Network.Sent.Where(m=>m.Packet.Kind=="DOCK_GRANT").ToList();
Assert.Equal(count,sent.Select(m=>m.Packet.Target).Distinct().Count());
// Wire drains may deliver different miners' first requests on adjacent
// frames; each request should receive its own immediate allocation.
Assert.InRange(sent.Max(m=>m.At)-sent.Min(m=>m.At),0,.2);
var grants=sent.Select(m=>P.DockFrame.FromIni(m.Packet.Body)).ToList();
Assert.Equal(count,grants.Select(g=>g.ConnectorId).Distinct().Count());
Assert.Equal(Enumerable.Range(0,count).Select(n=>30d+n*10),grants.Select(g=>g.ApproachDistance).OrderBy(d=>d));
return grants;
}
static void StableApproachPoses(PortQueueRig rig,IEnumerable<P.DockFrame> grants)
{
var distances=grants.ToDictionary(g=>g.ConnectorId,g=>g.ApproachDistance);
foreach(var message in rig.Network.Sent.Where(m=>m.Packet.Kind=="DOCKPOSE" || m.Packet.Kind=="DOCK_GRANT"))
{
var pose=P.DockFrame.FromIni(message.Packet.Body);
Assert.Equal(distances[pose.ConnectorId],pose.ApproachDistance);
var port=rig.Ports.Single(p=>p.Value.EntityId==pose.ConnectorId).Value;
Assert.True(Vector3D.Distance(port.WorldMatrix.Forward,pose.Forward)<.000001);
}
}
[Theory]
[InlineData(2,0)]
[InlineData(2,1)]
[InlineData(3,0)]
[InlineData(3,1)]
public void SimultaneousDockGrantsUseStableStaggeredApproachesAndAllShipsPhysicallyConnect(int count,int seed)
{
var rig=new PortQueueRig(count,seed);
var grants=InitialGrants(rig,count);
for(int step=0;step<6000 && rig.Ships.Any(s=>!s.Connected);step++)rig.Step();
Assert.False(rig.SawMutualPairYield,rig.Diagnostics());
StableApproachPoses(rig,grants);
Assert.True(rig.MinimumSeparation>=8,rig.Diagnostics());
Assert.True(rig.Ships.All(s=>s.Connected),rig.Diagnostics());
Assert.Equal(count,rig.Ships.Select(s=>s.ConnectedPort).Distinct().Count());
Assert.All(rig.Ships,s=>
{
Assert.Equal(P.FlightState.Servicing,s.Rig.StateNow);
Assert.True(s.Travelled>40);
Assert.True(s.Rays>0);
Assert.Equal(1,s.ConnectCalls);
});
}
[Fact]
public void DifferentlyFacingBerthsWithStaggeredApproachesAllRemainReachable()
{
// Different approach lengths may avoid an encounter altogether. The
// requirement is safe completion along each real connector axis.
var rig=new PortQueueRig(3,0,true);
var grants=InitialGrants(rig,3);
for(int step=0;step<6000 && rig.Ships.Any(s=>!s.Connected);step++)rig.Step();
Assert.False(rig.SawMutualPairYield,rig.Diagnostics());
StableApproachPoses(rig,grants);
Assert.True(rig.MinimumSeparation>=8,rig.Diagnostics());
Assert.True(rig.Ships.All(s=>s.Connected),rig.Diagnostics());
Assert.Equal(3,rig.Ships.Select(s=>s.ConnectedPort).Distinct().Count());
Assert.All(rig.Ships,s=>
{
Assert.Equal(P.FlightState.Servicing,s.Rig.StateNow);
Assert.True(s.Travelled>40);Assert.True(s.Rays>0);Assert.Equal(1,s.ConnectCalls);
});
}
[Theory]
[InlineData(0)]
[InlineData(1)]
public void ARealNearEncounterStillYieldsAndBothMinersCompleteTheirStaggeredApproaches(int seed)
{
// Unlike the normal arrival layouts, these measured initial velocities
// are already closing a safe 10 m gap. Subsequent positions come only
// from thrust integration, including the side step chosen by traffic.
var rig=new PortQueueRig(2,seed,nearEncounter:true);
var grants=InitialGrants(rig,2);
for(int step=0;step<6000 && rig.Ships.Any(s=>!s.Connected);step++)rig.Step();
Assert.True(rig.SawSingleYield,rig.Diagnostics());
Assert.False(rig.SawMutualPairYield,rig.Diagnostics());
Assert.Single(rig.FirstEncounterYielders);
Assert.NotEmpty(rig.FirstEncounterPoints);
Assert.True(rig.MinimumSeparation>=8,rig.Diagnostics());
Assert.True(rig.Ships.All(s=>s.Connected),rig.Diagnostics());
Assert.Equal(2,rig.Ships.Select(s=>s.ConnectedPort).Distinct().Count());
StableApproachPoses(rig,grants);
Assert.All(rig.Ships,s=>{Assert.Equal(P.FlightState.Servicing,s.Rig.StateNow);Assert.True(s.Travelled>40);Assert.Equal(1,s.ConnectCalls);});
}
[Fact]
public void SixCloseBerthsWithTwoDockedMinersGrantAllFourRemainingArrivalsInOneSchedule()
{
// The stalled save had six berths at about 5 m spacing. The large
// conservative ship radii must not reserve neighboring empty berths.
var rig=new FleetStateTests.Rig(6,true);
var arrivals=new List<P.Telemetry>();
for(int n=0;n<6;n++)
{
var port=new Vector3D(n*5,0,0);rig.SetPort(n,port,Vector3D.Forward);
var miner=rig.Miners[n];miner.Position=port;
if(n==1 || n==5){miner.Radius=4.6;continue;}
rig.SetConnected(n,false);miner.State=P.FlightState.Holding;miner.Radius=4.1;
miner.Position=new Vector3D(n*20-50,20,-100);arrivals.Add(miner);
}
rig.RegisterAll();rig.Tick(30);rig.Sent.Clear();
foreach(var miner in arrivals)rig.Receive("DOCK_REQUEST",miner);
for(int n=0;n<12 && !rig.Sent.Any(p=>p.Kind=="DOCK_GRANT");n++)rig.Tick(1);
var grants=rig.Sent.Where(p=>p.Kind=="DOCK_GRANT").ToList();
Assert.Equal(4,grants.Count);
Assert.Equal(arrivals.Select(t=>t.Id).OrderBy(id=>id),grants.Select(p=>p.Target).OrderBy(id=>id));
Assert.Equal(new long[]{100,102,103,104},grants.Select(p=>P.DockFrame.FromIni(p.Body).ConnectorId).OrderBy(id=>id));
Assert.True(rig.Miners[1].Connected);Assert.True(rig.Miners[5].Connected);
}
}
}