AutoMiningScript

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

namespace AutoMiningScript.Tests
{
    public class WaitingBerthLayoutTests
    {
        const string WaitingOnly = "[Dock]\nStableSeconds=30\n";
        static readonly Vector3D RowOrigin = new Vector3D(-18, 7, 21);

        static MatrixD SetPort(FleetStateTests.Rig rig, int index, Vector3D position, Vector3D forward, double roll)
        {
            forward = Vector3D.Normalize(forward);
            var up = Vector3D.TransformNormal(P.Data.Perpendicular(forward, Vector3D.Up), MatrixD.CreateFromAxisAngle(forward, roll * Math.PI / 180));
            var pose = MatrixD.CreateWorld(position, forward, up);
            rig.SetPort(index, position, forward);
            var ports = (List<Stub<IMyShipConnector>>)typeof(FleetStateTests.Rig).GetField("portStubs", BindingFlags.NonPublic | BindingFlags.Instance).GetValue(rig);
            ports[index].Set("WorldMatrix", pose);
            return pose;
        }
        static void SetHorizontalPorts(FleetStateTests.Rig rig, int count)
        {
            for (int i = 0; i < count; i++) SetPort(rig, i, RowOrigin + Vector3D.Right * (12 * i), Vector3D.Forward, i * 90);
        }
        static void RequestPoses(FleetStateTests.Rig rig, IEnumerable<P.Telemetry> miners)
        {
            var list = miners.ToList(); rig.Sent.Clear();
            var ports = (List<Stub<IMyShipConnector>>)typeof(FleetStateTests.Rig).GetField("portStubs", BindingFlags.NonPublic | BindingFlags.Instance).GetValue(rig);
            for (int i = 0; i < ports.Count; i++)
            {
                var occupant = new Stub<IMyShipConnector>().Set("EntityId", 9000L + i);
                ports[i].Set("Status", MyShipConnectorStatus.Connected).Set("OtherConnector", occupant.Value);
            }
            foreach (var miner in list)
            {
                miner.State = P.FlightState.Holding; miner.Connected = false;
                rig.Receive("HELLO", miner); rig.Receive("DOCK_REQUEST", miner);
            }
            rig.Tick(20);
            // Occupied physical connectors keep these as waiting poses. Base
            // motion and stability no longer delay allocation of a free berth.
            Assert.DoesNotContain(rig.Sent, p => p.Kind == "DOCK_GRANT");
        }
        static P.DockFrame Pose(FleetStateTests.Rig rig, string miner)
        {
            return P.DockFrame.FromIni(rig.Sent.Last(p => p.Kind == "DOCKPOSE" && p.Target == miner).Body);
        }
        static Vector3D WaitingPoint(FleetStateTests.Rig rig, P.DockFrame pose)
        {
            pose.ReceivedAt = rig.Program.Now;
            return Vector3D.Transform(pose.WaitingLocal, pose.Predicted(rig.Program.Now));
        }
        static void Near(Vector3D expected, Vector3D actual)
        { Assert.True(Vector3D.Distance(expected, actual) < .000001, expected + " vs " + actual); }
        static P.Telemetry AddMiner(FleetStateTests.Rig rig, string id)
        {
            var miner = new P.Telemetry { Id = id, State = P.FlightState.Holding, Position = new Vector3D(1000, 1000, -1000), Radius = 3, Battery = 1, Hydrogen = 1 };
            rig.Miners.Add(miner); return miner;
        }

