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

namespace AutoMiningScript
{
    public partial class Program
    {
        // Every actuator and inventory is explicitly restricted to the programmable block's grid.
        public sealed class ShipHardware
        {
            readonly Program p;
            public IMyShipController Controller;
            public IMyShipConnector Connector;
            public readonly List<IMyThrust> Thrusters = Data.CreateList<IMyThrust>();
            readonly List<IMyThrust> localThrusters = Data.CreateList<IMyThrust>();
            bool parked;
            public readonly List<IMyGyro> Gyros = Data.CreateList<IMyGyro>();
            public readonly List<IMyShipDrill> Drills = Data.CreateList<IMyShipDrill>();
            public readonly List<IMyBatteryBlock> Batteries = Data.CreateList<IMyBatteryBlock>();
            public readonly List<IMyGasTank> Tanks = Data.CreateList<IMyGasTank>();
            // Keep functional supply inventories here too: departure mass is the whole ship.
            public readonly List<IMyTerminalBlock> InventoryBlocks = Data.CreateList<IMyTerminalBlock>();
            public readonly List<IMyCameraBlock> Cameras = Data.CreateList<IMyCameraBlock>();
            readonly List<IMyFunctionalBlock> consumers = Data.CreateList<IMyFunctionalBlock>();
            readonly List<IMyShipConnector> connectors = Data.CreateList<IMyShipConnector>();
            readonly List<IMyRemoteControl> remotes = Data.CreateList<IMyRemoteControl>();
            readonly Dictionary<long, bool> consumerStates = Data.CreateDictionary<long, bool>();
            public double Radius;
            public Vector3D BodyHalfSize, BodyMin, BodyMax, DrillTipLocal, ConnectorLocal, DrillForwardLocal, DrillUpLocal;
            public Vector3D ConnectorForwardLocal, ConnectorUpLocal;
            public bool UsesHydrogen, SamplingIsolated;
            bool sampling;
            readonly HashSet<Vector3I> weighedBlocks = new HashSet<Vector3I>();
            IMyCubeGrid weighedGrid;
            Vector3I massMin, massMax, massCursor;
            double dryMass, scanningDryMass, inventoryMass, inventoryMassAt = -100, dryMassAt = -100;
            bool massScanning, massAttached, inventoryMassValid;
            public string MassProblem = "";
            public double AutoDepartureMass
            {
                get
                {
                    if (!massAttached || massScanning || dryMass <= 0 || !inventoryMassValid ||
                        p.Now - inventoryMassAt > 0.75 || p.Now - dryMassAt > 5) return 0;
                    var total = dryMass + inventoryMass;
                    return Data.Finite(total) && total > 0 && total <= 1e10 ? total : 0;
                }
            }

            public bool IsAttached
            {
                get
                {
                    for (int i = 0; i < connectors.Count; i++)
                        if (IsLocal(connectors[i]) && connectors[i].Status == MyShipConnectorStatus.Connected) return true;
                    return false;
                }
            }

            public ShipHardware(Program program) { p = program; }
            bool IsLocal(IMyTerminalBlock block) => block.CubeGrid == p.Me.CubeGrid;

