AutoMiningScript

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

namespace AutoMiningScript.Tests
{
    public class GyroControlTests
    {
        // Independent PB actuator semantics verified against the installed game's MyGyro setters
        // (Steam build 24675677) and the referenced Sandbox.Game.dll: all three setters negate
        // the input and ignore it while override is off. Terminal sliders have different signs.
        // This ideal rate actuator checks control direction; it does not emulate game physics.
        sealed class GyroApi : RealProxy
        {
            public readonly IMyGyro Value;
            public IMyCubeGrid Grid;
            public MatrixD World;
            public Vector3D PhysicalRate;
            public bool Override;
            public int IgnoredWrites;
            public GyroApi(IMyCubeGrid grid) : base(typeof(IMyGyro))
            { Grid = grid; World = MatrixD.Identity; Value = (IMyGyro)GetTransparentProxy(); }
            public override IMessage Invoke(IMessage message)
            {
                var call = (IMethodCallMessage)message;
                object result = null;
                switch (call.MethodName)
                {
                    case "get_CubeGrid": result = Grid; break;
                    case "get_IsFunctional": case "get_Enabled": case "get_IsWorking": result = true; break;
                    case "get_WorldMatrix": result = World; break;
                    case "get_GyroOverride": result = Override; break;
                    case "set_GyroOverride": Override = (bool)call.Args[0]; break;
                    case "get_Pitch": result = (float)-PhysicalRate.X; break;
                    case "get_Yaw": result = (float)-PhysicalRate.Y; break;
                    case "get_Roll": result = (float)-PhysicalRate.Z; break;
                    case "set_Pitch": case "set_Yaw": case "set_Roll":
                        if (!Override) { IgnoredWrites++; break; }
                        double rate = -(float)call.Args[0];
                        if (call.MethodName == "set_Pitch") PhysicalRate.X = rate;
                        else if (call.MethodName == "set_Yaw") PhysicalRate.Y = rate;
                        else PhysicalRate.Z = rate;
                        break;
                }
                var method = (MethodInfo)call.MethodBase;
                if (result == null && method.ReturnType != typeof(void) && method.ReturnType.IsValueType)
                    result = Activator.CreateInstance(method.ReturnType);
                return new ReturnMessage(result, call.Args, call.ArgCount, call.LogicalCallContext, call);
            }
            public Vector3D WorldRate
            { get { return World.Right * PhysicalRate.X + World.Up * PhysicalRate.Y + World.Backward * PhysicalRate.Z; } }
        }

        sealed class Rig
        {
            public readonly P Program = TestRig.Program();
            public readonly P.ShipHardware Hardware;
            public readonly P.FlightController Flight;
            public readonly GyroApi Gyro;
            readonly FieldInfo basis = typeof(P.FlightController).GetField("basis", BindingFlags.Instance | BindingFlags.NonPublic);
            readonly MethodInfo attitude = typeof(P.FlightController).GetMethod("ApplyAttitude", BindingFlags.Instance | BindingFlags.NonPublic);
            public Rig()
            {
                Hardware = new P.ShipHardware(Program); Flight = new P.FlightController(Program, Hardware);
                Gyro = new GyroApi(Program.Me.CubeGrid); Hardware.Gyros.Add(Gyro.Value);
            }
            public Vector3D Command(MatrixD current, MatrixD desired, MatrixD mounting, Vector3D rate)
            {
                basis.SetValue(Flight, current); Gyro.World = mounting * current;
                Program.Now += 1.0 / 60;
                attitude.Invoke(Flight, new object[] { desired, rate });
                return Gyro.WorldRate;
            }
        }