        [Theory]
        [InlineData(0)]
        [InlineData(90)]
        [InlineData(180)]
        public void ConnectorRollDoesNotRotateAHorizontalWaitingRow(double roll)
        {
            var rig = new FleetStateTests.Rig(4, false, WaitingOnly);
            var ports = Enumerable.Range(0, 4).Select(i => SetPort(rig, i, RowOrigin + Vector3D.Right * (12 * i), Vector3D.Forward, roll)).ToArray();
            RequestPoses(rig, rig.Miners);
            var points = new List<Vector3D>();
            for (int i = 0; i < 4; i++)
            {
                var pose = Pose(rig, rig.Miners[i].Id); var point = WaitingPoint(rig, pose); points.Add(point);
                Assert.Equal(100L + i, pose.ConnectorId);
                Near(ports[i].Translation + ports[i].Forward * 80, point);
                Near(ports[i].Up, pose.Up); // Mating attitude still follows the real connector roll.
                Near(Vector3D.Zero, Vector3D.Cross(point - ports[i].Translation, ports[i].Forward));
            }
            for (int i = 1; i < points.Count; i++) Near(Vector3D.Right * 12, points[i] - points[i - 1]);
        }

        [Fact]
        public void MixedConnectorRollsKeepEachWaitingPointInFrontOfItsOwnHorizontalBerth()
        {
            var rig = new FleetStateTests.Rig(4, false, WaitingOnly);
            var rolls = new[] { 0d, 90d, 180d, -45d };
            var ports = Enumerable.Range(0, 4).Select(i => SetPort(rig, i, RowOrigin + Vector3D.Right * (12 * i), Vector3D.Forward, rolls[i])).ToArray();
            RequestPoses(rig, rig.Miners);
            for (int i = 0; i < 4; i++)
            {
                var pose = Pose(rig, rig.Miners[i].Id);
                Assert.Equal(100L + i, pose.ConnectorId);
                Near(RowOrigin + Vector3D.Right * (12 * i) + Vector3D.Forward * 80, WaitingPoint(rig, pose));
                Near(ports[i].Up, pose.Up);
            }
        }

        [Fact]
        public void DifferentlyFacingBerthsPlaceWaitingPointsAlongTheirActualOpeningDirections()
        {
            var rig = new FleetStateTests.Rig(3, false, WaitingOnly);
            var forwards = new[] { Vector3D.Forward, Vector3D.Right, Vector3D.Normalize(new Vector3D(-1, .2, 1)) };
            var ports = Enumerable.Range(0, 3).Select(i => SetPort(rig, i, RowOrigin + Vector3D.Right * (30 * i), forwards[i], i * 90)).ToArray();
            RequestPoses(rig, rig.Miners);
            for (int i = 0; i < 3; i++)
            {
                var pose = Pose(rig, rig.Miners[i].Id);
                Assert.Equal(100L + i, pose.ConnectorId);
                Near(ports[i].Translation + ports[i].Forward * 80, WaitingPoint(rig, pose));
                Near(ports[i].Forward, pose.Forward); Near(ports[i].Up, pose.Up);
            }
        }

        [Theory]
        [InlineData(40, 10)]
        [InlineData(180, 35)]
        [InlineData(500, 100)]
        public void AdditionalWaitingRowsUseTheSameBerthsAndConfiguredForwardSeparation(int distance, int spacing)
        {
            var rig = new FleetStateTests.Rig(2, false, WaitingOnly + "WaitingDistance=" + distance + "\nWaitingSpacing=" + spacing + "\n");
            SetHorizontalPorts(rig, 2);
            for (int i = 2; i < 6; i++) AddMiner(rig, "miner-" + i);
            RequestPoses(rig, rig.Miners);
            var points = new List<Vector3D>();
            for (int i = 0; i < 6; i++)
            {
                var telemetry = rig.Fleet.GetTelemetry().Single(t => t.Id == rig.Miners[i].Id);
                Assert.Equal(i, telemetry.WaitingSlot);
                var pose = Pose(rig, telemetry.Id); var point = WaitingPoint(rig, pose); points.Add(point);
                Assert.Equal(100L + i % 2, pose.ConnectorId);
                var firstRowDistance=Math.Max(distance,pose.ApproachDistance+spacing);
                Near(RowOrigin + Vector3D.Right * (12 * (i % 2)) + Vector3D.Forward * (firstRowDistance + i / 2 * spacing), point);
                if (i >= 2) Near(Vector3D.Forward * spacing, points[i] - points[i - 2]);
            }
        }

