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

namespace AutoMiningScript.Tests
{
    public class SavedMinerNavigationTests
    {
        // Actual remote-relative positions and directions from miner 7's 28 cameras,
        // 2026-09-14 14:41:28 save, grid 87777267991379386. Obstacles below are
        // controlled wall/railing models; they do not claim to reproduce game physics.
        static readonly double[,] Cameras={
            {1,1,1,0,1,0},{0,1,1.5,0,0,1},{0,1,0,0,0,-1},{2,0,1,1,0,0},
            {0,-.5,-.5,0,0,-1},{0,-1.5,.5,0,0,-1},{-2,0,1,-1,0,0},{1.5,0,2,0,0,1},
            {-1.5,0,2,0,0,1},{-1,1,1,0,1,0},{1,1,1.5,0,1,0},{-1,1,1.5,0,1,0},
            {.5,-1.5,1.5,0,-1,0},{-.5,-1.5,1.5,0,-1,0},{.5,-1.5,1,0,-1,0},{-.5,-1.5,1,0,-1,0},
            {0,-1.5,1.5,0,0,1},{1.5,.5,2,0,0,1},{-1.5,.5,2,0,0,1},{0,0,-.5,0,0,-1},
            {-.5,1,.5,0,0,-1},{.5,1,.5,0,0,-1},{-.5,1,1.5,0,0,1},{.5,1,1.5,0,0,1},
            {-2,0,.5,-1,0,0},{2,0,.5,1,0,0},{-2,0,1.5,-1,0,0},{2,0,1.5,1,0,0}
        };
        static readonly Vector3D[] Axes={Vector3D.Right,Vector3D.Left,Vector3D.Up,Vector3D.Down,Vector3D.Forward,Vector3D.Backward};
        const BindingFlags Private=BindingFlags.Instance|BindingFlags.NonPublic;
        static T Read<T>(NavigationDetourTests.World w,string name) => (T)typeof(P.FlightController).GetField(name,Private).GetValue(w.R.Flight);
        static bool Covers(MatrixD lens,Vector3D target)
        {
            var q=Vector3D.TransformNormal(target-lens.Translation,MatrixD.Transpose(lens));
            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;
        }
        static NavigationDetourTests.World World(HashSet<int> used,bool wholeGridBounds)
        {
            var w=new NavigationDetourTests.World();w.R.Ship.Cameras.Clear();
            // All 85 blocks expanded by their installed definition sizes and mounting
            // orientations, plus a half-cell, from the same ship's 15:01:28 save.
            w.R.Ship.BodyMin=new Vector3D(-2.25,-1.75,-2.25);w.R.Ship.BodyMax=new Vector3D(2.25,1.25,2.25);
            w.R.Ship.BodyHalfSize=new Vector3D(2.25,1.75,2.25);w.R.Ship.Radius=4.63146096089488;
            for(int n=0;n<Cameras.GetLength(0);n++)
            {
                int id=n;var direction=new Vector3D(Cameras[n,3],Cameras[n,4],Cameras[n,5]);
                var local=MatrixD.CreateWorld(new Vector3D(Cameras[n,0],Cameras[n,1],Cameras[n,2]),direction,P.Data.Perpendicular(direction,Vector3D.Up));
                int axis=Array.FindIndex(Axes,a=>Vector3D.Dot(a,direction)>.999);
                var camera=w.R.MakeCamera(n+1,()=>100000,()=>default(MyDetectedEntityInfo))
                    .Method("get_WorldMatrix",a=>local*w.R.Orientation).Method("GetPosition",a=>(local*w.R.Orientation).Translation)
                    .Method("CanScan",a=>Covers(local*w.R.Orientation,(Vector3D)a[0]) && Vector3D.Distance((local*w.R.Orientation).Translation,(Vector3D)a[0])<=100000)
                    .Method("Raycast",a=>{
                        used.Add(id);w.Rays[axis]++;var lens=local*w.R.Orientation;var end=(Vector3D)a[0];
                        Assert.True(Covers(lens,end));var offset=end-lens.Translation;double length=offset.Length(),nearest=length+1;
                        var ray=new RayD(lens.Translation,offset/length);BoundingBoxD nearestBox=default(BoundingBoxD);
                        var gridMin=new Vector3D(double.MaxValue);var gridMax=new Vector3D(double.MinValue);
                        foreach(var box in w.Obstacles)
                        {gridMin=Vector3D.Min(gridMin,box.Min);gridMax=Vector3D.Max(gridMax,box.Max);
                            var hit=box.Intersects(ray);if(hit.HasValue && hit.Value<=length && hit.Value<nearest){nearest=hit.Value;nearestBox=box;}}
                        if(nearest>length)return default(MyDetectedEntityInfo);
                        return new MyDetectedEntityInfo(80768303324895649L,"mother ship",MyDetectedEntityType.LargeGrid,ray.Position+ray.Direction*nearest,
                            MatrixD.Identity,Vector3.Zero,MyRelationsBetweenPlayerAndBlock.Owner,wholeGridBounds?new BoundingBoxD(gridMin,gridMax):nearestBox,1);
                    });
                w.R.Ship.Cameras.Add(camera.Value);
            }
            return w;
        }
        static double HullGap(NavigationDetourTests.World w)
        {
            // The enclosing world box contains every corner of the full oriented body.
            // A positive separation proves the entire body is clear, not only its center.
            var body=new BoundingBoxD(w.R.Ship.BodyMin,w.R.Ship.BodyMax);
            var min=new Vector3D(double.MaxValue);var max=new Vector3D(double.MinValue);
            for(int n=0;n<8;n++)
            {var corner=new Vector3D((n&1)==0?body.Min.X:body.Max.X,(n&2)==0?body.Min.Y:body.Max.Y,(n&4)==0?body.Min.Z:body.Max.Z);
                var point=Vector3D.Transform(corner,w.R.Orientation);min=Vector3D.Min(min,point);max=Vector3D.Max(max,point);}
            double nearest=double.MaxValue;
            foreach(var box in w.Obstacles)
            {
                var dx=Math.Max(box.Min.X-max.X,min.X-box.Max.X);
                var dy=Math.Max(box.Min.Y-max.Y,min.Y-box.Max.Y);
                var dz=Math.Max(box.Min.Z-max.Z,min.Z-box.Max.Z);
                nearest=Math.Min(nearest,Math.Max(dx,Math.Max(dy,dz)));
            }
            return nearest;
        }

