using System;
using System.Collections.Generic;
using Sandbox.ModAPI.Ingame;
using VRage.Game;
using VRage.Game.ModAPI.Ingame.Utilities;
using VRageMath;

namespace AutoMiningScript
{
    public partial class Program
    {
        public sealed class FlightController
        {
            class SeenPoint {public Vector3D Position,Velocity;public double At;public long Entity;}
            readonly List<SeenPoint> seen = Data.CreateList<SeenPoint>();
            readonly Program p;
            readonly ShipHardware h;
            readonly double[] force = new double[6];
            public double Acceleration, BrakeAcceleration, ThrustMargin, LastFlightMass;
            public string Problem = "";
            public bool Blocked;
            public double ScanClearance, AttitudeError, CommandSpeed;
            public JobReport SurfaceHit;
            public Vector3D RouteTarget => hasDetour ? detour : routeTarget;
            public bool HasRoute => routeKnown;
            public bool TrafficWait, TrafficAvoid;
            public Vector3D TrafficPoint;
            public string TrafficPeer="";
            public double TrafficAt=-100;
            double mass;
            MatrixD basis;
            MatrixD previousDesired;
            bool orientationKnown;
            double previousOrientationAt;
            Vector3D gravity;
            Vector3D holdPosition;
            bool holdActive;
            Vector3D routeTarget, detour;
            bool routeKnown, hasDetour, choosingRoute;
            int detourAttempts;
            double detourAt;
            readonly List<Vector3D> breadcrumbs = Data.CreateList<Vector3D>();
            readonly List<Vector3D> routeOptions = Data.CreateList<Vector3D>();
            readonly HashSet<long> occludedCameras = new HashSet<long>();
            readonly HashSet<long> scanCameras = new HashSet<long>();
            Vector3D scanOrigin, scanDirection, scanRight, scanUp, cameraTarget;
            MatrixD scanBasis, clearBasis;
            Vector3D clearOrigin, clearDirection;
            double scanHalfWidth, scanHalfHeight, clearLeading;
            double scanLength, scanTravelLimit, scanStarted, scanCompleted, previousScan = -100, firstRayAt, scanLimit = 2000, scanSafeLength;
            int scanIndex, obstacleRay;
            bool scanning, scanFailed, scanBlocked, scanObstacleFound, obstacleLocated;
            bool scanWaitingForCharge;
            string cameraProblem="",cameraDetail="";
            int readyCameras;
            double cameraCheckedAt=-100;
            Vector3D obstacle;
            BoundingBoxD obstacleBox;
            string obstacleName="";
            long dockGrid;
            Vector3D dockContact;
            double dockRadius;

            public FlightController(Program program, ShipHardware hardware) { p = program; h = hardware; }
            public void SetDockContact(long gridId, Vector3D contact, double radius)
            { dockGrid = gridId; dockContact = contact; dockRadius = Data.Max(0.1, radius); }
            public void ClearDockContact() { dockGrid = 0; }

            public void ResetRoute()
            {
                routeKnown = hasDetour = choosingRoute = scanning = scanBlocked = scanFailed = holdActive = orientationKnown = false;
                SurfaceHit = null;
                detourAttempts = 0; ScanClearance = CommandSpeed = 0; Blocked = false; Problem = cameraProblem = cameraDetail = ""; scanWaitingForCharge = false; breadcrumbs.Clear(); scanLimit = 2000; occludedCameras.Clear(); scanCameras.Clear();
            }
            public void Release() { h.Release(); ResetRoute(); ClearDockContact(); }

            void Refresh()
            {
                basis = h.Controller.WorldMatrix;
                gravity = h.Controller.GetNaturalGravity();
                var attached = h.IsAttached;
                if (!attached)
                {
                    mass = h.Controller.CalculateShipMass().PhysicalMass;
                    if (Data.Finite(mass) && mass > 0) LastFlightMass = mass;
                }
                else
                {
                    var configured = p.Config.Number("Flight", "DepartureMass", 0, 0, 1e10);
                    mass = configured > 0 ? configured : h.AutoDepartureMass;
                }
                mass = Data.Finite(mass) && mass > 0 ? mass : 0;
                for (int i = 0; i < 6; i++) force[i] = 0;
                MatrixD inverse = Data.Transpose(basis);
                for (int i = 0; i < h.Thrusters.Count; i++)
                {
                    var thruster = h.Thrusters[i];
                    if (!thruster.IsFunctional || !thruster.Enabled || thruster.CubeGrid != p.Me.CubeGrid) continue;
                    Vector3D direction = Data.TransformNormal(thruster.WorldMatrix.Backward, inverse);
                    int axis = Axis(direction);
                    force[axis] += Data.Max(0, thruster.MaxEffectiveThrust);
                }
            }