            public bool Scan(out string error)
            {
                InvalidateDepartureMass();
                SetSampling(false);
                Controller = null; Connector = null;
                Thrusters.Clear(); localThrusters.Clear(); Gyros.Clear(); Drills.Clear(); Batteries.Clear(); Tanks.Clear();
                Cameras.Clear(); InventoryBlocks.Clear(); consumers.Clear(); connectors.Clear(); remotes.Clear();
                UsesHydrogen = false;
                var all = Data.CreateList<IMyTerminalBlock>();
                p.GridTerminalSystem.GetBlocksOfType(all, b => IsLocal(b));
                var groupName = p.Config.Text("Hardware", "Group", "");
                var selected = Data.CreateList<IMyTerminalBlock>();
                if (groupName.Length > 0)
                {
                    var group = p.GridTerminalSystem.GetBlockGroupWithName(groupName);
                    if (group == null) { error = L.F(L.MinerHardwareGroupMissing, groupName); return false; }
                    group.GetBlocks(selected);
                    for (int i = 0; i < selected.Count; i++)
                        if (!IsLocal(selected[i]))
                        { error = L.MinerHardwareSubgridUnsupported; return false; }
                }
                else selected.AddRange(all);
                var controllerName = p.Config.Text("Hardware", "Controller", "");
                var connectorName = p.Config.Text("Hardware", "Connector", "");
                for (int i = 0; i < all.Count; i++)
                {
                    var block = all[i];
                    var localThrust = block as IMyThrust; if (localThrust != null) localThrusters.Add(localThrust);
                    if (block.HasInventory) InventoryBlocks.Add(block);
                    var consumer = block as IMyFunctionalBlock;
                    if (consumer != null && (block is IMyGasGenerator || block is IMyProductionBlock)) consumers.Add(consumer);
                    var port = block as IMyShipConnector;
                    if (port != null) connectors.Add(port);
                    var remote = block as IMyRemoteControl;
                    if (remote != null) remotes.Add(remote);
                }
                for (int i = 0; i < selected.Count; i++)
                {
                    var block = selected[i];
                    var controller = block as IMyShipController;
                    if (controller != null && (controllerName.Length == 0 || controller.CustomName == controllerName))
                        if (Controller == null || controller.IsMainCockpit) Controller = controller;
                    var connector = block as IMyShipConnector;
                    if (connector != null && (connectorName.Length == 0 || connector.CustomName == connectorName))
                        if (Connector == null || connector.Status == MyShipConnectorStatus.Connected) Connector = connector;
                    var thrust = block as IMyThrust;
                    if (thrust != null)
                    {
                        Thrusters.Add(thrust);
                        if (thrust.BlockDefinition.SubtypeName.IndexOf("Hydrogen", Data.IgnoreCase) >= 0) UsesHydrogen = true;
                    }
                    var gyro = block as IMyGyro; if (gyro != null) Gyros.Add(gyro);
                    var drill = block as IMyShipDrill; if (drill != null) Drills.Add(drill);
                    var battery = block as IMyBatteryBlock; if (battery != null) Batteries.Add(battery);
                    var tank = block as IMyGasTank;
                    if (tank != null && tank.BlockDefinition.SubtypeName.IndexOf("Hydrogen", Data.IgnoreCase) >= 0) Tanks.Add(tank);
                    var camera = block as IMyCameraBlock;
                    if (camera != null) { camera.Enabled = true; camera.EnableRaycast = true; Cameras.Add(camera); }
                }
                UsesHydrogen = p.Config.Flag("Hardware", "UsesHydrogen", UsesHydrogen);
                if (IsAttached) ParkThrusters(true);
                if (!Healthy(out error)) return false;
                MatrixD inverse = Data.Transpose(Controller.WorldMatrix);
                var origin = Controller.GetPosition();
                ConnectorLocal = Data.TransformNormal(Connector.GetPosition() - origin, inverse);
                ConnectorForwardLocal = Data.TransformNormal(Connector.WorldMatrix.Forward, inverse);
                ConnectorUpLocal = Data.TransformNormal(Connector.WorldMatrix.Up, inverse);
                var drillCenter = Data.Zero;
                for (int i = 0; i < Drills.Count; i++) drillCenter += Drills[i].GetPosition();
                drillCenter /= Drills.Count;
                var tipOffset = p.Config.Number("Hardware", "DrillTipOffset", p.Me.CubeGrid.GridSize, 0, 20);
                DrillTipLocal = Data.TransformNormal(drillCenter + Drills[0].WorldMatrix.Forward * tipOffset - origin, inverse);
                DrillForwardLocal = Data.TransformNormal(Drills[0].WorldMatrix.Forward, inverse);
                DrillUpLocal = Data.TransformNormal(Drills[0].WorldMatrix.Up, inverse);
                UpdateBodyBounds();
                for (int i = 0; i < remotes.Count; i++) remotes[i].SetAutoPilotEnabled(false);
                error = "";
                return true;
            }