        [Theory]
        [InlineData(false,false,false)] [InlineData(false,true,false)]
        [InlineData(true,false,false)] [InlineData(true,true,false)]
        [InlineData(false,false,true)] [InlineData(false,true,true)]
        [InlineData(true,false,true)] [InlineData(true,true,true)]
        public void ActualSevenCameraLayoutFindsAScannedRoutePastACloseMotherWall(bool railing,bool actualBraking,bool wholeGridBounds)
        {
            var used=new HashSet<int>();var w=World(used,wholeGridBounds);
            // World integrates force / 1000 kg. Match the save's available 5.24 m/s²
            // braking acceleration without assuming its unknown per-thruster split.
            if(actualBraking)foreach(var thrust in w.R.Thrusters)thrust.Set("MaxEffectiveThrust",5240f);
            var front=w.R.Ship.BodyMin.Z-3;
            w.Obstacles.Add(new BoundingBoxD(new Vector3D(-30,-3,front-2),new Vector3D(30,3,front)));
            if(railing)
            {
                w.Obstacles.Clear();
                w.Obstacles.Add(new BoundingBoxD(new Vector3D(-30,2.7,front-.3),new Vector3D(30,3,front)));
                for(int n=-5;n<=5;n++)w.Obstacles.Add(new BoundingBoxD(new Vector3D(n*4-.2,-3,front-.3),new Vector3D(n*4+.2,3,front)));
            }
            var target=new Vector3D(15,0,-75);bool arrived=false;double gap=HullGap(w);int highestCandidate=0;
            w.Step(target);Assert.False(w.Turned,w.Diagnostics);
            for(int n=0;n<7000 && !arrived;n++)
            {
                arrived=w.Step(target);gap=Math.Min(gap,HullGap(w));highestCandidate=Math.Max(highestCandidate,Read<int>(w,"detourAttempts"));
                Assert.True(gap>=.01,"Whole-body gap="+gap+" t="+w.R.Program.Now+" position="+w.Position+" velocity="+w.Velocity+
                    " waypoint="+w.R.Flight.RouteTarget+" / "+w.R.Flight.Problem+" / "+w.R.Flight.CameraDiagnostics);
            }
            Assert.True(arrived,w.Diagnostics);Assert.NotEmpty(w.Waypoints);Assert.True(used.Count>=3,w.Diagnostics);
            Assert.False(w.TurnedWithoutCoverageGap,w.Diagnostics);Assert.False(w.Collided,w.Diagnostics);
            Assert.InRange(w.Travel,70,210);
            Assert.True(w.Waypoints.Any(p=>Math.Abs(p.Y)>4),"Failed to choose a full-body opening; max candidate="+highestCandidate+"\n"+w.Diagnostics);
        }
    }
}