using System;
using System.Linq;
using System.Reflection;
using System.Runtime.Remoting;
using Sandbox.ModAPI.Ingame;
using VRageMath;
using Xunit;
using P=AutoMiningScript.Program;

namespace AutoMiningScript.Tests
{
    public class SurveyNavigationTests
    {
        static readonly BindingFlags Private=BindingFlags.Instance|BindingFlags.NonPublic;
        static T Read<T>(object instance,string name) {return (T)instance.GetType().GetField(name,Private).GetValue(instance);}
        static void Write(object instance,string name,object value) {instance.GetType().GetField(name,Private).SetValue(instance,value);}
        static void Call(object instance,string name,params object[] args) {instance.GetType().GetMethod(name,Private).Invoke(instance,args);}

        sealed class SurveyRig
        {
            public readonly MinerStateTests.MinerRig R=new MinerStateTests.MinerRig();
            public readonly P.FlightController Flight;
            public P.Job Job;
            public MatrixD Orientation=MatrixD.Identity;
            public Vector3D Velocity;
            public bool Ready=true;
            public int NavigationRays,SurveyRays;
            public Func<Vector3D,MyDetectedEntityInfo> Observation=end=>default(MyDetectedEntityInfo);
            public SurveyRig()
            {
                R.Task();Job=Read<P.Job>(R.Miner,"job");Job.Kind=P.JobKind.Survey;Job.Depth=2000;Job.EntityId=0;
                R.State(P.FlightState.Transit);Flight=Read<P.FlightController>(R.Miner,"flight");
                R.Controller.Method("get_WorldMatrix",a=>MatrixD.CreateWorld(R.Position,Orientation.Forward,Orientation.Up))
                    .Method("GetShipVelocities",a=>new MyShipVelocities(Velocity,Vector3D.Zero));
                foreach(var thrust in R.Hardware.Thrusters)
                {
                    var proxy=(Stub<IMyThrust>)RemotingServices.GetRealProxy(thrust);var local=thrust.WorldMatrix;
                    proxy.Values.Remove("WorldMatrix");proxy.Method("get_WorldMatrix",a=>local*Orientation);
                }
                var gyro=(Stub<IMyGyro>)RemotingServices.GetRealProxy(R.Hardware.Gyros[0]);
                gyro.Values.Remove("WorldMatrix");gyro.Method("get_WorldMatrix",a=>Orientation);
                // A single forward camera matches miners 2-4 in the reported save.
                R.Camera.Set("RaycastConeLimit",45f).Set("RaycastDistanceLimit",-1d).Set("EnableRaycast",true)
                    .Method("get_WorldMatrix",a=>MatrixD.CreateWorld(R.Position,Orientation.Forward,Orientation.Up))
                    .Method("CanScan",a=>{
                        Vector3D local=Vector3D.TransformNormal((Vector3D)a[0]-R.Position,MatrixD.Transpose(Orientation));
                        return Ready && -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=>{
                        if(R.StateNow==P.FlightState.Survey){SurveyRays++;return Observation((Vector3D)a[0]);}
                        NavigationRays++;return default(MyDetectedEntityInfo);
                    });
            }
            public void Tick(bool integrate=true)
            {
                const double dt=.05;R.Program.Now+=dt;
                if(R.StateNow!=P.FlightState.Transit && R.StateNow!=P.FlightState.Survey)return;
                Call(R.Miner,R.StateNow==P.FlightState.Transit?"Transit":"Survey",dt);
                if(!integrate)return;
                Vector3D acceleration=Vector3D.Zero;
                foreach(var thrust in R.Hardware.Thrusters)acceleration+=thrust.WorldMatrix.Backward*thrust.MaxEffectiveThrust*thrust.ThrustOverridePercentage/1000;
                Velocity+=acceleration*dt;R.Position+=Velocity*dt;
                // Ideal attitude response; this fixture validates phase/camera geometry,
                // not the game's inertia, gyro torque or physics.
                Orientation=Read<MatrixD>(Flight,"previousDesired").GetOrientation();
            }
            public P.JobReport Finish()
            {
                for(int n=0;n<1200 && !R.Sent.Any(p=>p.Kind=="RESULT");n++)Tick();
                Assert.Equal(P.FlightState.Survey,R.StateNow);Assert.True(Read<bool>(R.Miner,"continuing"));
                Assert.Equal(1,SurveyRays);
                return P.JobReport.FromIni(Assert.Single(R.Sent,p=>p.Kind=="RESULT").Body);
            }
        }