            public bool CanSupport(Vector3D forward, Vector3D up, double reserve)
            {
                if (h.Controller == null) { Problem = L.MinerMissingController; return false; }
                Refresh();
                if (mass <= 0) { ThrustMargin = 0; Problem = h.IsAttached ? h.DepartureMassProblem : L.MinerMassUncalibrated; return false; }
                MatrixD desired;
                if (!Frame(forward, up, out desired)) { Problem = L.MinerInvalidAttitude; return false; }
                Vector3D localGravity = Data.TransformNormal(gravity, Data.Transpose(desired));
                var worst = double.MaxValue;
                var hoverUse = 1 - p.Config.Number("Flight", "HoverReserve", 0.3, 0.1, 0.8);
                for (int axis = 0; axis < 6; axis++)
                {
                    var g = Component(localGravity, axis);
                    var available = force[axis] / mass + g;
                    worst = Data.Min(worst, available);
                    if (g < 0 && -g * mass > force[axis] * hoverUse)
                    { ThrustMargin = worst; Problem = L.F(L.MinerHoverReserve, (1 - hoverUse) * 100); return false; }
                    if (force[axis] <= 1 || available < reserve)
                    { ThrustMargin = worst; Problem = L.MinerAttitudeThrustInsufficient; return false; }
                }
                ThrustMargin = worst; return true;
            }

            public bool Move(Vector3D target, Vector3D targetVelocity, Vector3D forward, Vector3D up,
                double speed, double tolerance, bool scan, double dt, bool faceTravel = false, bool allowDetour = true, double minimumSpeed = 0)
            {
                Blocked = false; Problem = ""; holdActive = false; CommandSpeed = 0;
                if (h.Controller == null) { Blocked = true; Problem = L.MinerMissingController; return false; }
                if (!Data.Finite(target) || !Data.Finite(targetVelocity) || !Data.Finite(forward) || !Data.Finite(up) || !Data.Finite(speed))
                { h.Release(); Blocked = true; Problem = L.MinerNonFiniteFlight; return false; }
                if (h.Controller.IsUnderControl)
                { h.Release(); Problem = L.MinerManualControl; Blocked = true; return false; }
                Refresh();
                if (mass <= 0) { Problem = L.MinerMassUncalibrated; Blocked = true; h.Release(); return false; }
                if (h.IsAttached)
                { h.Release(); Problem = L.MinerConnectedThrustDisabled; return false; }
                MatrixD desired;
                if (!Frame(forward, up, out desired))
                { Problem = L.MinerInvalidAttitude; Blocked = true; Hold(dt); return false; }
                dt = Data.Max(1.0 / 120, Data.Min(0.25, dt));
                speed = Data.Max(0, Data.Min(95, speed)); tolerance = Data.Max(0.03, tolerance);
                var position = h.Controller.GetPosition();
                var velocities = h.Controller.GetShipVelocities();
                Vector3D referenceVelocity = velocities.LinearVelocity + Data.Cross(velocities.AngularVelocity, position - h.Controller.CenterOfMass);
                var relativeVelocity = referenceVelocity - targetVelocity;
                if (!routeKnown || Data.DistanceSquared(routeTarget, target) > Data.Max(100, h.Radius * h.Radius * 4))
                {
                    var continuation=routeKnown && !hasDetour &&
                        Data.Dot(Data.Unit(routeTarget-position,Data.Zero),Data.Unit(target-position,Data.Zero))>0.98;
                    routeTarget = target; routeKnown = true; hasDetour = choosingRoute = false; detourAttempts = 0;breadcrumbs.Clear();
                    // Extending a straight route does not invalidate its already scanned
                    // portion. The sweep still enforces its original travel limit and TTL.
                    if(!continuation) {scanning = false; ScanClearance = 0; scanBlocked = false;}
                }
                var activeTarget = hasDetour ? detour : target;
                var displacement = activeTarget - position;
                var distance = Data.Length(displacement);
                var direction = distance > 0.001 ? displacement / distance : desired.Forward;
                // Keep existing camera coverage and hull attitude. Turn only when the
                // required direction has no working camera, independently of its charge.
                if (faceTravel && distance > tolerance)
                    desired=h.CameraAttitude(position+direction*Data.Max(distance,h.Radius*3+5),up);
                else if (faceTravel) desired = basis.GetOrientation();
                Acceleration = AvailableAcceleration(direction);
                Vector3D brakeDirection = Data.LengthSquared(relativeVelocity) > 0.01 ? -Vector3D.Normalize(relativeVelocity) : -direction;
                BrakeAcceleration = AvailableAcceleration(brakeDirection);
                ThrustMargin = Data.Min(Acceleration, BrakeAcceleration);
                if (BrakeAcceleration < 0.1 || Acceleration < 0.05)
                {
                    Problem = L.MinerThrustMarginInsufficient; Blocked = true;
                    ApplyAcceleration(-referenceVelocity * 1.5); ApplyAttitude(desired, velocities.AngularVelocity);
                    return false;
                }
                var usableBrake = Data.Max(0.05, BrakeAcceleration * 0.65);
                var desiredSpeed = Data.Min(speed, Math.Sqrt(2 * usableBrake * Data.Max(0, distance - tolerance * 0.5)));
                desiredSpeed = Data.Min(desiredSpeed, distance * p.Config.Number("Flight", "PositionGain", 0.8, 0.1, 3));
                desiredSpeed = Data.Max(desiredSpeed, minimumSpeed);
                if (scan && distance > tolerance)
                {
                    var currentSpeed = dockGrid != 0 ? Data.Length(relativeVelocity) : Data.Length(referenceVelocity);
                    var leading = LeadingExtent(direction);
                    var scanDistance = Data.Max(h.Radius * 3 + 5,
                        currentSpeed * currentSpeed / (2 * usableBrake) + currentSpeed * 2 + h.Radius * 2 + 5);
                    scanDistance = Data.Max(scanDistance, Data.Min(distance + h.Radius, desiredSpeed * 3 + h.Radius * 3));
                    // A nearby waypoint must not squeeze the camera targets against the
                    // nose. Observe at a useful horizon, but authorise only this route leg.
                    ScanEnvelope(position, direction, Data.Min(2000, scanDistance), distance + leading + 0.2);
                    var traveled = Data.Max(0, Data.Dot(position - clearOrigin, clearDirection));
                    var clear = Data.Max(0, ScanClearance - traveled - clearLeading - currentSpeed * 0.5);
                    var known=KnownClearance(position,direction,distance);
                    if(known<distance){scanBlocked=true;clear=Data.Min(clear,Data.Max(0,known-currentSpeed*.5));}
                    desiredSpeed = Data.Min(desiredSpeed, Math.Sqrt(2 * usableBrake * clear));
                    if(scanBlocked)Problem=obstacleLocated?L.F(L.CameraObstacle,obstacleName,Data.Max(0,Data.Dot(obstacle-position,direction)-leading-.25),obstacleRay):L.F(L.CameraObstacleUnknown,obstacleName);
                    if(choosingRoute)
                    {
                        desiredSpeed=0;Problem=L.MinerFindingRoute;
                        if(scanBlocked || (scanFailed && !scanWaitingForCharge && p.Now-detourAt>3))NextDetour();
                        else if(!scanning && ScanClearance>=distance+leading-.1)choosingRoute=false;
                    }
                    else if (scanBlocked && clear < 0.2)
                    {
                        desiredSpeed = 0;
                        Problem = L.MinerObstacleBraking+" / "+Problem;
                        if (Data.Length(referenceVelocity) < 0.3)
                        {
                            if (!scanning && dockGrid == 0 && allowDetour)FindDetour(position,target);
                            else { Blocked = !scanning; }
                        }
                    }
                    else if (!scanBlocked && clear < 0.2)
                    {
                        Problem = scanFailed ? (cameraProblem.Length>0?cameraProblem:L.MinerCameraUnavailable) : L.MinerEnvelopeScanPending+" "+scanIndex+"/18";
                    }
                }
                if(faceTravel && scan && scanFailed && !scanWaitingForCharge)desired=h.CameraAttitude(cameraTarget,up);
                if (faceTravel && !CanSupport(desired.Forward, desired.Up, 0.1))
                {
                    var failure = Problem;
                    Hold(dt); Blocked = !choosingRoute;if(choosingRoute)NextDetour();Problem = failure;
                    return false;
                }
                AttitudeError = ApplyAttitude(desired, velocities.AngularVelocity);
                if (AttitudeError > 0.35) desiredSpeed = Data.Min(desiredSpeed, 0.5);
                if (AttitudeError > 1) desiredSpeed = 0;
                if(scan && TrafficWait && p.Now-TrafficAt<3) {desiredSpeed=0;Problem=L.F(L.MinerTrafficYield,TrafficPeer);}
                CommandSpeed = desiredSpeed;
                var desiredVelocity = targetVelocity + direction * desiredSpeed;
                var desiredAcceleration = (desiredVelocity - referenceVelocity) * (TrafficBraking || (scan && scanBlocked)?1/Data.Max(.05,dt):p.Config.Number("Flight", "VelocityGain", 1.6, 0.2, 5));
                ApplyAcceleration(desiredAcceleration);
                var arrived = distance <= tolerance && Data.Length(relativeVelocity) <= Data.Max(0.08, Data.Min(0.3, speed * 0.2)) && AttitudeError < 0.035 && !(scan && TrafficWait && p.Now-TrafficAt<3);
                if (arrived && hasDetour && !choosingRoute)
                { hasDetour = false; scanning = false; ScanClearance = 0; return false; }
                return arrived && !choosingRoute;
            }