            void UpdateBodyBounds()
            {
                MatrixD inverse=Data.Transpose(Controller.WorldMatrix);var origin=Controller.GetPosition();
                Vector3D min = new Vector3D(double.MaxValue), max = new Vector3D(double.MinValue);
                var grid = p.Me.CubeGrid;
                for (int c = 0; c < 8; c++)
                {
                    var corner = new Vector3I((c & 1) == 0 ? grid.Min.X : grid.Max.X,
                        (c & 2) == 0 ? grid.Min.Y : grid.Max.Y, (c & 4) == 0 ? grid.Min.Z : grid.Max.Z);
                    Vector3D local = Data.TransformNormal(grid.GridIntegerToWorld(corner) - origin, inverse);
                    min = Vector3D.Min(min, local); max = Vector3D.Max(max, local);
                }
                BodyMin = min - new Vector3D(grid.GridSize * 0.5); BodyMax = max + new Vector3D(grid.GridSize * 0.5);
                BodyHalfSize = Vector3D.Max(new Vector3D(Math.Abs(min.X), Math.Abs(min.Y), Math.Abs(min.Z)),
                    new Vector3D(Math.Abs(max.X), Math.Abs(max.Y), Math.Abs(max.Z))) + new Vector3D(grid.GridSize * 0.5);
                Radius = Data.Max(1, Data.Length(BodyHalfSize)) + p.Config.Number("Flight", "Clearance", 1, 0.1, 20);
            }

            // CalculateShipMass includes connected grids. Read only this grid's slim blocks
            // (including armour), then add the latest complete local inventory snapshot.
            public void UpdateDepartureMass()
            {
                if (!IsAttached) { if (massAttached) InvalidateDepartureMass(); return; }
                if (p.Config.Number("Flight", "DepartureMass", 0, 0, 1e10) > 0) return;
                var grid = p.Me.CubeGrid;
                if (!massAttached || grid != weighedGrid || grid.Min != massMin || grid.Max != massMax ||
                    (!massScanning && p.Now - dryMassAt > 5))
                {
                    massAttached = true; weighedGrid = grid; massMin = grid.Min; massMax = grid.Max;
                    massCursor = massMin; dryMass = scanningDryMass = 0; weighedBlocks.Clear();
                    double x = (double)massMax.X - massMin.X + 1, y = (double)massMax.Y - massMin.Y + 1,
                        z = (double)massMax.Z - massMin.Z + 1;
                    if (x <= 0 || y <= 0 || z <= 0 || x * y * z > 1000000)
                    { massScanning = false; dryMassAt = p.Now; MassProblem = L.MinerAutoMassGridTooLarge; return; }
                    massScanning = true; MassProblem = L.MinerAutoMassScanning;
                }
                if (!massScanning) return;
                // Both a fixed cell limit and runtime headroom apply even for sparse grids.
                for (int cells = 0; cells < 128 && p.HasBudget(0.45); cells++)
                {
                    var slim = grid.GetCubeBlock(massCursor);
                    if (slim != null && slim.CubeGrid == grid && weighedBlocks.Add(slim.Position))
                    {
                        double blockMass = slim.Mass;
                        if (!Data.Finite(blockMass) || blockMass < 0 || weighedBlocks.Count > 32768)
                        { FailMass(L.MinerAutoMassInvalid); return; }
                        scanningDryMass += blockMass;
                        if (!Data.Finite(scanningDryMass) || scanningDryMass > 1e10)
                        { FailMass(L.MinerAutoMassInvalid); return; }
                    }
                    if (massCursor.X < massMax.X) massCursor.X++;
                    else if (massCursor.Y < massMax.Y) { massCursor.X = massMin.X; massCursor.Y++; }
                    else if (massCursor.Z < massMax.Z) { massCursor.X = massMin.X; massCursor.Y = massMin.Y; massCursor.Z++; }
                    else
                    {
                        if (scanningDryMass <= 0) { FailMass(L.MinerAutoMassInvalid); return; }
                        dryMass = scanningDryMass; dryMassAt = p.Now; massScanning = false;
                        MassProblem = ""; weighedBlocks.Clear(); return;
                    }
                }
            }
            void FailMass(string problem)
            { dryMass = 0; dryMassAt = p.Now; massScanning = false; weighedBlocks.Clear(); MassProblem = problem; }
            public void InvalidateDepartureMass()
            {
                dryMass = scanningDryMass = 0; dryMassAt = -100; massScanning = massAttached = false;
                weighedGrid = null; weighedBlocks.Clear(); MassProblem = L.MinerAutoMassScanning;
            }
            public void SetInventoryMass(double value, bool valid)
            {
                // The game scales inventory mass in physics by the world's cargo multiplier.
                double multiplier = p.World == null ? 1 : p.World.InventoryMultiplier;
                inventoryMassValid = valid && Data.Finite(value) && value >= 0 && value <= 1e10 &&
                    Data.Finite(multiplier) && multiplier > 0;
                inventoryMass = inventoryMassValid ? value / multiplier : 0; inventoryMassAt = p.Now;
            }
            public string DepartureMassProblem
            {
                get
                {
                    if (MassProblem.Length > 0) return MassProblem;
                    return !inventoryMassValid || p.Now - inventoryMassAt > 0.75
                        ? L.MinerAutoMassInventoryPending : L.MinerAutoMassScanning;
                }
            }

