using System;
using System.Collections.Generic;
using System.Reflection;
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 ReturnNavigationTests
    {
        internal sealed class Rig
        {
            public readonly P Program = TestRig.Program();
            public readonly P.ShipHardware Ship;
            public readonly P.FlightController Flight;
            public readonly Stub<IMyShipController> Controller;
            public MatrixD Orientation = MatrixD.Identity;
            public readonly Stub<IMyGyro> Gyro;
            public readonly Stub<IMyCameraBlock> Camera;
            public readonly List<Stub<IMyThrust>> Thrusters = new List<Stub<IMyThrust>>();
            public int Rays;
            public bool CameraReady = true;
            public double Charge = 100000;
            public Func<MyDetectedEntityInfo> Hit = () => default(MyDetectedEntityInfo);
            public Rig()
            {
                Ship = new P.ShipHardware(Program) { Radius = 2, BodyHalfSize = new Vector3D(1), BodyMin = new Vector3D(-1), BodyMax = new Vector3D(1) };
                Controller = new Stub<IMyShipController>().Set("CubeGrid", Program.Me.CubeGrid).Set("IsFunctional",true)
                    .Method("get_WorldMatrix", a => Orientation).Method("GetPosition", a => Vector3D.Zero)
                    .Method("CalculateShipMass", a => new MyShipMass(1000, 1000, 1000));
                Ship.Controller = Controller.Value;
                Gyro = new Stub<IMyGyro>().Set("CubeGrid", Program.Me.CubeGrid).Set("IsFunctional", true)
                    .Method("get_WorldMatrix", a => Orientation);
                Ship.Gyros.Add(Gyro.Value);
                foreach (Vector3D axis in new[] { Vector3D.Right, Vector3D.Left, Vector3D.Up, Vector3D.Down, Vector3D.Forward, Vector3D.Backward })
                {
                    MatrixD local = MatrixD.CreateWorld(Vector3D.Zero, -axis, Math.Abs(axis.Y) > .9 ? Vector3D.Forward : Vector3D.Up);
                    var thrust = new Stub<IMyThrust>().Set("CubeGrid", Program.Me.CubeGrid).Set("IsFunctional", true).Set("Enabled", true)
                        .Set("MaxEffectiveThrust", 100000f).Method("get_WorldMatrix", a => local * Orientation);
                    Ship.Thrusters.Add(thrust.Value); Thrusters.Add(thrust);
                }
                Camera = MakeCamera(1, () => Charge, () => Hit());
                Ship.Cameras.Add(Camera.Value);
                Flight = new P.FlightController(Program, Ship);
            }
            public Stub<IMyCameraBlock> MakeCamera(long id, Func<double> range, Func<MyDetectedEntityInfo> hit)
            {
                return new Stub<IMyCameraBlock>().Set("EntityId", id).Set("CubeGrid", Program.Me.CubeGrid).Set("IsWorking", true)
                    .Set("IsFunctional",true).Set("Enabled",true).Set("EnableRaycast",true).Set("RaycastDistanceLimit",-1d)
                    .Set("RaycastConeLimit", 45f).Method("get_AvailableScanRange", a => range())
                    .Method("get_WorldMatrix", a => Orientation).Method("GetPosition", a => Vector3D.Zero)
                    .Method("CanScan", a => {
                        var target = (Vector3D)a[0];
                        var local = Vector3D.TransformNormal(target, MatrixD.Transpose(Orientation));
                        // Real world-target semantics include both camera pitch/yaw and charge.
                        return CameraReady && target.Length() <= range() && -local.Z > 0 &&
                            Math.Abs(Math.Atan2(local.X, -local.Z)) <= Math.PI / 4 &&
                            Math.Abs(Math.Atan2(local.Y, Math.Sqrt(local.X * local.X + local.Z * local.Z))) <= Math.PI / 4;
                    }).Method("Raycast", a => { Rays++; Charge -= ((Vector3D)a[0]).Length(); return hit(); });
            }
            public void Step(Vector3D target, bool faceTravel = false, double recharge = 0)
            { Program.Now += .05; Charge += recharge * .05; Flight.Move(target, Vector3D.Zero, Vector3D.Forward, Vector3D.Up, 10, 1, true, .05, faceTravel); }
            public bool HasTranslation { get { foreach (var t in Thrusters) if (t.Value.ThrustOverridePercentage > .0001) return true; return false; } }
            public static MyDetectedEntityInfo Detection(long entity, double distance=10)
            { return new MyDetectedEntityInfo(entity, "fixture", MyDetectedEntityType.LargeGrid, Vector3D.Forward * distance, MatrixD.Identity, Vector3.Zero, MyRelationsBetweenPlayerAndBlock.NoOwnership, new BoundingBoxD(new Vector3D(-1), new Vector3D(1)), 1); }
        }

        [Fact]
        public void OpenSpaceReturnFacesTheTravelDirectionBeforeTranslatingWithAForwardCamera()
        {
            var r = new Rig(); var target = Vector3D.Right * 100;
            r.Step(target, true);
            Assert.True(Math.Abs(r.Gyro.Value.Yaw) > .1); Assert.False(r.HasTranslation); Assert.Equal(0, r.Flight.ScanClearance);
            r.Orientation = MatrixD.CreateWorld(Vector3D.Zero, Vector3D.Right, Vector3D.Up);
            for (int n = 0; n < 12; n++) r.Step(target, true);
            Assert.True(r.Flight.ScanClearance > r.Ship.Radius); Assert.True(r.HasTranslation); Assert.True(r.Rays >= 18);
        }

        [Fact]
        public void ACollinearRouteExtensionKeepsItsFreshClearanceAndMotion()
        {
            var r=new Rig();for(int n=0;n<9;n++)r.Step(Vector3D.Forward*100);
            double clear=r.Flight.ScanClearance;Assert.True(clear>0);
            r.Step(Vector3D.Forward*200);
            Assert.True(r.Flight.ScanClearance>=clear);Assert.True(r.HasTranslation);
        }

        [Fact]
        public void ExtendingAShortLegDoesNotExtendItsEvidenceBeforeTheNextSweep()
        {
            var r=new Rig();for(int n=0;n<9;n++)r.Step(Vector3D.Forward*5);
            double clear=r.Flight.ScanClearance;Assert.InRange(clear,5,7);
            r.Step(Vector3D.Forward*100);Assert.InRange(r.Flight.ScanClearance,.01,clear);
        }

        [Fact]
        public void AChangedRouteDirectionStillInvalidatesPreviousClearance()
        {
            var r=new Rig();for(int n=0;n<9;n++)r.Step(Vector3D.Forward*100);
            Assert.True(r.Flight.ScanClearance>0);r.Step(Vector3D.Right*100);
            Assert.Equal(0,r.Flight.ScanClearance);Assert.False(r.HasTranslation);
        }

        [Fact]
        public void DockingDoesNotOptIntoTravelAttitudeOrTranslateWithoutSideCoverage()
        {
            var r = new Rig();
            for (int n = 0; n < 20; n++) r.Step(Vector3D.Right * 100);
            Assert.Equal(0, r.Gyro.Value.Yaw); Assert.False(r.HasTranslation); Assert.Equal(0, r.Flight.ScanClearance);
        }

        [Fact]
        public void GravityReturnRejectsAnUnsupportedTravelAttitudeAndMaintainsTheCurrentHover()
        {
            var r = new Rig(); Vector3D gravity = Vector3D.Down * 9.81;
            r.Controller.Method("GetNaturalGravity", a => gravity);
            // The current vertical engines can hover. The fore/aft engines cannot support
            // the ship if pointing its nose vertically swaps them into the lift direction.
            foreach (var thrust in r.Thrusters)
                if (Math.Abs(Vector3D.Dot(thrust.Value.WorldMatrix.Backward, Vector3D.Forward)) > .9)
                    thrust.Set("MaxEffectiveThrust", 2000f);
            r.Step(Vector3D.Down * 100, true);
            Assert.True(r.Flight.Blocked); Assert.Equal(P.L.F(P.L.MinerHoverReserve, 30d), r.Flight.Problem);
            Assert.Equal(0, r.Gyro.Value.Pitch); Assert.Equal(0, r.Gyro.Value.Yaw); Assert.Equal(0, r.Gyro.Value.Roll);
            Assert.Equal(0, r.Rays); Assert.Equal(0, r.Flight.ScanClearance);
            Vector3D acceleration = gravity;
            foreach (var thrust in r.Thrusters)
                acceleration += thrust.Value.WorldMatrix.Backward * thrust.Value.MaxEffectiveThrust * thrust.Value.ThrustOverridePercentage / 1000;
            Assert.True(acceleration.Length() < .00001, "The rejected turn must retain gravity support without accelerating along the requested route: " + acceleration);
        }

        [Fact]
        public void AnObstacleDetourIsOnlyFlownAfterItsWholeLegHasBeenScanned()
        {
            var r = new Rig(); r.Hit = () => Rig.Detection(42,.8);
            r.Step(Vector3D.Forward * 100, true);
            var flags = BindingFlags.Instance | BindingFlags.NonPublic;
            Assert.True((bool)typeof(P.FlightController).GetField("hasDetour", flags).GetValue(r.Flight));
            var detour = (Vector3D)typeof(P.FlightController).GetField("detour", flags).GetValue(r.Flight);
            r.Hit = () => default(MyDetectedEntityInfo);
            int before=r.Rays;
            r.Step(Vector3D.Forward * 100, true);
            Assert.False(r.HasTranslation);Assert.True(r.Rays-before<18);
            // The observed near obstacle remains known even if later rays pass through
            // a gap. Follow each newly selected candidate, not the rejected first leg.
            for (int n = 0; n < 150 && !r.HasTranslation; n++)
            {
                var current = (Vector3D)typeof(P.FlightController).GetField("detour", flags).GetValue(r.Flight);
                if(current!=detour){detour=current;before=r.Rays;}
                r.Orientation = P.Data.Frame(Vector3D.Zero,detour,Vector3D.Up);
                r.Step(Vector3D.Forward * 100, true);
                if(r.Rays-before<18)Assert.False(r.HasTranslation);
            }
            Assert.True(r.Rays-before>=18);Assert.True(r.Flight.ScanClearance>=detour.Length());Assert.True(r.HasTranslation);
        }

        [Fact]
        public void AnObstructedCameraDoesNotStarveAnotherCameraWithClearSight()
        {
            var r = new Rig(); int obstructed = 0, clear = 0;
            r.Hit = () => { obstructed++; return Rig.Detection(r.Program.Me.CubeGrid.EntityId); };
            r.Ship.Cameras.Add(r.MakeCamera(2, () => 10000, () => { clear++; return default(MyDetectedEntityInfo); }).Value);
            for (int n = 0; n < 30 && r.Flight.ScanClearance == 0; n++) r.Step(Vector3D.Forward * 100);
            Assert.Equal(18, obstructed); Assert.Equal(18, clear); Assert.True(r.Flight.ScanClearance > 0); Assert.True(r.HasTranslation);
        }

        [Fact]
        public void SelfHitsNeverCertifyClearSpace()
        {
            var r = new Rig(); r.Hit = () => Rig.Detection(r.Program.Me.CubeGrid.EntityId);
            for (int n = 0; n < 100; n++) r.Step(Vector3D.Forward * 100);
            Assert.Equal(0, r.Flight.ScanClearance); Assert.False(r.HasTranslation);
        }

        [Fact]
        public void APreviousClearanceExpiresEvenWhileItsReplacementWaitsForCameraCharge()
        {
            var r = new Rig();
            for (int n = 0; n < 9; n++) r.Step(Vector3D.Forward * 100);
            Assert.True(r.Flight.ScanClearance > 0);
            r.CameraReady = false;
            for (int n = 0; n < 31; n++) r.Step(Vector3D.Forward * 100);
            Assert.Equal(0, r.Flight.ScanClearance); Assert.False(r.HasTranslation);
        }

        [Fact]
        public void LimitedCameraRechargeShortensTheEnvelopeInsteadOfRepeatingTheSameUnfinishableScan()
        {
            var r = new Rig(); r.Charge = 0; bool completed = false;
            for (int n = 0; n < 300; n++)
            {
                r.Step(Vector3D.Forward * 100, false, 100);
                if (r.Flight.ScanClearance > 0) { completed = true; break; }
                Assert.False(r.HasTranslation);
            }
            Assert.True(completed); Assert.True(r.Flight.ScanClearance < 36); Assert.True(r.HasTranslation);
        }
    }
}