            void FindDetour(Vector3D position,Vector3D target)
            {
                if(breadcrumbs.Count==16)breadcrumbs.RemoveAt(0);breadcrumbs.Add(position);
                routeOptions.Clear();double step=Data.Max(6,h.Radius*3+5);
                for(int x=-1;x<=1;x++)for(int y=-1;y<=1;y++)for(int z=-1;z<=1;z++)
                    if(x!=0 || y!=0 || z!=0)routeOptions.Add(position+Data.TransformNormal(Data.Unit(new Vector3D(x,y,z),basis.Forward),basis)*step);
                routeOptions.Sort((a,b)=>RouteCost(a,target,step).CompareTo(RouteCost(b,target,step)));
                detourAttempts=-1;hasDetour=choosingRoute=true;NextDetour();
            }
            double RouteCost(Vector3D point,Vector3D target,double step)
            {
                double cost=Data.Distance(point,target);
                var crossing=obstacleBox.Intersects(new RayD(point,Data.Unit(target-point,basis.Forward)));
                if(crossing.HasValue && crossing.Value<cost)cost+=step*4;
                foreach(var visited in breadcrumbs)if(Data.DistanceSquared(point,visited)<step*step*.64)cost+=step*4;
                return cost;
            }
            void NextDetour()
            {
                detour=routeOptions[++detourAttempts%routeOptions.Count];
                detourAt=p.Now;
                scanning=scanBlocked=scanFailed=false;ScanClearance=0;scanLimit=2000;
            }
            void Remember(Vector3D point,Vector3D velocity,long entity)
            {
                foreach(var item in seen)if(item.Entity==entity && Data.DistanceSquared(item.Position,point)<1){item.Position=point;item.Velocity=velocity;item.At=p.Now;return;}
                if(seen.Count==16)seen.RemoveAt(0);
                seen.Add(new SeenPoint {Position=point,Velocity=velocity,At=p.Now,Entity=entity});
            }
            double KnownClearance(Vector3D position,Vector3D direction,double distance)
            {
                foreach(var item in seen)
                {
                    var age=p.Now-item.At;if(age>(Data.LengthSquared(item.Velocity)>.01?3:30))continue;
                    var point=item.Position+item.Velocity*age;
                    if(dockGrid!=0 && item.Entity==dockGrid && Data.DistanceSquared(point,dockContact)<=dockRadius*dockRadius)continue;
                    var delta=point-position;double along=Data.Dot(delta,direction);
                    double side=Data.LengthSquared(delta)-along*along,radius=h.Radius+.25;
                    if(along>0 && side<radius*radius)distance=Data.Min(distance,Data.Max(0,along-Math.Sqrt(radius*radius-side)));
                }
                return distance;
            }