        [Fact]
        public void ConnectorApproachDistancesAreUniqueAndStableAcrossRotationEnumerationAndReload()
        {
            var rig=new FleetStateTests.Rig(6,false,WaitingOnly);SetHorizontalPorts(rig,6);
            RequestPoses(rig,rig.Miners);
            var initial=rig.Miners.Select(t=>Pose(rig,t.Id)).ToDictionary(p=>p.ConnectorId,p=>p.ApproachDistance);
            Assert.Equal(new[]{30d,40d,50d,60d,70d,80d},initial.Values.OrderBy(d=>d));

            var rotation=MatrixD.CreateFromAxisAngle(Vector3D.Normalize(new Vector3D(1,2,3)),.7);
            var actual=new Dictionary<long,MatrixD>();
            for(int n=0;n<6;n++)
            {
                var position=Vector3D.TransformNormal(RowOrigin+Vector3D.Right*(12*n),rotation)+new Vector3D(7,-11,17);
                actual[100+n]=SetPort(rig,n,position,Vector3D.TransformNormal(Vector3D.Forward,rotation),n*60+90);
            }
            RequestPoses(rig,rig.Miners);
            foreach(var miner in rig.Miners)
            {
                var pose=Pose(rig,miner.Id);var port=actual[pose.ConnectorId];
                Assert.Equal(initial[pose.ConnectorId],pose.ApproachDistance);
                Near(port.Forward,pose.Forward);Near(port.Up,pose.Up);
                var entry=pose.Position+pose.Forward*pose.ApproachDistance;
                Near(port.Translation+port.Forward*initial[pose.ConnectorId],entry);
                Near(Vector3D.Zero,Vector3D.Cross(entry-port.Translation,port.Forward));
                Assert.True(Vector3D.Dot(WaitingPoint(rig,pose)-port.Translation,port.Forward)>=pose.ApproachDistance+20-.000001);
            }

            // Rescan a genuinely reversed terminal enumeration, so cached offsets
            // must be rebuilt for the same IDs instead of merely staying cached.
            var group=rig.Program.GridTerminalSystem.GetBlockGroupWithName("AMS Docks");
            var source=(Stub<IMyBlockGroup>)RemotingServices.GetRealProxy(group);
            var fill=source.Methods["GetBlocksOfType"];
            source.Method("GetBlocksOfType",args=>{var result=fill(args);((List<IMyShipConnector>)args[0]).Reverse();return result;});
            typeof(P.FleetController).GetMethod("Scan",BindingFlags.NonPublic|BindingFlags.Instance).Invoke(rig.Fleet,new object[0]);
            RequestPoses(rig,rig.Miners);
            foreach(var miner in rig.Miners){var pose=Pose(rig,miner.Id);Assert.Equal(initial[pose.ConnectorId],pose.ApproachDistance);}

            var restored=new FleetStateTests.Rig(6,false,WaitingOnly);SetHorizontalPorts(restored,6);restored.Restore(rig.Checkpoint());
            RequestPoses(restored,restored.Miners);
            foreach(var miner in restored.Miners){var pose=Pose(restored,miner.Id);Assert.Equal(initial[pose.ConnectorId],pose.ApproachDistance);}
        }

        [Fact]
        public void ConfiguredApproachBaseIsRespectedAndOldFramesRemainCompatible()
        {
            var rig=new FleetStateTests.Rig(3,false,WaitingOnly+"ApproachDistance=45\n");SetHorizontalPorts(rig,3);
            RequestPoses(rig,rig.Miners);
            Assert.Equal(new[]{45d,55d,65d},rig.Miners.Select(t=>Pose(rig,t.Id).ApproachDistance).OrderBy(d=>d));
            Assert.Equal(0,P.DockFrame.FromIni(new MyIni()).ApproachDistance);
            var sent=new P.DockFrame {ApproachDistance=95};Assert.Equal(95,P.DockFrame.FromIni(sent.ToIni()).ApproachDistance);
        }