            public bool Healthy(out string error)
            {
                if (Controller == null || !Controller.IsFunctional || !IsLocal(Controller))
                { error = L.MinerWorkingControllerMissing; return false; }
                if (Connector == null || !Connector.IsFunctional || !IsLocal(Connector))
                { error = L.MinerWorkingConnectorMissing; return false; }
                if (Thrusters.Count == 0 || Gyros.Count == 0 || Drills.Count == 0 || Cameras.Count == 0 || Batteries.Count == 0)
                { error = L.MinerHardwareRequired; return false; }
                bool gyroOK = false, cameraOK = false, batteryOK = false;
                for (int i = 0; i < Gyros.Count; i++) if (Gyros[i].IsFunctional && IsLocal(Gyros[i])) gyroOK = true;
                for (int i = 0; i < Cameras.Count; i++) if (Cameras[i].IsFunctional && IsLocal(Cameras[i])) cameraOK = true;
                for (int i = 0; i < Batteries.Count; i++) if (Batteries[i].IsFunctional && IsLocal(Batteries[i])) batteryOK = true;
                if (!gyroOK || !cameraOK || !batteryOK) { error = L.MinerHardwareDamaged; return false; }
                for (int i = 0; i < Drills.Count; i++)
                    if (!Drills[i].IsFunctional || !IsLocal(Drills[i]) ||
                        Data.Dot(Drills[i].WorldMatrix.Forward, Drills[0].WorldMatrix.Forward) < 0.999)
                    { error = L.MinerDrillsMisaligned; return false; }
                var directions = new Vector3D[] { Controller.WorldMatrix.Right, Controller.WorldMatrix.Left,
                    Controller.WorldMatrix.Up, Controller.WorldMatrix.Down, Controller.WorldMatrix.Forward, Controller.WorldMatrix.Backward };
                for (int axis = 0; axis < directions.Length; axis++)
                {
                    var found = false;
                    for (int i = 0; i < Thrusters.Count; i++)
                        if (IsLocal(Thrusters[i]) && Thrusters[i].IsFunctional && Data.Dot(Thrusters[i].WorldMatrix.Backward, directions[axis]) > 0.99) found = true;
                    if (!found) { error = L.F(L.MinerThrustAxisMissing, axis); return false; }
                }
                if (UsesHydrogen && Tanks.Count == 0) { error = L.MinerHydrogenTankMissing; return false; }
                error = ""; return true;
            }

            public void Release()
            {
                for (int i = 0; i < Thrusters.Count; i++) if (IsLocal(Thrusters[i])) Thrusters[i].ThrustOverridePercentage = 0;
                for (int i = 0; i < Gyros.Count; i++) if (IsLocal(Gyros[i]))
                { Gyros[i].Pitch = 0; Gyros[i].Yaw = 0; Gyros[i].Roll = 0; Gyros[i].GyroOverride = false; }
                SetDrills(false);
                SetSampling(false);
                if (Controller != null && IsLocal(Controller)) Controller.DampenersOverride = true;
                for (int i = 0; i < remotes.Count; i++) if (IsLocal(remotes[i])) remotes[i].SetAutoPilotEnabled(false);
            }

            public void SetDrills(bool enabled)
            {
                for (int i = 0; i < Drills.Count; i++) if (IsLocal(Drills[i])) Drills[i].Enabled = enabled;
            }

            public void ParkThrusters(bool park)
            {
                if (!park && !parked) return;
                foreach (var thrust in localThrusters) if (IsLocal(thrust))
                { thrust.ThrustOverridePercentage = 0; thrust.Enabled = !park; }
                parked = park;
            }