            bool TrafficBraking => p.Now-TrafficAt<1.5 && (TrafficWait || TrafficAvoid);
            public void Hold(double dt)
            {
                if (h.Controller == null) return;
                if (h.Controller.IsUnderControl || h.IsAttached) { h.Release(); return; }
                if (!holdActive) { holdPosition = h.Controller.GetPosition(); holdActive = true; }
                orientationKnown = false;
                Refresh();
                if (mass <= 0) { h.Release(); return; }
                var velocity = h.Controller.GetShipVelocities();
                ThrustMargin=AvailableAcceleration(-Data.Unit(velocity.LinearVelocity,basis.Forward));
                Vector3D offset = holdPosition - h.Controller.GetPosition();
                if (Data.Length(offset) > 2) offset = Vector3D.Normalize(offset) * 2;
                ApplyAcceleration(offset * 0.8 - velocity.LinearVelocity * (TrafficBraking?1/Data.Max(.05,dt):1.6));
                ApplyAttitude(basis, velocity.AngularVelocity);
            }

            double AvailableAcceleration(Vector3D worldDirection)
            {
                Vector3D direction = Data.TransformNormal(worldDirection, Data.Transpose(basis));
                Vector3D localGravity = Data.TransformNormal(gravity, Data.Transpose(basis));
                // Largest feasible acceleration along direction after paying gravity on all axes.
                var limit = double.MaxValue;
                for (int axis = 0; axis < 6; axis++)
                {
                    var component = Component(direction, axis);
                    var available = force[axis] / Data.Max(1, mass) + Component(localGravity, axis);
                    if (available < -0.01 && component >= 0) return 0;
                    if (component > 0.0001) limit = Data.Min(limit, available / component);
                }
                return Data.Max(0, Data.Min(1000, limit));
            }

            void ApplyAcceleration(Vector3D acceleration)
            {
                h.Controller.DampenersOverride = false;
                Vector3D requested = Data.TransformNormal((acceleration - gravity) * mass, Data.Transpose(basis));
                // Clamp independently per body axis so saturation does not discard gravity support.
                requested.X = Data.Max(-force[1], Data.Min(force[0], requested.X));
                requested.Y = Data.Max(-force[3], Data.Min(force[2], requested.Y));
                requested.Z = Data.Max(-force[5], Data.Min(force[4], requested.Z));
                MatrixD inverse = Data.Transpose(basis);
                for (int i = 0; i < h.Thrusters.Count; i++)
                {
                    var thruster = h.Thrusters[i];
                    if (thruster.CubeGrid != p.Me.CubeGrid) continue;
                    Vector3D direction = Data.TransformNormal(thruster.WorldMatrix.Backward, inverse);
                    int axis = Axis(direction);
                    var ratio = force[axis] > 0 ? Data.Max(0, Component(requested, axis)) / force[axis] : 0;
                    thruster.ThrustOverridePercentage = (float)Data.Max(0, Data.Min(1, ratio));
                }
            }

