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
        {
            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;
            int detourAttempts;
            readonly List<Vector3D> breadcrumbs = new List<Vector3D>();
            readonly HashSet<long> occludedCameras = new HashSet<long>();
            readonly HashSet<long> scanCameras = new HashSet<long>();
            Vector3D scanOrigin, scanDirection, scanRight, scanUp;
            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;
            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 = Math.Max(0.1, radius); }
            public void ClearDockContact() { dockGrid = 0; }

            public void ResetRoute()
            {
                routeKnown = hasDetour = 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 (Finite(mass) && mass > 0) LastFlightMass = mass;
                }
                else
                {
                    var configured = p.Config.Number("Flight", "DepartureMass", 0, 0, 1e10);
                    mass = configured > 0 ? configured : h.AutoDepartureMass;
                }
                mass = Finite(mass) && mass > 0 ? mass : 0;
                for (int i = 0; i < 6; i++) force[i] = 0;
                MatrixD inverse = MatrixD.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 = Vector3D.TransformNormal(thruster.WorldMatrix.Backward, inverse);
                    int axis = Axis(direction);
                    force[axis] += Math.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 = Vector3D.TransformNormal(gravity, MatrixD.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 = Math.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)
            {
                Blocked = false; Problem = ""; holdActive = false; CommandSpeed = 0;
                if (h.Controller == null) { Blocked = true; Problem = L.MinerMissingController; return false; }
                if (!Finite(target) || !Finite(targetVelocity) || !Finite(forward) || !Finite(up) || !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 = Math.Max(1.0 / 120, Math.Min(0.25, dt));
                speed = Math.Max(0, Math.Min(95, speed)); tolerance = Math.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) > Math.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 = false; detourAttempts = 0;
                    // 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;
                if(scan && allowDetour && TrafficAvoid && p.Now-TrafficAt<3 && Data.Distance(position,TrafficPoint)>tolerance)
                {detour=TrafficPoint;hasDetour=true;activeTarget=detour;}
                var displacement = activeTarget - position;
                var distance = displacement.Length();
                // Only open-space navigation opts in. Docking and bore retreat retain their
                // calibrated attitude; a detour must face its actual leg, not the final target.
                if (faceTravel && distance > tolerance)
                    Frame(displacement, Data.Perpendicular(displacement, up), out desired);
                else if (faceTravel) desired = basis.GetOrientation();
                if (faceTravel && !CanSupport(desired.Forward, desired.Up, 0.1))
                {
                    var failure = Problem;
                    Hold(dt); Blocked = true; Problem = failure;
                    return false;
                }
                var direction = distance > 0.001 ? displacement / distance : desired.Forward;
                Acceleration = AvailableAcceleration(direction);
                Vector3D brakeDirection = relativeVelocity.LengthSquared() > 0.01 ? -Vector3D.Normalize(relativeVelocity) : -direction;
                BrakeAcceleration = AvailableAcceleration(brakeDirection);
                ThrustMargin = Math.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 = Math.Max(0.05, BrakeAcceleration * 0.65);
                var desiredSpeed = Math.Min(speed, Math.Sqrt(2 * usableBrake * Math.Max(0, distance - tolerance * 0.5)));
                desiredSpeed = Math.Min(desiredSpeed, distance * p.Config.Number("Flight", "PositionGain", 0.8, 0.1, 3));
                if (scan && distance > tolerance)
                {
                    var currentSpeed = dockGrid != 0 ? relativeVelocity.Length() : referenceVelocity.Length();
                    var leading = LeadingExtent(direction);
                    var scanDistance = Math.Max(h.Radius * 3 + 5,
                        currentSpeed * currentSpeed / (2 * usableBrake) + currentSpeed * 2 + h.Radius * 2 + 5);
                    scanDistance = Math.Max(scanDistance, Math.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, Math.Min(2000, scanDistance), distance + leading + 0.2);
                    var traveled = Math.Max(0, Data.Dot(position - clearOrigin, clearDirection));
                    var clear = Math.Max(0, ScanClearance - traveled - clearLeading - currentSpeed * 0.5);
                    desiredSpeed = Math.Min(desiredSpeed, Math.Sqrt(2 * usableBrake * clear));
                    if(scanBlocked)Problem=obstacleLocated?L.F(L.CameraObstacle,obstacleName,Math.Max(0,Data.Dot(obstacle-position,direction)-leading-.25),obstacleRay):L.F(L.CameraObstacleUnknown,obstacleName);
                    if (scanBlocked && clear < 0.2)
                    {
                        desiredSpeed = 0;
                        Problem = L.MinerObstacleBraking+" / "+Problem;
                        if (referenceVelocity.Length() < 0.3)
                        {
                            if (!scanning && detourAttempts < 4 && dockGrid == 0 && allowDetour)
                            {
                                var right = Data.Cross(direction, desired.Up);
                                if (right.LengthSquared() < 0.01) right = desired.Right; else right.Normalize();
                                var side = detourAttempts == 0 ? right : detourAttempts == 1 ? -right :
                                    detourAttempts == 2 ? desired.Up : -desired.Up;
                                // Candidate motion is itself scanned; no assumed-free sideways jump.
                                detour = position + side * (h.Radius * 3 + 5) + direction * h.Radius;
                                hasDetour = true; detourAttempts++; scanBlocked = false; scanning = false; ScanClearance = 0;
                            }
                            else { Blocked = !scanning; }
                        }
                    }
                    else if (!scanBlocked && clear < 0.2)
                    {
                        Problem = scanFailed ? (cameraProblem.Length>0?cameraProblem:L.MinerCameraUnavailable) : L.MinerEnvelopeScanPending+" "+scanIndex+"/18";
                    }
                }
                AttitudeError = ApplyAttitude(desired, velocities.AngularVelocity);
                if (AttitudeError > 0.35) desiredSpeed = Math.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) * p.Config.Number("Flight", "VelocityGain", 1.6, 0.2, 5);
                ApplyAcceleration(desiredAcceleration);
                var arrived = distance <= tolerance && relativeVelocity.Length() <= Math.Max(0.08, Math.Min(0.3, speed * 0.2)) && AttitudeError < 0.035 && !(scan && TrafficWait && p.Now-TrafficAt<3);
                if (arrived && hasDetour)
                { hasDetour = false; scanning = false; ScanClearance = 0; return false; }
                if (scan && ScanClearance > h.Radius * 2 && (breadcrumbs.Count == 0 || Data.DistanceSquared(breadcrumbs[breadcrumbs.Count - 1], position) > 100))
                {
                    if (breadcrumbs.Count >= 128) breadcrumbs.RemoveAt(0);
                    breadcrumbs.Add(position);
                }
                return arrived;
            }

            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();
                Vector3D offset = holdPosition - h.Controller.GetPosition();
                if (offset.Length() > 2) offset = Vector3D.Normalize(offset) * 2;
                ApplyAcceleration(offset * 0.8 - velocity.LinearVelocity * 1.6);
                ApplyAttitude(basis, velocity.AngularVelocity);
            }

            double AvailableAcceleration(Vector3D worldDirection)
            {
                Vector3D direction = Vector3D.TransformNormal(worldDirection, MatrixD.Transpose(basis));
                Vector3D localGravity = Vector3D.TransformNormal(gravity, MatrixD.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] / Math.Max(1, mass) + Component(localGravity, axis);
                    if (available < -0.01 && component >= 0) return 0;
                    if (component > 0.0001) limit = Math.Min(limit, available / component);
                }
                return Math.Max(0, Math.Min(1000, limit));
            }

            void ApplyAcceleration(Vector3D acceleration)
            {
                h.Controller.DampenersOverride = false;
                Vector3D requested = Vector3D.TransformNormal((acceleration - gravity) * mass, MatrixD.Transpose(basis));
                // Clamp independently per body axis so saturation does not discard gravity support.
                requested.X = Math.Max(-force[1], Math.Min(force[0], requested.X));
                requested.Y = Math.Max(-force[3], Math.Min(force[2], requested.Y));
                requested.Z = Math.Max(-force[5], Math.Min(force[4], requested.Z));
                MatrixD inverse = MatrixD.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 = Vector3D.TransformNormal(thruster.WorldMatrix.Backward, inverse);
                    int axis = Axis(direction);
                    var ratio = force[axis] > 0 ? Math.Max(0, Component(requested, axis)) / force[axis] : 0;
                    thruster.ThrustOverridePercentage = (float)Math.Max(0, Math.Min(1, ratio));
                }
            }

            double ApplyAttitude(MatrixD desired, Vector3D angularVelocity)
            {
                var error = RotationError(basis, desired);
                var angle = error.Length();
                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 (change.Length() < 0.05) targetRate = change / elapsed;
                    if (targetRate.Length() > 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 (requested.Length() > 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 = Vector3D.TransformNormal(requested, MatrixD.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 && RotationError(clearBasis, basis).Length() > 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 = Math.Min(distance, scanLimit); scanTravelLimit = travelLimit; scanSafeLength=scanLength; scanObstacleFound=false;
                    scanBasis = basis.GetOrientation();
                    scanRight = Data.Cross(direction, basis.Up);
                    if (scanRight.LengthSquared() < 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 || RotationError(scanBasis, basis).Length() > 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 ? Math.Min(scanLength, Math.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 = Math.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 && 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 && Finite(point);
                            var limit=located?Math.Max(0,Data.Dot(point-scanOrigin,scanDirection)-.25):0;
                            if(!scanObstacleFound || limit<scanSafeLength)
                            {obstacle=point;obstacleLocated=located;obstacleName=string.IsNullOrEmpty(hit.Name)?hit.EntityId.ToString():hit.Name;obstacleRay=scanIndex+1;}
                            scanSafeLength=Math.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=Math.Min(ScanClearance,Math.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 = Math.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)+Math.Max(1,h.Radius*0.5);
                scanLimit=scanWaitingForCharge?Math.Min(scanLength,Math.Max(minimum,scanLength*0.65)):2000;
                scanning=false;ScanClearance=0;scanFailed=true;
            }

            double LeadingExtent(Vector3D direction)
            {
                Vector3D local = Vector3D.TransformNormal(direction, MatrixD.Transpose(basis));
                return Math.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 = Vector3D.TransformNormal(direction, MatrixD.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=Math.Max(0.001,ray.Length());
                    double score=2*Data.Dot(ray/length,camera.WorldMatrix.Forward)-length/Math.Max(1,scanLength);
                    if (score > best) { result = camera; best = score; selected=point; }
                }
                if(result==null)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=Math.Max(0,scanLength-Data.Dot(target-scanOrigin,scanDirection));
                MatrixD inverse=MatrixD.Transpose(camera.WorldMatrix);
                Vector3D q=Vector3D.TransformNormal(target-camera.GetPosition(),inverse),d=Vector3D.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*Math.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=Math.Max(low,edge);else high=Math.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=Vector3D.TransformNormal(scanDirection,MatrixD.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,Math.Min(18,scanIndex+1),Math.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=relative.Length();
                    Vector3D ray=Vector3D.TransformNormal(relative,MatrixD.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 && ray.LengthSquared()>0.000001 && (Math.Abs(pitch)>cone || Math.Abs(yaw)>cone))
                    {
                        var error=Math.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,Math.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 (!Finite(forward) || !Finite(up) || forward.LengthSquared() < 0.000001 || up.LengthSquared() < 0.000001) return false;
                forward.Normalize(); up -= forward * Data.Dot(up, forward);
                if (up.LengthSquared() < 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 = MatrixD.Transpose(current.GetOrientation()) * desired.GetOrientation();
                var cos = Math.Max(-1, Math.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(Math.Max(0, (delta.M11 + 1) * 0.5));
                    var y = Math.Sqrt(Math.Max(0, (delta.M22 + 1) * 0.5));
                    var z = Math.Sqrt(Math.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 (signedAxis.LengthSquared() > 1e-20 && Data.Dot(axis, signedAxis) < 0) axis = -axis;
                }
                if (axis.LengthSquared() < 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 - Vector3D.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 MatrixD.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;
            static bool Finite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
            static bool Finite(Vector3D value) => Finite(value.X) && Finite(value.Y) && Finite(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 = Finite(stored) && stored > 0 && stored < 1e10 ? stored : 0;
            }
        }
    }
}