        [Fact]
        public void SurveyTransitKeepsFacingItsTravelUntilTheObservationPointIsReached()
        {
            var r=new SurveyRig();r.Job.Entry=Vector3D.Right*4;r.Orientation=MatrixD.CreateWorld(Vector3D.Zero,Vector3D.Right,Vector3D.Up);
            r.Tick(false);
            Assert.True(Vector3D.Dot(Read<MatrixD>(r.Flight,"previousDesired").Forward,Vector3D.Right)>.999);
            Assert.Equal(P.FlightState.Transit,r.R.StateNow);
        }

        [Theory]
        [InlineData(0d)]
        [InlineData(1d)]
        public void AtmosphericObservationArrivalKeepsGravityUpBeforeTurningDownForTheScan(double lateral)
        {
            var r=new SurveyRig();Vector3D travel=Vector3D.Normalize(new Vector3D(lateral,0,-1));
            r.Job.Entry=travel*4;r.Job.Direction=Vector3D.Down;r.Job.Up=Vector3D.Forward;
            r.Orientation=MatrixD.CreateWorld(Vector3D.Zero,travel,Vector3D.Up);
            r.R.Controller.Method("GetNaturalGravity",a=>Vector3D.Down*9.81);
            r.Tick(false);MatrixD desired=Read<MatrixD>(r.Flight,"previousDesired");
            Assert.True(Vector3D.Dot(desired.Forward,travel)>.999);
            Assert.True(Vector3D.Dot(desired.Up,Vector3D.Up)>.999);
        }

        [Theory]
        [InlineData(-3.336105458482,1.21255582737,-2.10187508931)]
        [InlineData(1.35609391866,-2.42185151840,-2.33387649985)]
        [InlineData(1.83350782323,2.07241880727,-2.22492569889)]
        [InlineData(.601750779737,-1.50573927145,3.02794924745)]
        public void SavedFinalMetresWithOneForwardCameraReachObservationAndPerformTheScan(double x,double y,double z)
        {
            var r=new SurveyRig();r.Job.Entry=new Vector3D(-67227.817161019266,-123791.63501934477,-161805.93741404629);
            r.Job.Direction=new Vector3D(.6281360745318959,.58511134572200962,-.512922786566399);
            r.Job.Up=new Vector3D(-.74590343947643378,.26512158333808944,-.61101440656135975);
            r.R.Position=r.Job.Entry+new Vector3D(x,y,z);
            r.Orientation=MatrixD.CreateWorld(Vector3D.Zero,r.Job.Direction,r.Job.Up);
            Assert.Equal("SurveyEmpty",r.Finish().Outcome);Assert.True(r.NavigationRays>0);
            Assert.InRange(Vector3D.Distance(r.R.Position,r.Job.Entry),0,1.25);
        }

        [Theory]
        [InlineData(.6)]
        [InlineData(.9)]
        public void ObservationDoesNotDemandASecondTighterPositionCorrectionBeforeScanning(double offset)
        {
            var r=new SurveyRig();r.R.Position=Vector3D.Right*offset;r.R.State(P.FlightState.Survey);
            Vector3D observation=r.R.Position;r.Tick();
            Assert.Equal(1,r.SurveyRays);Assert.Equal(P.FlightState.Survey,r.R.StateNow);Assert.True(Read<bool>(r.R.Miner,"continuing"));
            Assert.InRange(Vector3D.Distance(r.R.Position,observation),0,.001);
            Assert.Equal("SurveyEmpty",P.JobReport.FromIni(Assert.Single(r.R.Sent,p=>p.Kind=="RESULT").Body).Outcome);
        }

