using System;
using System.Collections.Generic;
using System.Linq;
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 NavigationDetourTests
    {
        internal sealed class World
        {
            public readonly ReturnNavigationTests.Rig R=new ReturnNavigationTests.Rig();
            public readonly List<BoundingBoxD> Obstacles=new List<BoundingBoxD>();
            public readonly int[] Rays=new int[6];
            public readonly bool[] Charged={true,true,true,true,true,true};
            public Vector3D Position,Velocity,Gravity;
            public double Travel;
            public bool Collided,Turned;
            public bool TurnedWithoutCoverageGap;
            public readonly List<string> Turns=new List<string>();
            public Func<Vector3D,bool> SelfShadow;
            public int SelfHits,PartialSelfHits;
            public readonly HashSet<Vector3D> PartiallyShadowedCandidates=new HashSet<Vector3D>();
            public readonly List<Vector3D> Waypoints=new List<Vector3D>();
            readonly Queue<string> events=new Queue<string>();
            readonly Dictionary<int,string> candidates=new Dictionary<int,string>();
            static readonly Vector3D[] Axes={Vector3D.Right,Vector3D.Left,Vector3D.Up,Vector3D.Down,Vector3D.Forward,Vector3D.Backward};
            static readonly BindingFlags Private=BindingFlags.Instance|BindingFlags.NonPublic;
            static bool Covers(MatrixD camera,Vector3D target)
            {
                var q=Vector3D.TransformNormal(target-camera.Translation,MatrixD.Transpose(camera));
                return -q.Z>0 && Math.Abs(Math.Atan2(q.X,-q.Z))<=Math.PI/4 &&
                    Math.Abs(Math.Atan2(q.Y,Math.Sqrt(q.X*q.X+q.Z*q.Z)))<=Math.PI/4;
            }
            T Read<T>(string name) => (T)typeof(P.FlightController).GetField(name,Private).GetValue(R.Flight);
            void Record(string text)
            {events.Enqueue("t="+R.Program.Now.ToString("F2")+" "+text);while(events.Count>100)events.Dequeue();}
            string ScanState => "attempt="+Read<int>("detourAttempts")+" target="+R.Flight.RouteTarget+" index="+Read<int>("scanIndex")+" scanning="+Read<bool>("scanning")+" blocked="+Read<bool>("scanBlocked")+" failed="+Read<bool>("scanFailed")+" choosing="+Read<bool>("choosingRoute")+" clearance="+R.Flight.ScanClearance+" direction="+Read<Vector3D>("scanDirection")+" length="+Read<double>("scanLength")+" limit="+Read<double>("scanTravelLimit");
            public World(bool six=true)
            {
                R.Controller.Method("GetPosition",a=>Position).Method("GetShipVelocities",a=>new MyShipVelocities(Velocity,Vector3D.Zero)).Method("GetNaturalGravity",a=>Gravity);
                R.Ship.Cameras.Clear();
                for(int n=0;n<6;n++)
                {
                    if(!six && n!=4)continue;
                    int index=n;var axis=Axes[n];var local=MatrixD.CreateWorld(axis*1.1,axis,Math.Abs(axis.Y)>.9?Vector3D.Forward:Vector3D.Up);
                    var camera=R.MakeCamera(n+1,()=>Charged[index]?100000:0,()=>default(MyDetectedEntityInfo))
                        .Method("get_WorldMatrix",a=>local*R.Orientation)
                        .Method("GetPosition",a=>Vector3D.Transform(axis*1.1,R.Orientation))
                        .Method("CanScan",a=>
                        {
                            return Charged[index] && Covers(local*R.Orientation,(Vector3D)a[0]);
                        }).Method("Raycast",a=>
                        {
                            Rays[index]++;var origin=Vector3D.Transform(axis*1.1,R.Orientation);var end=(Vector3D)a[0];
                            var length=(end-origin).Length();var direction=(end-origin)/length;double nearest=length+1;BoundingBoxD box=default(BoundingBoxD);
                            if(SelfShadow!=null && SelfShadow(Vector3D.TransformNormal(end-Position,MatrixD.Transpose(R.Orientation))))
                            {
                                SelfHits++;
                                if(Read<bool>("choosingRoute") && Read<int>("scanIndex")>0)
                                {PartialSelfHits++;PartiallyShadowedCandidates.Add(R.Flight.RouteTarget);}
                                Record("SELF camera="+index+" ray="+Read<int>("scanIndex")+" "+ScanState);
                                return new MyDetectedEntityInfo(R.Program.Me.CubeGrid.EntityId,"own lens housing",MyDetectedEntityType.SmallGrid,origin,MatrixD.CreateTranslation(Position),Vector3.Zero,MyRelationsBetweenPlayerAndBlock.Owner,new BoundingBoxD(Position-Vector3D.One,Position+Vector3D.One),1);
                            }
                            foreach(var b in Obstacles){var hit=b.Intersects(new RayD(origin,direction));if(hit.HasValue && hit.Value<=length && hit.Value<nearest){nearest=hit.Value;box=b;}}
                            if(nearest>length)return default(MyDetectedEntityInfo);
                            Record("HIT camera="+index+" ray="+Read<int>("scanIndex")+" origin="+origin+" end="+end+" hit="+(origin+direction*nearest)+" "+ScanState);
                            return new MyDetectedEntityInfo(42,"mother ship",MyDetectedEntityType.LargeGrid,origin+direction*nearest,MatrixD.Identity,Vector3.Zero,MyRelationsBetweenPlayerAndBlock.Owner,box,1);
                        });
                    R.Ship.Cameras.Add(camera.Value);
                }
            }
            public bool Step(Vector3D target,double speed=6,bool allowDetour=true)
            {
                const double dt=.05;R.Program.Now+=dt;
                var before=ScanState;
                var requestedBefore=Read<bool>("hasDetour")?Read<Vector3D>("detour"):target;
                var aim=requestedBefore-Position;
                requestedBefore=Position+P.Data.Unit(aim,R.Orientation.Forward)*Math.Max(aim.Length(),R.Ship.Radius*3+5);
                bool arrived=R.Flight.Move(target,Vector3D.Zero,Vector3D.Forward,Vector3D.Up,speed,.4,true,dt,true,allowDetour);
                var after=ScanState;
                if(before!=after)Record(after+" "+R.Flight.CameraDiagnostics);
                if(Read<bool>("choosingRoute"))candidates[Read<int>("detourAttempts")%26]=after+" "+R.Flight.CameraDiagnostics;
                var desired=(MatrixD)typeof(P.FlightController).GetField("previousDesired",Private).GetValue(R.Flight);
                var turn=P.FlightController.RotationError(R.Orientation,desired).Length();
                if(turn>.001)
                {
                    Turned=true;var required=Read<bool>("scanFailed")?Read<Vector3D>("cameraTarget"):requestedBefore;
                    // Evaluate the old hull pose and geometric field of view only.
                    // Depleted charge or a previously self-occluded camera cannot
                    // justify rotating a direction that already has coverage.
                    var coverage=R.Ship.Cameras.Select(c=>c.EntityId+":"+Covers(c.WorldMatrix,required)).ToArray();
                    int available=R.Ship.Cameras.Count(c=>Covers(c.WorldMatrix,required));TurnedWithoutCoverageGap|=available>0;
                    Turns.Add("t="+R.Program.Now+" angle="+turn+" requested="+required+" geometric coverage="+string.Join(",",coverage)+" "+after);
                }
                Vector3D force=Vector3D.Zero;foreach(var t in R.Ship.Thrusters)force+=t.WorldMatrix.Backward*t.MaxEffectiveThrust*t.ThrustOverridePercentage;
                Velocity+=(force/1000+Gravity)*dt;var next=Position+Velocity*dt;
                foreach(var b in Obstacles)
                {
                    var inflated=new BoundingBoxD(b.Min-Vector3D.One,b.Max+Vector3D.One);
                    if(next.X>inflated.Min.X && next.X<inflated.Max.X && next.Y>inflated.Min.Y && next.Y<inflated.Max.Y && next.Z>inflated.Min.Z && next.Z<inflated.Max.Z)Collided=true;
                }
                Travel+=(next-Position).Length();Position=next;R.Orientation=MatrixD.CreateWorld(Position,desired.Forward,desired.Up);
                if((bool)typeof(P.FlightController).GetField("hasDetour",Private).GetValue(R.Flight) && !(bool)typeof(P.FlightController).GetField("choosingRoute",Private).GetValue(R.Flight))
                {var point=R.Flight.RouteTarget;if(Waypoints.Count==0 || point!=Waypoints[Waypoints.Count-1])Waypoints.Add(point);}
                return arrived;
            }
            public string Diagnostics => Position+" vel="+Velocity+" travel="+Travel+" waypoints="+string.Join(";",Waypoints)+" / "+R.Flight.Problem+" / "+R.Flight.CameraDiagnostics+"\nturns:\n"+string.Join("\n",Turns)+"\ncandidates:\n"+string.Join("\n",candidates.OrderBy(p=>p.Key).Select(p=>p.Key+": "+p.Value))+"\nrecent events:\n"+string.Join("\n",events);
        }

        [Theory]
        [InlineData(0)] [InlineData(1)] [InlineData(2)] [InlineData(3)] [InlineData(4)] [InlineData(5)]
        public void NavigationUsesEachInstalledDirectionWithoutTurningTheHull(int axis)
        {
            var directions=new[]{Vector3D.Right,Vector3D.Left,Vector3D.Up,Vector3D.Down,Vector3D.Forward,Vector3D.Backward};
            var w=new World();for(int n=0;n<30;n++)w.Step(directions[axis]*100);
            Assert.False(w.Turned,w.Diagnostics);Assert.True(w.Rays[axis]>0,w.Diagnostics);Assert.True(w.Travel>1,w.Diagnostics);
        }

        [Fact]
        public void AnInstalledUnchargedSideCameraWaitsInsteadOfTurningToTheFrontCamera()
        {
            var w=new World();w.Charged[0]=false;for(int n=0;n<20;n++)w.Step(Vector3D.Right*100);
            Assert.False(w.Turned,w.Diagnostics);Assert.InRange(w.Travel,0,.001);Assert.Equal(0,w.Rays.Sum());
            w.Charged[0]=true;for(int n=0;n<30;n++)w.Step(Vector3D.Right*100);
            Assert.False(w.Turned,w.Diagnostics);Assert.True(w.Travel>1,w.Diagnostics);
        }

        [Theory]
        [InlineData(true)] [InlineData(false)]
        public void ACloseMotherShipWallIsScannedAndBypassedBeforeResumingExploration(bool sixCameras)
        {
            var w=new World(sixCameras);w.Obstacles.Add(new BoundingBoxD(new Vector3D(-30,-3,-12),new Vector3D(30,3,-3.4)));
            var target=new Vector3D(15,0,-65);bool arrived=false;
            for(int n=0;n<6000 && !arrived;n++){arrived=w.Step(target);Assert.False(w.Collided,w.Diagnostics);}
            Assert.True(arrived,w.Diagnostics);Assert.NotEmpty(w.Waypoints);Assert.InRange(w.Travel,60,150);
            Assert.False(w.TurnedWithoutCoverageGap,w.Diagnostics);
            if(!sixCameras)Assert.True(w.Turned,w.Diagnostics);
            if(sixCameras)Assert.True(w.Rays.Count(n=>n>0)>=3,w.Diagnostics);
            Assert.True(w.Waypoints.Any(p=>Math.Abs(p.Y)>4),w.Diagnostics);
        }

        [Fact]
        public void NoCandidateIsFlownWithoutAClearFullBodyScanAndAnOpeningCanBeFoundLater()
        {
            var w=new World();
            w.Obstacles.Add(new BoundingBoxD(new Vector3D(-20,-20,-4),new Vector3D(20,20,-3)));
            w.Obstacles.Add(new BoundingBoxD(new Vector3D(-20,-20,3),new Vector3D(20,20,4)));
            w.Obstacles.Add(new BoundingBoxD(new Vector3D(-4,-20,-20),new Vector3D(-3,20,20)));
            w.Obstacles.Add(new BoundingBoxD(new Vector3D(3,-20,-20),new Vector3D(4,20,20)));
            w.Obstacles.Add(new BoundingBoxD(new Vector3D(-20,3,-20),new Vector3D(20,4,20)));
            w.Obstacles.Add(new BoundingBoxD(new Vector3D(-20,-4,-20),new Vector3D(20,-3,20)));
            for(int n=0;n<500;n++)w.Step(Vector3D.Forward*50);
            Assert.False(w.Collided,w.Diagnostics);Assert.Empty(w.Waypoints);Assert.True(w.Position.Length()<2,w.Diagnostics);
            w.Obstacles.Clear();bool arrived=false;for(int n=0;n<4000 && !arrived;n++)arrived=w.Step(Vector3D.Forward*50);
            Assert.True(arrived,w.Diagnostics);Assert.False(w.Collided,w.Diagnostics);
        }

        [Fact]
        public void APartiallySelfOccludedCandidateExpiresAndAnotherScannedRouteBypassesTheWall()
        {
            var w=new World();
            w.Obstacles.Add(new BoundingBoxD(new Vector3D(-30,-3,-12),new Vector3D(30,3,-3.4)));
            // A lens housing shadows the upper-right edge of the view, while
            // central and other directional rays still return actual world hits.
            w.SelfShadow=v=>v.Y>2 && v.X>v.Y*1.05;
            var target=new Vector3D(15,0,-65);bool arrived=false;
            for(int n=0;n<6000 && !arrived;n++)
            {
                arrived=w.Step(target);
                Assert.False(w.Collided,w.Diagnostics);
                if(w.PartialSelfHits>0 && w.Waypoints.Count==0)Assert.Equal(0,w.R.Flight.CommandSpeed);
            }
            Assert.True(w.PartialSelfHits>0,w.Diagnostics);
            Assert.NotEmpty(w.PartiallyShadowedCandidates);
            Assert.NotEmpty(w.Waypoints);
            Assert.True(arrived,w.Diagnostics);
            Assert.InRange(w.Travel,60,170);
        }

        [Fact]
        public void AGravityUnsupportedCandidateIsDiscardedBeforeAScannedHorizontalLegMoves()
        {
            var w=new World(false) {Gravity=Vector3D.Down*9.81};
            // The vertical engines can hover, but pitching toward an upper or
            // lower route would ask the weak fore/aft engines to support gravity.
            foreach(var thrust in w.R.Thrusters)
                if(Math.Abs(Vector3D.Dot(thrust.Value.WorldMatrix.Backward,Vector3D.Forward))>.9)
                    thrust.Set("MaxEffectiveThrust",2000f);
            w.Obstacles.Add(new BoundingBoxD(new Vector3D(-30,-3,-12),new Vector3D(30,3,-3.4)));
            var target=new Vector3D(15,0,-65);int rejected=0;
            var flags=BindingFlags.Instance|BindingFlags.NonPublic;
            for(int n=0;n<1500 && (w.Waypoints.Count==0 || w.Travel<6);n++)
            {
                var choosing=(bool)typeof(P.FlightController).GetField("choosingRoute",flags).GetValue(w.R.Flight);
                var attempt=(int)typeof(P.FlightController).GetField("detourAttempts",flags).GetValue(w.R.Flight);
                var aim=w.R.Ship.CameraAttitude(w.R.Flight.RouteTarget,Vector3D.Up);
                var gravityInAim=Vector3D.TransformNormal(w.Gravity,MatrixD.Transpose(aim));
                bool unsupported=choosing && Math.Abs(gravityInAim.Z)>2;
                w.Step(target,3);
                if(unsupported)
                {
                    Assert.True((int)typeof(P.FlightController).GetField("detourAttempts",flags).GetValue(w.R.Flight)>attempt,w.Diagnostics);
                    rejected++;
                }
                Assert.False(w.Collided,w.Diagnostics);
            }
            Assert.True(rejected>0,w.Diagnostics);
            Assert.NotEmpty(w.Waypoints);
            Assert.True(w.Travel>=6,w.Diagnostics);
            Assert.Contains(w.Waypoints,point=>Math.Abs(point.Y)<.1);
        }

        [Fact]
        public void AnUnsupportedEnvelopeCameraTurnRejectsTheCandidateWithoutLosingHover()
        {
            var r=new ReturnNavigationTests.Rig();var gravity=Vector3D.Down*9.81;
            r.Controller.Method("GetNaturalGravity",a=>gravity);
            foreach(var thrust in r.Thrusters)
                if(Math.Abs(Vector3D.Dot(thrust.Value.WorldMatrix.Backward,Vector3D.Forward))>.9)
                    thrust.Set("MaxEffectiveThrust",2000f);
            r.Ship.Cameras.Clear();
            // The travel centre is inside this real 45-degree view. Its lower
            // envelope corner is outside even at the far scan horizon, so the
            // fallback would pitch the hull farther than its weak engines allow.
            double angle=44*Math.PI/180;
            var lenses=new[]{MatrixD.CreateWorld(Vector3D.Zero,new Vector3D(0,Math.Sin(angle),-Math.Cos(angle)),new Vector3D(0,Math.Cos(angle),Math.Sin(angle))),
                MatrixD.CreateWorld(Vector3D.Zero,Vector3D.Right,Vector3D.Up)};
            for(int n=0;n<lenses.Length;n++)
            {
                var local=lenses[n];
                r.Ship.Cameras.Add(r.MakeCamera(n+10,()=>100000,()=>default(MyDetectedEntityInfo))
                    .Method("get_WorldMatrix",a=>local*r.Orientation)
                    .Method("CanScan",a=>
                    {
                        var q=Vector3D.TransformNormal((Vector3D)a[0],MatrixD.Transpose(local*r.Orientation));
                        return -q.Z>0 && Math.Abs(Math.Atan2(q.X,-q.Z))<=Math.PI/4 &&
                            Math.Abs(Math.Atan2(q.Y,Math.Sqrt(q.X*q.X+q.Z*q.Z)))<=Math.PI/4;
                    }).Value);
            }
            var target=Vector3D.Forward*65;var flags=BindingFlags.Instance|BindingFlags.NonPublic;
            r.Flight.Move(target,Vector3D.Zero,Vector3D.Forward,Vector3D.Up,6,.4,false,.05,true);
            // Start the normal candidate generator directly; the regression is
            // the support check after envelope scanning chooses its own attitude.
            typeof(P.FlightController).GetMethod("FindDetour",flags).Invoke(r.Flight,new object[]{Vector3D.Zero,target});
            bool rejectedEnvelopeTurn=false,approved=false;
            for(int n=0;n<500 && !approved;n++)
            {
                int before=(int)typeof(P.FlightController).GetField("detourAttempts",flags).GetValue(r.Flight);
                var centre=r.Ship.CameraAttitude(r.Flight.RouteTarget,Vector3D.Up);
                bool centreSupported=r.Flight.CanSupport(centre.Forward,centre.Up,.1);
                r.Program.Now+=.05;
                r.Flight.Move(target,Vector3D.Zero,Vector3D.Forward,Vector3D.Up,6,.4,true,.05,true);
                var desired=(MatrixD)typeof(P.FlightController).GetField("previousDesired",flags).GetValue(r.Flight);
                var localGravity=Vector3D.TransformNormal(gravity,MatrixD.Transpose(desired));
                Assert.True(Math.Abs(localGravity.Z)<=1.40001,"An envelope-camera turn must retain the 30% hover reserve: "+localGravity+" / "+r.Flight.Problem);
                int after=(int)typeof(P.FlightController).GetField("detourAttempts",flags).GetValue(r.Flight);
                if(centreSupported && after>before && r.Flight.Problem==P.L.F(P.L.MinerHoverReserve,30d))rejectedEnvelopeTurn=true;
                r.Orientation=desired.GetOrientation();
                approved=!(bool)typeof(P.FlightController).GetField("choosingRoute",flags).GetValue(r.Flight);
            }
            Assert.True(rejectedEnvelopeTurn,"The travel centre must be supportable when the envelope-specific turn is rejected.");
            Assert.True(approved,r.Flight.Problem+" / "+r.Flight.CameraDiagnostics);
            Assert.True(r.Rays>=18);Assert.True(r.Flight.ScanClearance>0);
        }

        [Fact]
        public void AStationaryObservedSurfaceExpiresAfterItMovesAwayAndTheRouteIsRescanned()
        {
            var w=new World();var target=Vector3D.Forward*12;
            w.Obstacles.Add(new BoundingBoxD(new Vector3D(-4,-4,-1.9),new Vector3D(4,4,-1.8)));
            for(int n=0;n<5;n++)w.Step(target,2,false);
            Assert.True(w.Rays.Sum()>0);Assert.Equal(0,w.R.Flight.CommandSpeed);
            w.Obstacles.Clear();
            // An empty replacement sweep alone must not immediately forget the
            // thin surface. A dynamic grid initially observed at rest can move
            // away later, so the old world-space point cannot block forever.
            for(int n=0;n<300;n++)w.Step(target,2,false);
            Assert.Equal(0,w.R.Flight.CommandSpeed);Assert.InRange(w.Travel,0,.001);
            bool arrived=false;
            for(int n=0;n<1000 && !arrived;n++){arrived=w.Step(target,2,false);Assert.False(w.Collided,w.Diagnostics);}
            Assert.True(arrived,w.Diagnostics);Assert.Empty(w.Waypoints);
        }

        [Theory]
        [InlineData(true)]
        [InlineData(false)]
        public void AnObservedPointInTheContactAreaIsExemptOnlyForItsGrantedGrid(bool sameGrid)
        {
            var r=new ReturnNavigationTests.Rig();var contact=Vector3D.Forward*1.8;
            r.Camera.Method("Raycast",a=>
            {
                r.Rays++;var end=(Vector3D)a[0];
                return new MyDetectedEntityInfo(42,"connector surface",MyDetectedEntityType.LargeGrid,end*(1.8/-end.Z),MatrixD.Identity,
                    Vector3.Zero,MyRelationsBetweenPlayerAndBlock.Owner,new BoundingBoxD(new Vector3D(-4,-4,-1.9),new Vector3D(4,4,-1.8)),1);
            });
            Action step=()=>{r.Program.Now+=.05;r.Flight.Move(Vector3D.Forward*12,Vector3D.Zero,Vector3D.Forward,Vector3D.Up,2,.1,true,.05,false,false);};
            step();Assert.Equal(0,r.Flight.CommandSpeed);Assert.True(r.Rays>0);
            r.Flight.ResetRoute();r.Flight.SetDockContact(sameGrid?42:99,contact,1.5);
            for(int n=0;n<20;n++)step();
            if(sameGrid){Assert.True(r.Flight.CommandSpeed>0,r.Flight.Problem);Assert.True(r.Flight.ScanClearance>10);}
            else Assert.Equal(0,r.Flight.CommandSpeed);
        }
    }
}