        [Fact]
        public void AnActiveAssignmentKeepsItsApproachDistanceWhenTheConfiguredBaseChanges()
        {
            var rig=new FleetStateTests.Rig(3,false);
            foreach(var miner in rig.Miners){miner.State=P.FlightState.Holding;miner.Position+=Vector3D.Forward*100;}
            rig.RegisterAll();rig.Tick(30);rig.Receive("DOCK_REQUEST",rig.Miners[0]);rig.Tick(12);
            var granted=P.DockFrame.FromIni(Assert.Single(rig.Sent,p=>p.Kind=="DOCK_GRANT").Body);

            rig.Program.Config=new P.Settings("[System]\nId=base\nBaseId=base\nFleetId=mining\n[Dock]\nApproachDistance=70\n","fleet","base");
            rig.Sent.Clear();rig.Receive("TELEMETRY",rig.Miners[0]);rig.Tick(12);
            var updated=Pose(rig,rig.Miners[0].Id);
            Assert.Equal(granted.Token,updated.Token);
            Assert.Equal(granted.ApproachDistance,updated.ApproachDistance);
            Assert.Equal(granted.ApproachDistance,P.Data.ReadNumber(rig.Save(),"fleet","LaneApproachDistance",0));

            rig.Sent.Clear();rig.Receive("DOCK_REQUEST",rig.Miners[1]);rig.Tick(12);
            var next=P.DockFrame.FromIni(Assert.Single(rig.Sent,p=>p.Kind=="DOCK_GRANT").Body);
            Assert.InRange(next.ApproachDistance,70,90);
            Assert.Equal(granted.ApproachDistance,Pose(rig,rig.Miners[0].Id).ApproachDistance);
        }

        [Fact]
        public void RemovingAnOfflineMinerAndReloadingDoNotMoveSurvivingWaitingAssignments()
        {
            var before = new FleetStateTests.Rig(3, false, WaitingOnly); SetHorizontalPorts(before, 3);
            RequestPoses(before, before.Miners);
            var original = before.Miners.Skip(1).ToDictionary(t => t.Id, t => WaitingPoint(before, Pose(before, t.Id)));
            before.Program.Now += 10;
            foreach (var miner in before.Miners.Skip(1)) before.Receive("TELEMETRY", miner);
            before.Fleet.Command("clear-offline");
            Assert.DoesNotContain(before.Fleet.GetTelemetry(), t => t.Id == "miner-0");
            var newcomer = AddMiner(before, "newcomer");
            RequestPoses(before, before.Miners.Skip(1));
            foreach (var pair in original) Near(pair.Value, WaitingPoint(before, Pose(before, pair.Key)));
            Assert.Equal(3, before.Fleet.GetTelemetry().Single(t => t.Id == newcomer.Id).WaitingSlot);
            var newcomerPoint = WaitingPoint(before, Pose(before, newcomer.Id));
            Near(RowOrigin + Vector3D.Forward * 100, newcomerPoint);

            var after = new FleetStateTests.Rig(3, false, WaitingOnly); SetHorizontalPorts(after, 3);
            after.Restore(before.Checkpoint()); var restoredNewcomer = AddMiner(after, newcomer.Id);
            // Client values must not reallocate the server's persisted waiting slot.
            foreach (var miner in after.Miners.Skip(1)) miner.WaitingSlot = 999;
            RequestPoses(after, after.Miners.Skip(1));
            foreach (var pair in original) Near(pair.Value, WaitingPoint(after, Pose(after, pair.Key)));
            Near(newcomerPoint, WaitingPoint(after, Pose(after, restoredNewcomer.Id)));
            Assert.Equal(new[] { 1, 2, 3 }, after.Fleet.GetTelemetry().Select(t => t.WaitingSlot).OrderBy(s => s).ToArray());
            var next = AddMiner(after, "next"); after.Receive("HELLO", next);
            Assert.Equal(4, after.Fleet.GetTelemetry().Single(t => t.Id == next.Id).WaitingSlot);
        }
    }
}