            double ApplyAttitude(MatrixD desired, Vector3D angularVelocity)
            {
                var error = RotationError(basis, desired);
                var angle = Data.Length(error);
                var maxRate = p.Config.Number("Flight", "MaxAngularRate", 0.6, 0.05, 2);
                var targetRate = Data.Zero;
                var elapsed = p.Now - previousOrientationAt;
                if (orientationKnown && elapsed > 0.0001 && elapsed < 0.3)
                {
                    var change = RotationError(previousDesired, desired);
                    if (Data.Length(change) < 0.05) targetRate = change / elapsed;
                    if (Data.Length(targetRate) > maxRate) targetRate = Vector3D.Normalize(targetRate) * maxRate;
                }
                previousDesired = desired; previousOrientationAt = p.Now; orientationKnown = true;
                var requested = targetRate + error * p.Config.Number("Flight", "AttitudeGain", 1.8, 0.1, 6) - (angularVelocity - targetRate) * 0.35;
                if (Data.Length(requested) > maxRate) requested = Vector3D.Normalize(requested) * maxRate;
                for (int i = 0; i < h.Gyros.Count; i++)
                {
                    var gyro = h.Gyros[i];
                    if (!gyro.IsFunctional || gyro.CubeGrid != p.Me.CubeGrid) continue;
                    Vector3D local = Data.TransformNormal(requested, Data.Transpose(gyro.WorldMatrix));
                    // PB properties negate all three physical axes and ignore writes until override is on.
                    gyro.GyroOverride = true;
                    gyro.Pitch = (float)-local.X; gyro.Yaw = (float)-local.Y; gyro.Roll = (float)-local.Z;
                }
                return angle;
            }

            void ScanEnvelope(Vector3D origin, Vector3D direction, double distance, double travelLimit)
            {
                var turn = Data.Dot(direction, scanDirection);
                var stale = p.Now - scanCompleted > 1.5;
                if (stale) ScanClearance = 0;
                if (ScanClearance > 0 && Data.Length(RotationError(clearBasis, basis)) > 0.035) ScanClearance = 0;
                if (!scanning && (ScanClearance <= 0 || p.Now - previousScan > 0.3 || stale || turn < 0.98))
                {
                    if (turn < 0.98 || stale) ScanClearance = 0;
                    scanOrigin = origin; scanDirection = direction; scanLength = Data.Min(distance, scanLimit); scanTravelLimit = travelLimit; scanSafeLength=scanLength; scanObstacleFound=false;
                    scanBasis = basis.GetOrientation();
                    scanRight = Data.Cross(direction, basis.Up);
                    if (Data.LengthSquared(scanRight) < 0.01) scanRight = basis.Right; else scanRight.Normalize();
                    scanUp = Vector3D.Normalize(Data.Cross(scanRight, direction));
                    scanHalfWidth = ProjectedExtent(scanRight); scanHalfHeight = ProjectedExtent(scanUp);
                    scanIndex = 0; scanStarted = p.Now; previousScan = p.Now; scanning = true; scanFailed = false; scanWaitingForCharge = false; cameraProblem = cameraDetail = ""; occludedCameras.Clear(); scanCameras.Clear();
                }
                if (!scanning) return;
                if (Data.Dot(direction, scanDirection) < 0.98 || Data.DistanceSquared(origin, scanOrigin) > h.Radius * h.Radius || Data.Length(RotationError(scanBasis, basis)) > 0.035)
                { scanning = false; ScanClearance = 0; scanLimit = 2000; return; }
                // A charge-limited camera must not repeatedly spend its entire charge on an
                // incomplete long scan. Shorten the next envelope, which also lowers speed.
                // Never certify a completed envelope using rays older than the clearance TTL.
                if (scanIndex > 0 && p.Now - firstRayAt > 1.5)
                { RestartScan(); return; }
                int budget = p.Config.Integer("Flight", "RaysPerTick", 2, 1, 4);
                for (int ray = 0; ray < budget && scanIndex < 18; ray++)
                {
                    int lane = scanIndex % 9;
                    var along = scanIndex < 9 ? Data.Min(scanLength, Data.Max(LeadingExtent(scanDirection) + 0.2, scanLength * 0.35)) : scanLength;
                    var lateral = Data.Zero;
                    if (lane > 0)
                    {
                        var a = (lane - 1) * Math.PI / 4;
                        double x = Math.Cos(a), y = Math.Sin(a), scale = Data.Max(Math.Abs(x), Math.Abs(y));
                        lateral = scanRight * (x / scale * scanHalfWidth) + scanUp * (y / scale * scanHalfHeight);
                    }
                    var endpoint = scanOrigin + scanDirection * along + lateral;
                    var camera = FindCamera(ref endpoint);
                    if (camera == null)
                    {
                        scanFailed = true;
                        if (scanIndex == 0 && p.Now - scanStarted > 3) RestartScan();
                        return;
                    }
                    var hit = camera.Raycast(endpoint);
                    if (hit.EntityId != p.Me.CubeGrid.EntityId && hit.HitPosition.HasValue && Data.Finite(hit.HitPosition.Value) &&
                        (hit.Type == MyDetectedEntityType.Asteroid || hit.Type == MyDetectedEntityType.Planet))
                        SurfaceHit = new JobReport { Scan = ScanRecord.Capture(camera.GetPosition(),endpoint,hit), Outcome = "SurveyHit", Position = hit.HitPosition.Value,
                            Direction = Data.Unit(endpoint - camera.GetPosition(), basis.Forward), Up = basis.Up, EntityId = hit.EntityId, Message = L.MinerSurfaceHitUnconfirmed };
                    if (!hit.IsEmpty() && hit.EntityId != p.Me.CubeGrid.EntityId)
                    {
                        Vector3D point = hit.HitPosition.HasValue ? hit.HitPosition.Value : hit.Position;
                        bool permittedContact = dockGrid != 0 && hit.EntityId == dockGrid && hit.HitPosition.HasValue &&
                            Data.DistanceSquared(point, dockContact) <= dockRadius * dockRadius;
                        // Rays may extend past a short waypoint to obtain a usable view.
                        // A hit beyond this leg limits evidence; it does not obstruct this leg.
                        bool beyondLeg = hit.HitPosition.HasValue && Data.Dot(point - scanOrigin, scanDirection) > scanTravelLimit;
                        if (!permittedContact && !beyondLeg)
                        {
                            bool located=hit.HitPosition.HasValue && Data.Finite(point);
                            if(located)Remember(point,hit.Velocity,hit.EntityId);
                            var limit=located?Data.Max(0,Data.Dot(point-scanOrigin,scanDirection)-.25):0;
                            if(!scanObstacleFound || limit<scanSafeLength)
                            {obstacle=point;obstacleLocated=located;obstacleBox=new BoundingBoxD(hit.BoundingBox.Min-new Vector3D(h.Radius),hit.BoundingBox.Max+new Vector3D(h.Radius));obstacleName=string.IsNullOrEmpty(hit.Name)?hit.EntityId.ToString():hit.Name;obstacleRay=scanIndex+1;}
                            scanSafeLength=Data.Min(scanSafeLength,limit);scanObstacleFound=scanBlocked=true;
                            // A hit bounds the observed free prefix, not the whole route.
                            // Revoke conflicting old clearance immediately; extend it only
                            // after every region in this sweep has been observed.
                            if(scanSafeLength<=LeadingExtent(scanDirection)+.2)
                            {scanning=false;ScanClearance=0;return;}
                            ScanClearance=Data.Min(ScanClearance,Data.Max(0,Data.Dot(point-clearOrigin,clearDirection)-.25));
                        }
                    }
                    else if (!hit.IsEmpty())
                    {
                        // Retry this same point with another camera, within the ray budget.
                        // One obstructed lens must not erase the previous sweep's clearance.
                        occludedCameras.Add(camera.EntityId); scanWaitingForCharge = false;
                        cameraProblem=L.F(L.CameraSelfHit,CameraName(camera));continue;
                    }
                    if (scanIndex == 0) firstRayAt = p.Now;
                    scanIndex++; scanCameras.Add(camera.EntityId); occludedCameras.Clear(); scanFailed = false;
                }
                if (scanIndex == 18)
                { scanning = false; scanBlocked=scanObstacleFound; ScanClearance = Data.Min(scanSafeLength, scanTravelLimit); clearOrigin = scanOrigin; clearDirection = scanDirection; clearBasis = scanBasis; clearLeading = LeadingExtent(scanDirection); scanCompleted = p.Now; scanFailed = false; }
            }

