AutoMiningScript

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;
            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>();
            Vector3D scanOrigin, scanDirection, scanRight, scanUp;
            MatrixD scanBasis, clearBasis;
            Vector3D clearOrigin, clearDirection;
            double scanHalfWidth, scanHalfHeight, clearLeading;
            double scanLength, scanStarted, scanCompleted, previousScan = -100;
            int scanIndex;
            bool scanning, scanFailed, scanBlocked;
            Vector3D obstacle;
            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;
                detourAttempts = 0; ScanClearance = 0; Blocked = false; Problem = ""; breadcrumbs.Clear();
            }
            public void Release() { h.Release(); ResetRoute(); ClearDockContact(); }

            void Refresh()
            {
                basis = h.Controller.WorldMatrix;
                gravity = h.Controller.GetNaturalGravity();
                bool attached = h.IsAttached;
                if (!attached)
                {
                    mass = h.Controller.CalculateShipMass().PhysicalMass;
                    if (Finite(mass) && mass > 0) LastFlightMass = mass;
                }
                else
                {
                    double 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));
                double worst = double.MaxValue;
                double hoverUse = 1 - p.Config.Number("Flight", "HoverReserve", 0.3, 0.1, 0.8);
                for (int axis = 0; axis < 6; axis++)
                {
                    double g = Component(localGravity, axis);
                    double 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)
            {
                Blocked = false; Problem = ""; holdActive = false;
                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);
                Vector3D position = h.Controller.GetPosition();
                var velocities = h.Controller.GetShipVelocities();
                Vector3D referenceVelocity = velocities.LinearVelocity + Vector3D.Cross(velocities.AngularVelocity, position - h.Controller.CenterOfMass);
                Vector3D relativeVelocity = referenceVelocity - targetVelocity;
                if (!routeKnown || Vector3D.DistanceSquared(routeTarget, target) > Math.Max(100, h.Radius * h.Radius * 4))
                {
                    routeTarget = target; routeKnown = true; hasDetour = false; detourAttempts = 0;
                    scanning = false; ScanClearance = 0; scanBlocked = false;
                }
                Vector3D activeTarget = hasDetour ? detour : target;
                Vector3D displacement = activeTarget - position;
                double distance = displacement.Length();
                Vector3D 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;
                }
                double usableBrake = Math.Max(0.05, BrakeAcceleration * 0.65);
                double 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)
                {
                    double currentSpeed = dockGrid != 0 ? relativeVelocity.Length() : referenceVelocity.Length();
                    double leading = LeadingExtent(direction);
                    double 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));
                    scanDistance = Math.Min(scanDistance, distance + leading + 0.2);
                    ScanEnvelope(position, direction, Math.Min(2000, scanDistance));
                    if (scanBlocked)
                    {
                        desiredSpeed = 0;
                        Problem = L.MinerObstacleBraking;
                        if (referenceVelocity.Length() < 0.3)
                        {
                            if (detourAttempts < 4 && dockGrid == 0)
                            {
                                Vector3D right = Vector3D.Cross(direction, desired.Up);
                                if (right.LengthSquared() < 0.01) right = desired.Right; else right.Normalize();
                                Vector3D 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 = true; Problem = L.MinerDetoursBlocked; }
                        }
                    }
                    else
                    {
                        double traveled = Math.Max(0, Vector3D.Dot(position - clearOrigin, clearDirection));
                        double clear = Math.Max(0, ScanClearance - traveled - clearLeading - currentSpeed * 0.5);
                        desiredSpeed = Math.Min(desiredSpeed, Math.Sqrt(2 * usableBrake * clear));
                        if (clear < 0.2) Problem = scanFailed ? L.MinerCameraUnavailable : L.MinerEnvelopeScanPending;
                    }
                }
                AttitudeError = ApplyAttitude(desired, velocities.AngularVelocity);
                if (AttitudeError > 0.35) desiredSpeed = Math.Min(desiredSpeed, 0.5);
                if (AttitudeError > 1) desiredSpeed = 0;
                Vector3D desiredVelocity = targetVelocity + direction * desiredSpeed;
                Vector3D desiredAcceleration = (desiredVelocity - referenceVelocity) * p.Config.Number("Flight", "VelocityGain", 1.6, 0.2, 5);
                ApplyAcceleration(desiredAcceleration);
                bool arrived = distance <= tolerance && relativeVelocity.Length() <= Math.Max(0.08, Math.Min(0.3, speed * 0.2)) && AttitudeError < 0.035;
                if (arrived && hasDetour)
                { hasDetour = false; scanning = false; ScanClearance = 0; return false; }
                if (scan && ScanClearance > h.Radius * 2 && (breadcrumbs.Count == 0 || Vector3D.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.
                double limit = double.MaxValue;
                for (int axis = 0; axis < 6; axis++)
                {
                    double component = Component(direction, axis);
                    double 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);
                    double 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)
            {
                Vector3D error = RotationError(basis, desired);
                double angle = error.Length();
                double maxRate = p.Config.Number("Flight", "MaxAngularRate", 0.6, 0.05, 2);
                Vector3D targetRate = Vector3D.Zero;
                double elapsed = p.Now - previousOrientationAt;
                if (orientationKnown && elapsed > 0.0001 && elapsed < 0.3)
                {
                    Vector3D 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;
                Vector3D 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));
                    // Keen MyGyro stores physical (X,Y,Z); the terminal API reverses Yaw and Roll.
                    gyro.Pitch = (float)local.X; gyro.Yaw = (float)-local.Y; gyro.Roll = (float)-local.Z;
                    gyro.GyroOverride = true;
                }
                return angle;
            }

            void ScanEnvelope(Vector3D origin, Vector3D direction, double distance)
            {
                double turn = Vector3D.Dot(direction, scanDirection);
                bool stale = p.Now - scanCompleted > 1.5;
                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 = distance;
                    scanBasis = basis.GetOrientation();
                    scanRight = Vector3D.Cross(direction, basis.Up);
                    if (scanRight.LengthSquared() < 0.01) scanRight = basis.Right; else scanRight.Normalize();
                    scanUp = Vector3D.Normalize(Vector3D.Cross(scanRight, direction));
                    scanHalfWidth = ProjectedExtent(scanRight); scanHalfHeight = ProjectedExtent(scanUp);
                    scanIndex = 0; scanStarted = p.Now; previousScan = p.Now; scanning = true; scanFailed = false; scanBlocked = false;
                }
                if (!scanning) return;
                if (Vector3D.Dot(direction, scanDirection) < 0.98 || Vector3D.DistanceSquared(origin, scanOrigin) > h.Radius * h.Radius || RotationError(scanBasis, basis).Length() > 0.035)
                { scanning = false; ScanClearance = 0; return; }
                int budget = p.Config.Integer("Flight", "RaysPerTick", 2, 1, 4);
                for (int ray = 0; ray < budget && scanIndex < 18; ray++)
                {
                    int lane = scanIndex % 9;
                    double along = scanIndex < 9 ? Math.Min(scanLength, Math.Max(LeadingExtent(scanDirection) + 0.2, scanLength * 0.35)) : scanLength;
                    Vector3D lateral = Vector3D.Zero;
                    if (lane > 0)
                    {
                        double 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);
                    }
                    Vector3D endpoint = scanOrigin + scanDirection * along + lateral;
                    IMyCameraBlock camera = FindCamera(endpoint);
                    if (camera == null)
                    {
                        scanFailed = true;
                        if (p.Now - scanStarted > 3) { scanning = false; ScanClearance = 0; }
                        return;
                    }
                    var hit = camera.Raycast(endpoint);
                    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 &&
                            Vector3D.DistanceSquared(point, dockContact) <= dockRadius * dockRadius;
                        if (!permittedContact)
                        {
                            obstacle = point; scanBlocked = true; scanning = false; ScanClearance = 0; return;
                        }
                    }
                    else if (!hit.IsEmpty())
                    {
                        // A self-hit does not certify the space beyond the camera.
                        scanFailed = true; scanning = false; ScanClearance = 0; return;
                    }
                    scanIndex++;
                }
                if (scanIndex == 18)
                { scanning = false; ScanClearance = scanLength; clearOrigin = scanOrigin; clearDirection = scanDirection; clearBasis = scanBasis; clearLeading = LeadingExtent(scanDirection); scanCompleted = p.Now; scanFailed = false; }
            }

            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(Vector3D target)
            {
                IMyCameraBlock result = null;
                double best = -1;
                for (int i = 0; i < h.Cameras.Count; i++)
                {
                    var camera = h.Cameras[i];
                    if (!camera.IsWorking || camera.CubeGrid != p.Me.CubeGrid || !camera.CanScan(target)) continue;
                    if (camera.AvailableScanRange > best) { result = camera; best = camera.AvailableScanRange; }
                }
                return result;
            }

            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 * Vector3D.Dot(up, forward);
                if (up.LengthSquared() < 0.000001) return false;
                up.Normalize(); matrix = MatrixD.CreateWorld(Vector3D.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();
                double cos = Math.Max(-1, Math.Min(1, (delta.M11 + delta.M22 + delta.M33 - 1) * 0.5));
                double angle = Math.Acos(cos);
                if (angle < 0.0000001) return Vector3D.Zero;
                Vector3D axis = new Vector3D(delta.M23 - delta.M32, delta.M31 - delta.M13, delta.M12 - delta.M21);
                if (Math.PI - angle < 0.0001)
                {
                    Vector3D signedAxis = axis;
                    double x = Math.Sqrt(Math.Max(0, (delta.M11 + 1) * 0.5));
                    double y = Math.Sqrt(Math.Max(0, (delta.M22 + 1) * 0.5));
                    double 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 && Vector3D.Dot(axis, signedAxis) < 0) axis = -axis;
                }
                if (axis.LengthSquared() < 1e-24) axis = Vector3D.Up;
                axis.Normalize(); return axis * angle;
            }

            public static Vector3D PointVelocity(Vector3D centerVelocity, Vector3D angularVelocity, Vector3D point, Vector3D center)
            { return centerVelocity + Vector3D.Cross(angularVelocity, point - center); }
            public static Vector3D ReferencePositionForPoint(Vector3D point, Vector3D pointLocal, MatrixD desiredOrientation)
            { return 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)
            { return 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) { return !double.IsNaN(value) && !double.IsInfinity(value); }
            static bool Finite(Vector3D value) { return Finite(value.X) && Finite(value.Y) && Finite(value.Z); }

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