        [Fact]
        public void LongRangeAsteroidHitBecomesAHigherPriorityProbeAtTheActualHitPoint()
        {
            var r=new SurveyRig();var planner=new P.TaskPlanner(r.R.Program);
            planner.Survey(Vector3D.Zero,Vector3D.Zero,Vector3D.Forward,Vector3D.Up,2000);
            r.Job=planner.Jobs.First();Write(r.R.Miner,"job",r.Job);
            r.R.Position=r.Job.Entry+Vector3D.Right*4;r.Orientation=MatrixD.CreateWorld(Vector3D.Zero,r.Job.Direction,r.Job.Up);
            var surface=r.Job.Entry+r.Job.Direction*100;
            r.Observation=end=>new MyDetectedEntityInfo(42,"Asteroid",MyDetectedEntityType.Asteroid,surface,MatrixD.Identity,Vector3.Zero,
                VRage.Game.MyRelationsBetweenPlayerAndBlock.NoOwnership,new BoundingBoxD(surface-new Vector3D(10),surface+new Vector3D(10)),1);
            P.JobReport report=r.Finish();Assert.Equal("SurveyHit",report.Outcome);planner.Report(r.Job,report);
            P.Job probe=Assert.Single(planner.Jobs,j=>j.Kind==P.JobKind.Probe);
            Assert.Equal(surface,probe.Entry);Assert.Equal(42,probe.EntityId);Assert.Equal(40,probe.Depth);
            Assert.True(probe.Priority>r.Job.Priority);Assert.NotEqual(Vector3D.Zero,probe.Direction);
        }

        [Fact]
        public void MissingNavigationCoverageReportsTheBlockerInsteadOfOnlyTheOriginalTaskMessage()
        {
            var r=new SurveyRig();r.R.Position=Vector3D.Backward*30;r.Ready=false;
            r.Tick(false);Assert.Equal(P.FlightState.Transit,r.R.StateNow);
            Assert.NotEmpty(r.Flight.Problem);Assert.Contains(r.Flight.Problem,Read<string>(r.R.Miner,"reason"));
            Assert.Equal(0,r.SurveyRays);Assert.DoesNotContain(r.R.Sent,p=>p.Kind=="RESULT");
        }

        [Fact]
        public void ObservationWithoutAChargedCameraWaitsWithoutReportingEmptySpace()
        {
            var r=new SurveyRig();r.Ready=false;r.R.State(P.FlightState.Survey);
            for(int n=0;n<30;n++)r.Tick();
            Assert.Equal(P.FlightState.Survey,r.R.StateNow);Assert.Equal(0,r.SurveyRays);
            Assert.DoesNotContain(r.R.Sent,p=>p.Kind=="RESULT");Assert.Equal(P.L.MinerWaitSurveyScan,Read<string>(r.R.Miner,"reason"));
            r.Ready=true;Assert.Equal("SurveyEmpty",r.Finish().Outcome);
        }
        [Fact]
        public void ProbeTransitReachesTheAlignmentToleranceBeforeTurningToTheBore()
        {
            var r=new SurveyRig();r.Job.Kind=P.JobKind.Probe;r.Job.Depth=40;
            Vector3D approach=r.Job.Entry-r.Job.Direction*22;r.R.Position=approach+Vector3D.Right*.8;
            r.Orientation=MatrixD.CreateWorld(Vector3D.Zero,Vector3D.Left,Vector3D.Up);r.Ready=false;
            r.Tick(false);Assert.Equal(P.FlightState.Transit,r.R.StateNow);
            r.Ready=true;for(int n=0;n<600 && r.R.StateNow==P.FlightState.Transit;n++)r.Tick();
            Assert.Equal(P.FlightState.Align,r.R.StateNow);Assert.InRange(Vector3D.Distance(approach,r.R.Position),0,.5);
            r.Velocity=Vector3D.Zero;r.R.Camera.Method("Raycast",a=>MinerStateTests.MinerRig.Hit(MyDetectedEntityType.Asteroid));
            for(int n=0;n<10 && r.R.StateNow==P.FlightState.Align;n++)
            {Call(r.R.Miner,"Align",.05);r.Orientation=Read<MatrixD>(r.Flight,"previousDesired").GetOrientation();}
            Assert.Equal(P.FlightState.Drilling,r.R.StateNow);
        }
    }
}