            void RestartScan()
            {
                // Only energy-limited scans benefit from a shorter range. Shortening after
                // a field-of-view failure moves corners closer and makes their angle worse.
                var minimum=LeadingExtent(scanDirection)+Data.Max(1,h.Radius*0.5);
                scanLimit=scanWaitingForCharge?Data.Min(scanLength,Data.Max(minimum,scanLength*0.65)):2000;
                scanning=false;ScanClearance=0;scanFailed=true;
            }

            double LeadingExtent(Vector3D direction)
            {
                Vector3D local = Data.TransformNormal(direction, Data.Transpose(basis));
                return Data.Max(0, local.X * (local.X >= 0 ? h.BodyMax.X : h.BodyMin.X) +
                    local.Y * (local.Y >= 0 ? h.BodyMax.Y : h.BodyMin.Y) + local.Z * (local.Z >= 0 ? h.BodyMax.Z : h.BodyMin.Z));
            }
            double ProjectedExtent(Vector3D direction)
            {
                Vector3D local = Data.TransformNormal(direction, Data.Transpose(basis));
                return Math.Abs(local.X) * h.BodyHalfSize.X + Math.Abs(local.Y) * h.BodyHalfSize.Y + Math.Abs(local.Z) * h.BodyHalfSize.Z + 0.2;
            }

            IMyCameraBlock FindCamera(ref Vector3D target)
            {
                IMyCameraBlock result = null;
                readyCameras=0;cameraCheckedAt=p.Now;
                var best = double.MinValue; var selected = target;
                for (int i = 0; i < h.Cameras.Count; i++)
                {
                    var camera = h.Cameras[i];
                    if (!camera.IsWorking || camera.CubeGrid != p.Me.CubeGrid || occludedCameras.Contains(camera.EntityId)) continue;
                    var point=target;
                    if (!camera.CanScan(point)) {point=CameraTarget(camera,target);if(!camera.CanScan(point))continue;}
                    readyCameras++;
                    Vector3D ray=point-camera.GetPosition();var length=Data.Max(0.001,Data.Length(ray));
                    double score=2*Data.Dot(ray/length,camera.WorldMatrix.Forward)-length/Data.Max(1,scanLength);
                    if (score > best) { result = camera; best = score; selected=point; }
                }
                if(result==null){cameraTarget=target;ExplainCameraFailure(target);}
                else {target=selected;scanWaitingForCharge=false;cameraProblem=cameraDetail="";}
                return result;
            }