        static IEnumerable<MatrixD> Mountings()
        {
            var axes = new[] { Vector3D.Right, Vector3D.Left, Vector3D.Up, Vector3D.Down, Vector3D.Forward, Vector3D.Backward };
            foreach (var forward in axes)
                foreach (var up in axes)
                    if (Math.Abs(Vector3D.Dot(forward, up)) < .1)
                        yield return MatrixD.CreateWorld(Vector3D.Zero, forward, up);
        }
        static Vector3D Axis(int axis) { return axis == 0 ? Vector3D.Right : axis == 1 ? Vector3D.Up : Vector3D.Backward; }
        static Vector3D Rotate(Vector3D v, Vector3D axis, double angle)
        {
            // Rodrigues integration is independent of FlightController.RotationError and gyro mappings.
            return v * Math.Cos(angle) + Vector3D.Cross(axis, v) * Math.Sin(angle) + axis * Vector3D.Dot(axis, v) * (1 - Math.Cos(angle));
        }
        static MatrixD Advance(MatrixD orientation, Vector3D rate, double dt)
        {
            double speed = rate.Length();
            if (speed < 1e-12) return orientation;
            Vector3D axis = rate / speed;
            return MatrixD.CreateWorld(Vector3D.Zero, Rotate(orientation.Forward, axis, speed * dt), Rotate(orientation.Up, axis, speed * dt));
        }
        static double PoseDistance(MatrixD a, MatrixD b)
        { return Vector3D.DistanceSquared(a.Forward, b.Forward) + Vector3D.DistanceSquared(a.Up, b.Up); }

        [Theory]
        [InlineData(0, -1)] [InlineData(0, 1)] [InlineData(1, -1)]
        [InlineData(1, 1)] [InlineData(2, -1)] [InlineData(2, 1)]
        public void EveryGyroMountingConvergesTowardRequestedWorldRotation(int axis, int sign)
        {
            int count = 0;
            foreach (MatrixD mounting in Mountings())
            {
                count++;
                var r = new Rig();
                MatrixD current = MatrixD.CreateFromYawPitchRoll(.31, -.47, .23);
                Vector3D requiredAxis = Axis(axis) * sign;
                MatrixD desired = Advance(current, requiredAxis, .32);
                Vector3D rate = r.Command(current, desired, mounting, Vector3D.Zero);
                Assert.True(Vector3D.Dot(rate, requiredAxis) > .5, "Gyro mounting " + count + " rotated away from target");
                double previous = PoseDistance(current, desired);
                for (int step = 0; step < 360; step++)
                {
                    current = Advance(current, rate, 1.0 / 60);
                    double distance = PoseDistance(current, desired);
                    Assert.True(distance <= previous + 1e-10, "Ideal actuator increased pose error at step " + step);
                    previous = distance;
                    rate = r.Command(current, desired, mounting, rate);
                }
                Assert.True(PoseDistance(current, desired) < 1e-7);
                Assert.Equal(0, r.Gyro.IgnoredWrites);
            }
            Assert.Equal(24, count);
        }

        [Theory]
        [InlineData(0)] [InlineData(1)] [InlineData(2)]
        public void ZeroPoseErrorDampsWorldAngularVelocityForEveryGyroMounting(int axis)
        {
            foreach (MatrixD mounting in Mountings())
            {
                var r = new Rig();
                MatrixD current = MatrixD.CreateFromYawPitchRoll(-.4, .5, .6);
                Vector3D velocity = Axis(axis) * .4;
                Vector3D command = r.Command(current, current, mounting, velocity);
                Assert.True(Vector3D.Dot(command, velocity) < 0);
                Assert.True(Vector3D.Distance(command, -velocity * .35) < 1e-7);
            }
        }

        [Fact]
        public void FirstEnableOverwritesDormantCommandsBeforeTheyCanBeUsed()
        {
            var r = new Rig();
            r.Gyro.PhysicalRate = new Vector3D(.2, -.3, .4);
            Assert.False(r.Gyro.Override);
            Vector3D rate = r.Command(MatrixD.Identity, MatrixD.Identity, MatrixD.Identity, Vector3D.Zero);
            Assert.True(r.Gyro.Override); Assert.Equal(Vector3D.Zero, rate); Assert.Equal(0, r.Gyro.IgnoredWrites);
        }

        [Fact]
        public void ReleasingActiveControlZerosRatesBeforeDisablingOverride()
        {
            var r = new Rig();
            r.Command(MatrixD.Identity, Advance(MatrixD.Identity, Vector3D.Up, .2), MatrixD.Identity, Vector3D.Zero);
            Assert.True(r.Gyro.PhysicalRate.Length() > .1);
            r.Flight.Release();
            Assert.False(r.Gyro.Override); Assert.Equal(Vector3D.Zero, r.Gyro.PhysicalRate); Assert.Equal(0, r.Gyro.IgnoredWrites);
        }
    }
}