            // Geometry does not consume raycast charge. The API must still accept a ray.
            public bool CameraCovers(IMyCameraBlock camera, Vector3D target)
            {
                if (!IsLocal(camera) || !camera.IsWorking) return false;
                var ray = Data.TransformNormal(target-camera.GetPosition(), Data.Transpose(camera.WorldMatrix));
                double cone = camera.RaycastConeLimit*Math.PI/180, limit = camera.RaycastDistanceLimit;
                return (limit<=0 || Data.LengthSquared(ray)<=limit*limit) && (cone<=0 ||
                    Math.Abs(Math.Atan2(ray.X,-ray.Z))<=cone && Math.Abs(Math.Atan2(ray.Y,Math.Sqrt(ray.X*ray.X+ray.Z*ray.Z)))<=cone);
            }

            public MatrixD CameraAttitude(Vector3D target, Vector3D up)
            {
                var current = Controller.WorldMatrix; IMyCameraBlock best = null; double score = -2;
                foreach (var camera in Cameras)
                {
                    if (!IsLocal(camera) || !camera.IsWorking) continue;
                    if (CameraCovers(camera,target)) return current;
                    var alignment = Data.Dot(camera.WorldMatrix.Forward,Data.Unit(target-camera.GetPosition(),current.Forward));
                    if (alignment>score) { score=alignment; best=camera; }
                }
                return best==null ? current : current.GetOrientation()*Data.Transpose(best.WorldMatrix.GetOrientation())*
                    Data.Frame(Data.Zero,Data.Unit(target-best.GetPosition(),current.Forward),up);
            }

            public void SetSampling(bool enabled)
            {
                if (!enabled)
                {
                    if (sampling)
                        for (int i = 0; i < consumers.Count; i++)
                        {
                            bool state;
                            if (IsLocal(consumers[i]) && consumerStates.TryGetValue(consumers[i].EntityId, out state)) consumers[i].Enabled = state;
                        }
                    consumerStates.Clear(); sampling = false; SamplingIsolated = false; return;
                }
                if (!sampling)
                    for (int i = 0; i < consumers.Count; i++) consumerStates[consumers[i].EntityId] = consumers[i].Enabled;
                sampling = true;
                SamplingIsolated = true;
                for (int i = 0; i < consumers.Count; i++)
                {
                    if (!IsLocal(consumers[i])) { SamplingIsolated = false; continue; }
                    consumers[i].Enabled = false;
                    if (consumers[i].Enabled) SamplingIsolated = false;
                }
                for (int i = 0; i < connectors.Count; i++)
                {
                    if (!IsLocal(connectors[i])) { SamplingIsolated = false; continue; }
                    connectors[i].ThrowOut = false;
                    if (connectors[i].ThrowOut || connectors[i].Status == MyShipConnectorStatus.Connected) SamplingIsolated = false;
                }
            }

            public void PrepareFlight()
            {
                parked = true; ParkThrusters(false);
                SetCharging(false); SetRefilling(false);
                for (int i = 0; i < Cameras.Count; i++) if (IsLocal(Cameras[i])) {Cameras[i].Enabled=true;Cameras[i].EnableRaycast=true;}
                for (int i = 0; i < Thrusters.Count; i++) if (IsLocal(Thrusters[i])) Thrusters[i].Enabled = true;
                for (int i = 0; i < Gyros.Count; i++) if (IsLocal(Gyros[i])) Gyros[i].Enabled = true;
                for (int i = 0; i < connectors.Count; i++) if (IsLocal(connectors[i])) connectors[i].ThrowOut = false;
                for (int i = 0; i < remotes.Count; i++) if (IsLocal(remotes[i])) remotes[i].SetAutoPilotEnabled(false);
            }
            public void SetCharging(bool enabled)
            {
                IMyBatteryBlock keeper = null;
                if (enabled)
                    for (int i = 0; i < Batteries.Count; i++)
                        if (IsLocal(Batteries[i]) && Batteries[i].IsFunctional &&
                            (keeper == null || Batteries[i].CurrentStoredPower > keeper.CurrentStoredPower)) keeper = Batteries[i];
                for (int i = 0; i < Batteries.Count; i++) if (IsLocal(Batteries[i]))
                    Batteries[i].ChargeMode = enabled && Batteries[i] != keeper ? ChargeMode.Recharge : ChargeMode.Auto;
            }
            public void SetRefilling(bool enabled)
            {
                for (int i = 0; i < Tanks.Count; i++) if (IsLocal(Tanks[i])) Tanks[i].Stockpile = enabled;
            }
        }
    }
}