            Vector3D CameraTarget(IMyCameraBlock camera, Vector3D target)
            {
                // Each lane is assigned independently. If its nominal near sample is too
                // close for this lens, find a visible depth in the same lane, within the
                // far horizon. The API still has to accept, and actually cast, that ray.
                if (scanIndex>=9 || camera.RaycastConeLimit<=0 || camera.RaycastConeLimit>=89) return target;
                double low=0,high=Data.Max(0,scanLength-Data.Dot(target-scanOrigin,scanDirection));
                MatrixD inverse=Data.Transpose(camera.WorldMatrix);
                Vector3D q=Data.TransformNormal(target-camera.GetPosition(),inverse),d=Data.TransformNormal(scanDirection,inverse);
                var tangent=Math.Tan(camera.RaycastConeLimit*Math.PI/180);
                if(!ViewInterval(-q.Z-0.01,-d.Z,ref low,ref high) ||
                    !ViewInterval(-q.Z*tangent+q.X,-d.Z*tangent+d.X,ref low,ref high) ||
                    !ViewInterval(-q.Z*tangent-q.X,-d.Z*tangent-d.X,ref low,ref high) ||
                    !ViewInterval(-q.Z*tangent+q.Y,-d.Z*tangent+d.Y,ref low,ref high) ||
                    !ViewInterval(-q.Z*tangent-q.Y,-d.Z*tangent-d.Y,ref low,ref high))return target;
                return target+scanDirection*Data.Min(high,low+0.001);
            }
            static bool ViewInterval(double offset,double slope,ref double low,ref double high)
            {
                if(Math.Abs(slope)<1e-9)return offset>=0;
                var edge=-offset/slope;
                if(slope>0)low=Data.Max(low,edge);else high=Data.Min(high,edge);
                return low<=high;
            }

            public string CameraDiagnostics
            {
                get
                {
                    int count=0,working=0;
                    foreach(var camera in h.Cameras)if(camera.CubeGrid==p.Me.CubeGrid) {count++;if(camera.IsWorking)working++;}
                    Vector3D direction=Data.TransformNormal(scanDirection,Data.Transpose(h.Controller==null?MatrixD.Identity:h.Controller.WorldMatrix));
                    int axis=Axis(direction);
                    var name=axis==0?L.CameraRight:axis==1?L.CameraLeft:axis==2?L.CameraUp:axis==3?L.CameraDown:axis==4?L.CameraRear:L.CameraFront;
                    return L.F(L.CameraInventory,count,working,RecentCameraCheck?readyCameras:0)+" / "+
                        (RecentCameraCheck?L.F(L.CameraRay,name,Data.Min(18,scanIndex+1),Data.Max(0,p.Now-cameraCheckedAt))+" / "+L.F(L.CameraCoverage,scanIndex,scanCameras.Count)+(cameraDetail.Length>0?" / "+cameraDetail:""):L.CameraScanInactive);
                }
            }
            bool RecentCameraCheck => p.Now-cameraCheckedAt<5;

            string CameraName(IMyCameraBlock camera)
            => string.IsNullOrEmpty(camera.CustomName)?camera.EntityId.ToString():camera.CustomName;

            void ExplainCameraFailure(Vector3D target)
            {
                scanWaitingForCharge=false;cameraDetail="";cameraProblem=L.MinerCameraUnavailable;
                IMyCameraBlock candidate=null;string hardware="",view="",limit="",self="";
                double wait=double.MaxValue,viewError=double.MaxValue;int localCount=0,working=0;
                for(int n=0;n<h.Cameras.Count;n++)
                {
                    var c=h.Cameras[n];if(c.CubeGrid!=p.Me.CubeGrid)continue;localCount++;
                    var name=CameraName(c);
                    if(!c.IsWorking)
                    {hardware=L.F(!c.Enabled?L.CameraDisabled:!c.IsFunctional?L.CameraDamaged:L.CameraUnpowered,name);continue;}
                    working++;
                    if(occludedCameras.Contains(c.EntityId)) {self=L.F(L.CameraSelfHit,name);continue;}
                    if(!c.EnableRaycast && c.RaycastConeLimit>0) {hardware=L.F(L.CameraRaycastDisabled,name);continue;}
                    Vector3D point=CameraTarget(c,target),relative=point-c.GetPosition();var distance=Data.Length(relative);
                    Vector3D ray=Data.TransformNormal(relative,Data.Transpose(c.WorldMatrix));
                    var pitch=Math.Atan2(ray.Y,Math.Sqrt(ray.X*ray.X+ray.Z*ray.Z))*180/Math.PI;
                    double yaw=Math.Atan2(ray.X,-ray.Z)*180/Math.PI,cone=c.RaycastConeLimit;
                    // Use geometry only to explain a rejected API request, never to grant it.
                    if(cone>0 && Data.LengthSquared(ray)>0.000001 && (Math.Abs(pitch)>cone || Math.Abs(yaw)>cone))
                    {
                        var error=Data.Max(Math.Abs(pitch),Math.Abs(yaw))-cone;
                        if(error<viewError) {viewError=error;view=L.F(L.CameraView,name,pitch,yaw,cone,distance,scanIndex+1);}continue;
                    }
                    if(c.RaycastDistanceLimit>0 && distance>c.RaycastDistanceLimit)
                    {limit=L.F(L.CameraDistanceLimit,name,distance,c.RaycastDistanceLimit);continue;}
                    if(distance>c.AvailableScanRange+0.001 && distance-c.AvailableScanRange<wait)
                    {candidate=c;wait=distance-c.AvailableScanRange;}
                }
                if(candidate!=null)
                {
                    scanWaitingForCharge=true;var distance=Data.Distance(CameraTarget(candidate,target),candidate.GetPosition());
                    cameraProblem=L.F(L.CameraCharging,CameraName(candidate),candidate.AvailableScanRange,distance,Data.Max(0,candidate.TimeUntilScan(distance))/1000d);
                }
                else if(localCount==0)cameraProblem=L.CameraMissing;
                else if(working==0 && hardware.Length>0)cameraProblem=hardware;
                else if(self.Length>0 || view.Length>0) {cameraProblem=L.F(L.CameraRegionUnavailable,scanIndex+1,localCount);cameraDetail=self.Length>0?self:view;}
                else if(limit.Length>0)cameraProblem=limit;
                else if(hardware.Length>0)cameraProblem=hardware;
            }

            public static bool Frame(Vector3D forward, Vector3D up, out MatrixD matrix)
            {
                matrix = MatrixD.Identity;
                if (!Data.Finite(forward) || !Data.Finite(up) || Data.LengthSquared(forward) < 0.000001 || Data.LengthSquared(up) < 0.000001) return false;
                forward.Normalize(); up -= forward * Data.Dot(up, forward);
                if (Data.LengthSquared(up) < 0.000001) return false;
                up.Normalize(); matrix = MatrixD.CreateWorld(Data.Zero, forward, up); return true;
            }

            public static Vector3D RotationError(MatrixD current, MatrixD desired)
            {
                // Axis-angle from the world rotation matrix, including the 180-degree case.
                MatrixD delta = Data.Transpose(current.GetOrientation()) * desired.GetOrientation();
                var cos = Data.Max(-1, Data.Min(1, (delta.M11 + delta.M22 + delta.M33 - 1) * 0.5));
                var angle = Math.Acos(cos);
                if (angle < 0.0000001) return Data.Zero;
                var axis = new Vector3D(delta.M23 - delta.M32, delta.M31 - delta.M13, delta.M12 - delta.M21);
                if (Math.PI - angle < 0.0001)
                {
                    var signedAxis = axis;
                    var x = Math.Sqrt(Data.Max(0, (delta.M11 + 1) * 0.5));
                    var y = Math.Sqrt(Data.Max(0, (delta.M22 + 1) * 0.5));
                    var z = Math.Sqrt(Data.Max(0, (delta.M33 + 1) * 0.5));
                    if (x >= y && x >= z && x > 0.0001)
                        axis = new Vector3D(x, (delta.M12 + delta.M21) / (4 * x), (delta.M13 + delta.M31) / (4 * x));
                    else if (y >= z && y > 0.0001)
                        axis = new Vector3D((delta.M12 + delta.M21) / (4 * y), y, (delta.M23 + delta.M32) / (4 * y));
                    else if (z > 0.0001)
                        axis = new Vector3D((delta.M13 + delta.M31) / (4 * z), (delta.M23 + delta.M32) / (4 * z), z);
                    if (Data.LengthSquared(signedAxis) > 1e-20 && Data.Dot(axis, signedAxis) < 0) axis = -axis;
                }
                if (Data.LengthSquared(axis) < 1e-24) axis = Data.Up;
                axis.Normalize(); return axis * angle;
            }

            public static Vector3D PointVelocity(Vector3D centerVelocity, Vector3D angularVelocity, Vector3D point, Vector3D center)
            => centerVelocity + Data.Cross(angularVelocity, point - center);
            public static Vector3D ReferencePositionForPoint(Vector3D point, Vector3D pointLocal, MatrixD desiredOrientation)
            => point - Data.TransformNormal(pointLocal, desiredOrientation);
            public static MatrixD ReferenceOrientationForConnector(Vector3D connectorForwardLocal, Vector3D connectorUpLocal,
                Vector3D portForward, Vector3D portUp)
            {
                MatrixD local, desired;
                if (!Frame(connectorForwardLocal, connectorUpLocal, out local) || !Frame(-portForward, portUp, out desired)) return MatrixD.Identity;
                return Data.Transpose(local) * desired;
            }
            static int Axis(Vector3D direction)
            {
                double x = Math.Abs(direction.X), y = Math.Abs(direction.Y), z = Math.Abs(direction.Z);
                return x >= y && x >= z ? direction.X >= 0 ? 0 : 1 : y >= z ? direction.Y >= 0 ? 2 : 3 : direction.Z >= 0 ? 4 : 5;
            }
            static double Component(Vector3D value, int axis)
            => axis == 0 ? value.X : axis == 1 ? -value.X : axis == 2 ? value.Y : axis == 3 ? -value.Y : axis == 4 ? value.Z : -value.Z;

            public void Save(MyIni ini) { ini.Set("FlightState", "LastMass", LastFlightMass); }
            public void Load(MyIni ini)
            {
                var stored = ini.Get("FlightState", "LastMass").ToDouble(0);
                LastFlightMass = Data.Finite(stored) && stored > 0 && stored < 1e10 ? stored : 0;
            }
        }
    }
}