#region mdk preserve

// Sam's Autopilot Manager

public static string VERSION = "2.6.7";

//

// Documentation: http://steamcommunity.com/sharedfiles/filedetails/?id=1653875433

//

// Owner: Sam

// Contributors: SCBionicle

//

// Latest changes:

// - Fix block attribute capitalization.

// - Allows selecting primary connector for docking with tag MAIN.

// - Hacked around KEEN bug with surface GetText.

// - Added Orbital dock. "ADD ORBIT" will add a dock at Orbit imeadiatelly above ship.

// - Hydrogen tanks can now be considered CARGO too, just add the tag [SAM CARGO];

// - when using the ADD GPS command, dont add the current dock if docked;

// - better error log when no Remote Controls added;

// - tuned approach calculations to eliminate crashes against the dock;

// - halfed the docking distances;

// - made the obstacle finder cause the ship to fly further away from obstacles to reduce crashes against mountains;

// - fixed a bug that could cause ships to crash against the docks;

// - added support for sending remote commands to ships;

// - support for FORCE Tag in batteries and tanks to force charging/filling upon docking;

// - SAMv2 will now disable dampeners after docking in order to avoid thrusters from firing while docked (happens when docked to ships). This can be disabled with PB Tag NODAMPENERS;

// Change the tag used to identify blocks

public static string TAG = "SAM";

// -------------------------------------------------------

// Update at your own peril.

// -------------------------------------------------------

private static float HORIZONT_CHECK_DISTANCE = 2000.0f;

private static float MAX_SPEED = 95.0f;

private static float APPROACH_DISTANCE = 10.0f;

private static float DOCK_DISTANCE = 5.0f;

private static float UNDOCK_DISTANCE = 10.0f;

private static float DOCKING_SPEED = 2.5f;

private static float APPROACH_SAFE_DISTANCE = 5.0f;

private static float TAXIING_SPEED = 10.0f;

private static float COLLISION_CORRECTION_ANGLE = (float)Math.PI / 7.5f;

private static string ADVERT_ID = "SAMv2";

private static string ADVERT_ID_VER = "SAMv2V";

// private static int ADVERT_VERSION = 2;

private static string STORAGE_VERSION = "deadbeef";

// -------------------------------------------------------

// Avoid touching anything below this. Things will break.

// -------------------------------------------------------

private static string MSG_ALIGNING = "aligning...";

private static string MSG_DOCKING = "docking...";

private static string MSG_UNDOCKING = "undocking...";

private static string MSG_CONVERGING = "converging...";

private static string MSG_APPROACHING = "approaching...";

private static string MSG_NAVIGATING = "navigating...";

private static string MSG_TAXIING = "taxiing...";

private static string MSG_NAVIGATION_TO = "Navigating to ";

private static string MSG_NAVIGATION_TO_WAYPOINT = "Navigating to coordinates";

private static string MSG_NAVIGATION_SUCCESSFUL = "Navigation successful!";

private static string MSG_NO_CONNECTORS_AVAILABLE = "No connectors available!";

private static string MSG_FAILED_TO_DOCK = "Failed to dock!";

private static string MSG_DOCKING_SUCCESSFUL = "Docking successful!";

private static string MSG_NO_REMOTE_CONTROL = "No Remote Control!";

private static string MSG_INVALID_GPS_TYPE = "Invalid GPS format!";

private static float HORIZONT_CHECK_ANGLE_LIMIT = (float)Math.PI / 32.0f;

private static float HORIZONT_CHECK_ANGLE_STEP = (float)Math.PI / 75.0f;

private static float HORIZONT_MAX_UP_ANGLE = (float)Math.PI;

private static float COLLISION_DISABLE_RADIUS_MULTIPLIER = 2.0f;

private static float IDLE_POWER = 0.0000001f;

private static double TICK_TIME = 0.16666f;

private static double GYRO_GAIN = 1.0;

private static double GYRO_MAX_ANGULAR_VELOCITY = Math.PI;

private static float GUIDANCE_MIN_AIM_DISTANCE = 0.5f;

private static float DISTANCE_TO_GROUND_IGNORE_PLANET = 1.2f * HORIZONT_CHECK_DISTANCE;

private static int DOCK_ATTEMPTS = 5;

private static int LOG_MAX_LINES = 60;

private static string CMD_TAG = TAG + "CMD";

private static string CMD_RES_TAG = TAG + "CMDRES";

#endregion

public static class Raytracer {

public enum Result { NotRun, NoHit, Hit };

public static Vector3D hitPosition;

public static MyDetectedEntityInfo hit;

public static Result Trace(ref Vector3D target, bool ignorePlanet) {

foreach(IMyCameraBlock camera in GridBlocks.cameraBlocks) {

if(!camera.CanScan(target)) {

continue;

}

hit = camera.Raycast(target);

if(hit.IsEmpty()) {

return Result.NoHit;

}

if(hit.EntityId == GridBlocks.MasterProgrammableBlock.CubeGrid.EntityId) {

continue;

}

switch(hit.Type) {

case MyDetectedEntityType.Planet:

if(ignorePlanet) {

return Result.NoHit;

}

goto case MyDetectedEntityType.SmallGrid;

case MyDetectedEntityType.Asteroid:

case MyDetectedEntityType.LargeGrid:

case MyDetectedEntityType.SmallGrid:

hitPosition = hit.HitPosition.Value;

return Result.Hit;

default:

return Result.NoHit;

}

}

return Result.NotRun;

}

}

public static class Situation {

public static Vector3D position;

public static Vector3D linearVelocity;

public static double elevationVelocity;

public static Vector3D naturalGravity;

public static bool planetDetected;

public static Vector3D planetCenter = new Vector3D();

public static bool inGravity;

public static double distanceToGround;

public static double radius;

public static float mass;

public static Vector3D gravityUpVector;

public static Vector3D gravityDownVector;

public static Vector3D upVector;

public static Vector3D forwardVector;

public static Vector3D backwardVector;

public static Vector3D downVector;

public static Vector3D rightVector;

public static Vector3D leftVector;

public static MatrixD orientation;

public static Vector3D gridForwardVect;

public static Vector3D gridUpVect;

public static Vector3D gridLeftVect;

private static Dictionary<Base6Directions.Direction, double> maxThrust = new Dictionary<Base6Directions.Direction, double>() { { Base6Directions.Direction.Backward, 0 }, { Base6Directions.Direction.Down, 0 }, { Base6Directions.Direction.Forward, 0 }, { Base6Directions.Direction.Left, 0 }, { Base6Directions.Direction.Right, 0 }, { Base6Directions.Direction.Up, 0 }, };

private static double forwardChange, upChange, leftChange;

private static Vector3D maxT;

public static double GetMaxThrust(Vector3D dir) {

// return MAX_TRUST_UNDERESTIMATE_PERCENTAGE * maxThrust.MinBy(kvp => (float)kvp.Value).Value;

forwardChange = Vector3D.Dot(dir, Situation.gridForwardVect);

upChange = Vector3D.Dot(dir, Situation.gridUpVect);

leftChange = Vector3D.Dot(dir, Situation.gridLeftVect);

maxT = new Vector3D();

maxT.X = forwardChange * maxThrust[(forwardChange > 0) ? Base6Directions.Direction.Forward : Base6Directions.Direction.Backward];

maxT.Y = upChange * maxThrust[(upChange > 0) ? Base6Directions.Direction.Up : Base6Directions.Direction.Down];

maxT.Z = leftChange * maxThrust[(leftChange > 0) ? Base6Directions.Direction.Left : Base6Directions.Direction.Right];

return maxT.Length();

}

public static void RefreshParameters() {

foreach(Base6Directions.Direction dir in maxThrust.Keys.ToList()) {

maxThrust[dir] = 0;

}

foreach(IMyThrust thruster in GridBlocks.thrustBlocks) {

if(!thruster.IsWorking) {

continue;

}

maxThrust[thruster.Orientation.Forward] += thruster.MaxEffectiveThrust;

}

//var myList = maxThrust.ToList();

//myList.Sort((pair1, pair2) => pair1.Value.CompareTo(pair2.Value));

//for(int i=0; i<myList.Count-2; ++i) {

// maxThrust[myList[i].Key] = myList[i].Value / 2.0f;

//}

gridForwardVect = RemoteControl.block.CubeGrid.WorldMatrix.GetDirectionVector(Base6Directions.Direction.Forward);

gridUpVect = RemoteControl.block.CubeGrid.WorldMatrix.GetDirectionVector(Base6Directions.Direction.Up);

gridLeftVect = RemoteControl.block.CubeGrid.WorldMatrix.GetDirectionVector(Base6Directions.Direction.Left);

mass = RemoteControl.block.CalculateShipMass().PhysicalMass;

position = RemoteControl.block.CenterOfMass;

orientation = RemoteControl.block.WorldMatrix.GetOrientation();

radius = RemoteControl.block.CubeGrid.WorldVolume.Radius;

forwardVector = RemoteControl.block.WorldMatrix.Forward;

backwardVector = RemoteControl.block.WorldMatrix.Backward;

rightVector = RemoteControl.block.WorldMatrix.Right;

leftVector = RemoteControl.block.WorldMatrix.Left;

upVector = RemoteControl.block.WorldMatrix.Up;

downVector = RemoteControl.block.WorldMatrix.Down;

linearVelocity = RemoteControl.block.GetShipVelocities().LinearVelocity;

elevationVelocity = Vector3D.Dot(linearVelocity, upVector);

planetDetected = RemoteControl.block.TryGetPlanetPosition(out planetCenter);

naturalGravity = RemoteControl.block.GetNaturalGravity();

inGravity = naturalGravity.Length() >= 0.5;

if(inGravity) {

RemoteControl.block.TryGetPlanetElevation(MyPlanetElevation.Surface, out distanceToGround);

gravityDownVector = Vector3D.Normalize(naturalGravity);

gravityUpVector = -1 * gravityDownVector;

} else {

distanceToGround = DISTANCE_TO_GROUND_IGNORE_PLANET;

gravityDownVector = downVector;

gravityUpVector = upVector;

}

}

}

public static class Horizont {

public static float angle = 0.0f;

public static bool hit = false;

private static bool ignorePlanet;

private static Vector3D tracePosition;

private static float angleDir = 1.0f;

private static float up = 1.0f, down = 1.0f, mult = 1.0f;

public static Vector3D? ScanHorizont(float distance, Vector3D forwardVector, Vector3D rightVector) {

tracePosition = Situation.position + Math.Min(distance, HORIZONT_CHECK_DISTANCE) * Vector3D.Transform(forwardVector, Quaternion.CreateFromAxisAngle(rightVector, angle));

ignorePlanet = Situation.distanceToGround >= DISTANCE_TO_GROUND_IGNORE_PLANET;

if(hit) {

switch(Raytracer.Trace(ref tracePosition, ignorePlanet)) {

case Raytracer.Result.Hit:

angle += HORIZONT_CHECK_ANGLE_STEP * up;

up = Math.Min(10.0f, up + 1.0f);

down = 1.0f;

mult = 1.0f;

angle = (float)Math.Min(HORIZONT_MAX_UP_ANGLE, angle);

return Vector3D.Transform(forwardVector, Quaternion.CreateFromAxisAngle(rightVector, angle));

case Raytracer.Result.NoHit:

angle -= HORIZONT_CHECK_ANGLE_STEP * down;

down = Math.Min(10.0f, down + 1.0f);

up = 1.0f;

mult = 1.0f;

if(angle < -HORIZONT_CHECK_ANGLE_LIMIT) {

hit = false;

angle = 0.0f;

up = down = mult = 1.0f;

return Vector3D.Zero;

}

return Vector3D.Transform(forwardVector, Quaternion.CreateFromAxisAngle(rightVector, angle));

}

} else {

switch(Raytracer.Trace(ref tracePosition, ignorePlanet)) {

case Raytracer.Result.Hit:

hit = true;

up = down = mult = 1.0f;

return Vector3D.Transform(forwardVector, Quaternion.CreateFromAxisAngle(rightVector, angle));

case Raytracer.Result.NoHit:

up = down = 1.0f;

angle += angleDir * mult * HORIZONT_CHECK_ANGLE_STEP;

mult = Math.Min(10.0f, mult + 1.0f);

if(Math.Abs(angle) > HORIZONT_CHECK_ANGLE_LIMIT) {

angle = Math.Min(HORIZONT_CHECK_ANGLE_LIMIT, Math.Max(angle, -HORIZONT_CHECK_ANGLE_LIMIT));

angleDir *= -1.0f;

mult = 1.0f;

}

return Vector3D.Zero;

}

}

return null;

}

}

public static class Guidance {

private static Vector3D desiredPosition = new Vector3D();

private static Vector3D desiredFront = new Vector3D();

private static Vector3D desiredUp = new Vector3D();

private static float desiredSpeed = MAX_SPEED;

public static void Set(Waypoint w) {

desiredPosition = w.stance.position;

desiredFront = w.stance.forward;

desiredUp = w.stance.up;

desiredSpeed = w.maxSpeed;

}

public static void Release() {

foreach(IMyGyro gyro in GridBlocks.gyroBlocks) {

gyro.GyroOverride = false;

}

foreach(IMyThrust thruster in GridBlocks.thrustBlocks) {

thruster.ThrustOverride = 0;

}

}

public static void Tick() {

Guidance.StanceTick();

Guidance.GyroTick();

Guidance.ThrusterTick();

}

public static bool Done() {

return worldVector.Length() < 0.05 && pathLen <= 0.1f;

}

private static Vector3D pathNormal, path, aimTarget, upVector, aimVector;

private static float pathLen;

private static void StanceTick() {

path = desiredPosition - Situation.position;

pathLen = (float)path.Length();

pathNormal = Vector3D.Normalize(path);

if(desiredFront != Vector3D.Zero) {

aimTarget = Situation.position + desiredFront * Situation.radius;

} else {

aimVector = (pathLen > GUIDANCE_MIN_AIM_DISTANCE) ? pathNormal : Situation.forwardVector;

if(Situation.inGravity) {

aimTarget = Situation.position + Vector3D.Normalize(Vector3D.ProjectOnPlane(ref aimVector, ref Situation.gravityUpVector)) * Situation.radius;

} else {

aimTarget = Situation.position + aimVector * Situation.radius;

}

}

if(Situation.inGravity) {

upVector = (desiredUp == Vector3D.Zero) ? Situation.gravityUpVector : desiredUp;

} else {

upVector = (desiredUp == Vector3D.Zero) ? Vector3D.Cross(aimVector, Situation.leftVector) : desiredUp;

}

}

private static Quaternion invQuat;

private static Vector3D direction, refVector, worldVector, localVector, realUpVect, realRightVect;

private static double azimuth, elevation, roll;

private static void GyroTick() {

if(GridBlocks.gyroBlocks.Count == 0) {

return;

}

direction = Vector3D.Normalize(aimTarget - Situation.position);

invQuat = Quaternion.Inverse(Quaternion.CreateFromForwardUp(Situation.forwardVector, Situation.upVector));

refVector = Vector3D.Transform(direction, invQuat);

Vector3D.GetAzimuthAndElevation(refVector, out azimuth, out elevation);

realUpVect = Vector3D.ProjectOnPlane(ref upVector, ref direction);

realUpVect.Normalize();

realRightVect = Vector3D.Cross(direction, realUpVect);

realRightVect.Normalize();

roll = Vector3D.Dot(Situation.upVector, realRightVect);

worldVector = Vector3.Transform((new Vector3D(elevation, azimuth, roll)), Situation.orientation);

foreach(IMyGyro gyro in GridBlocks.gyroBlocks) {

localVector = Vector3.Transform(worldVector, Matrix.Transpose(gyro.WorldMatrix.GetOrientation()));

gyro.Pitch = (float)MathHelper.Clamp((-localVector.X * GYRO_GAIN), -GYRO_MAX_ANGULAR_VELOCITY, GYRO_MAX_ANGULAR_VELOCITY);

gyro.Yaw = (float)MathHelper.Clamp(((-localVector.Y) * GYRO_GAIN), -GYRO_MAX_ANGULAR_VELOCITY, GYRO_MAX_ANGULAR_VELOCITY);

gyro.Roll = (float)MathHelper.Clamp(((-localVector.Z) * GYRO_GAIN), -GYRO_MAX_ANGULAR_VELOCITY, GYRO_MAX_ANGULAR_VELOCITY);

gyro.GyroOverride = true;

}

}

private static float forwardChange, upChange, leftChange, applyPower, massFix;

private static Vector3D force, directVel, directNormal, indirectVel;

private static double ttt, maxFrc, maxVel, maxAcc, TIME_STEP = 2.5 * TICK_TIME, smooth;

private static float massA = 2000000.0f;

private static float massB = 5000.0f;

private static float massM = (massA - 2.0f * massB) / (massA - massB);

private static float massN = 1.0f / (massA - massB);

private static void ThrusterTick() {

if(pathLen == 0.0f) {

return;

}

massFix = massM * Situation.mass + massN * Situation.mass * Situation.mass;

force = Situation.mass * Situation.naturalGravity;

directVel = Vector3D.ProjectOnVector(ref Situation.linearVelocity, ref pathNormal);

directNormal = Vector3D.Normalize(directVel);

if(!directNormal.IsValid()) {

directNormal = Vector3D.Zero;

}

maxFrc = Situation.GetMaxThrust(pathNormal) - ((Vector3D.Dot(force, pathNormal) > 0) ? Vector3D.ProjectOnVector(ref force, ref pathNormal).Length() : 0.0);

maxVel = Math.Sqrt(2.0 * pathLen * maxFrc / massFix);

smooth = Math.Min(Math.Max((desiredSpeed + 1.0f - directVel.Length()) / 2.0f, 0.0f), 1.0f);

maxAcc = 1.0f + (maxFrc / massFix) * smooth * smooth * (3.0f - 2.0f * smooth);

ttt = Math.Max(TIME_STEP, Math.Abs(maxVel / maxAcc));

force += massFix * -2.0 * (pathNormal * pathLen / ttt / ttt - directNormal * directVel.Length() / ttt);

indirectVel = Vector3D.ProjectOnPlane(ref Situation.linearVelocity, ref pathNormal);

force += massFix * indirectVel / TIME_STEP;

forwardChange = (float)Vector3D.Dot(force, Situation.gridForwardVect);

upChange = (float)Vector3D.Dot(force, Situation.gridUpVect);

leftChange = (float)Vector3D.Dot(force, Situation.gridLeftVect);

foreach(IMyThrust thruster in GridBlocks.thrustBlocks) {

if(!thruster.IsWorking) {

thruster.ThrustOverridePercentage = 0;

continue;

}

switch(thruster.Orientation.Forward) {

case Base6Directions.Direction.Forward:

thruster.ThrustOverridePercentage = ((forwardChange < 0) ? IDLE_POWER : (Guidance.Drain(ref forwardChange, thruster.MaxEffectiveThrust)));

break;

case Base6Directions.Direction.Backward:

thruster.ThrustOverridePercentage = ((forwardChange > 0) ? IDLE_POWER : (Guidance.Drain(ref forwardChange, thruster.MaxEffectiveThrust)));

break;

case Base6Directions.Direction.Up:

thruster.ThrustOverridePercentage = ((upChange < 0) ? IDLE_POWER : (Guidance.Drain(ref upChange, thruster.MaxEffectiveThrust)));

break;

case Base6Directions.Direction.Down:

thruster.ThrustOverridePercentage = ((upChange > 0) ? IDLE_POWER : (Guidance.Drain(ref upChange, thruster.MaxEffectiveThrust)));

break;

case Base6Directions.Direction.Left:

thruster.ThrustOverridePercentage = ((leftChange < 0) ? IDLE_POWER : (Guidance.Drain(ref leftChange, thruster.MaxEffectiveThrust)));

break;

case Base6Directions.Direction.Right:

thruster.ThrustOverridePercentage = ((leftChange > 0) ? IDLE_POWER : (Guidance.Drain(ref leftChange, thruster.MaxEffectiveThrust)));

break;

}

}

}

private static float Drain(ref float remainingPower, float maxEffectiveThrust) {

applyPower = Math.Min(Math.Abs(remainingPower), maxEffectiveThrust);

remainingPower = (remainingPower > 0) ? (remainingPower - applyPower) : (remainingPower + applyPower);

return Math.Max(applyPower / maxEffectiveThrust, IDLE_POWER);

}

}

public static class Navigation {

public static List<Waypoint> waypoints = new List<Waypoint> { };

public static string Status() {

if(waypoints.Count == 0) {

return "";

}

return waypoints[0].GetTypeMsg();

}

private static Vector3D? horizontDirectionNormal;

private static Vector3D endTarget, endTargetPath, endTargetNormal, newDirection, newTarget, endTargetRightVector;

private static void CheckColision() {

switch(waypoints[0].type) {

case Waypoint.wpType.CONVERGING:

if((waypoints[0].stance.position - Situation.position).Length() < COLLISION_DISABLE_RADIUS_MULTIPLIER * Situation.radius) {

return;

}

break;

case Waypoint.wpType.NAVIGATING:

break;

default:

return;

}

endTarget = waypoints[waypoints[0].type == Waypoint.wpType.NAVIGATING ? 1 : 0].stance.position;

endTargetPath = endTarget - Situation.position;

endTargetNormal = Vector3D.Normalize(endTargetPath);

endTargetRightVector = Vector3D.Normalize(Vector3D.Cross(endTargetNormal, Situation.gravityUpVector));

horizontDirectionNormal = Horizont.ScanHorizont((float)endTargetPath.Length(), endTargetNormal, endTargetRightVector);

if(!horizontDirectionNormal.HasValue) {

return;

}

if(Vector3D.IsZero(horizontDirectionNormal.Value)) {

if(waypoints[0].type == Waypoint.wpType.NAVIGATING) {

waypoints.RemoveAt(0);

}

return;

}

newDirection = Vector3D.Transform(horizontDirectionNormal.Value, Quaternion.CreateFromAxisAngle(endTargetRightVector, COLLISION_CORRECTION_ANGLE));

newTarget = Situation.position + Math.Min(HORIZONT_CHECK_DISTANCE, (Situation.position - waypoints.Last().stance.position).Length()) * newDirection;

if(waypoints[0].type == Waypoint.wpType.NAVIGATING) {

waypoints[0].stance.position = newTarget;

} else {

waypoints.Insert(0, new Waypoint(new Stance(newTarget, Vector3D.Zero, Vector3D.Zero), MAX_SPEED, Waypoint.wpType.NAVIGATING));

}

}

public static void Tick() {

if(waypoints.Count == 0) {

return;

}

Situation.RefreshParameters();

CheckColision();

Guidance.Set(waypoints.ElementAt(0));

Guidance.Tick();

if(Guidance.Done()) {

waypoints.RemoveAt(0);

if(waypoints.Count != 0) {

return;

}

Guidance.Release();

}

}

public static void AddWaypoint(Waypoint w) {

waypoints.Insert(0, w);

}

public static void AddWaypoint(Stance s, float m, Waypoint.wpType wt) {

AddWaypoint(new Waypoint(s, m, wt));

}

public static void AddWaypoint(Vector3D p, Vector3D f, Vector3D u, float m, Waypoint.wpType wt) {

AddWaypoint(new Stance(p, f, u), m, wt);

}

public static void Stop() {

Guidance.Release();

waypoints.Clear();

}

public static bool Done() {

return waypoints.Count == 0;

}

}

public static class Commander {

public static bool active = false;

public static Dock currentDock;

public enum Mode { SINGLE, LIST, LOOP };

public static Mode mode = Mode.SINGLE;

public static long idleStart = long.MaxValue;

public static long waitTime = TimeSpan.FromSeconds(10.0).Ticks;

public static void PilotDone() {

idleStart = DateTime.Now.Ticks;

}

public static void Activate() {

active = true;

currentDock = null;

}

public static void Activate(Dock dock) {

active = true;

currentDock = dock;

Logistics.Charge(false);

Logistics.Dampeners(true);

}

public static void Deactivate() {

active = false;

currentDock = null;

}

private static bool chargeDone;

private static bool cargoDone;

public static void Tick() {

if(!active || Pilot.running) {

return;

}

if(mode == Mode.SINGLE) {

Deactivate();

return;

}

if(currentDock != null && currentDock.job == Dock.JobType.NONE) {

if(DateTime.Now.Ticks - idleStart < waitTime) {

return;

}

} else if(currentDock != null) {

if(ConnectorControl.Connected() == null) {

return;

}

cargoDone = true;

switch(currentDock.job) {

case Dock.JobType.LOAD:

case Dock.JobType.CHARGE_LOAD:

if(!Logistics.CargoFull()) {

cargoDone = false;

}

break;

case Dock.JobType.UNLOAD:

case Dock.JobType.CHARGE_UNLOAD:

if(!Logistics.CargoEmpty()) {

cargoDone = false;

}

break;

}

chargeDone = true;

switch(currentDock.job) {

case Dock.JobType.CHARGE:

case Dock.JobType.CHARGE_LOAD:

case Dock.JobType.CHARGE_UNLOAD:

Logistics.Charge(true);

if(!Logistics.ChargeFull()) {

chargeDone = false;

return;

}

Logistics.Charge(false);

break;

}

if(!cargoDone || !chargeDone) {

return;

}

}

DockData.NAVScreenHandle(Pannels.ScreenAction.Next);

if(mode == Mode.LIST && DockData.selectedDockNAV == 0) {

Deactivate();

Logger.Info("Dock list finished, stopping autopilot.");

return;

}

if(currentDock == null) {

Logger.Info("Just a waypoint, navigating to next dock.");

} else {

switch(currentDock.job) {

case Dock.JobType.NONE:

Logger.Info("Wait time expired, resuming navigation.");

break;

case Dock.JobType.LOAD:

Logger.Info("Cargo loaded, resuming navigation.");

break;

case Dock.JobType.UNLOAD:

Logger.Info("Cargo unloaded, resuming navigation.");

break;

case Dock.JobType.CHARGE:

Logger.Info("Charged, resuming navigation.");

break;

case Dock.JobType.CHARGE_LOAD:

Logger.Info("Charged and cargo loaded, resuming navigation.");

break;

case Dock.JobType.CHARGE_UNLOAD:

Logger.Info("Cargo unloaded, resuming navigation.");

break;

}

}

Pilot.Start();

}

private static string retStr;

private static bool match;

private static string myName;

private static List<KeyValuePair<NavCmd, Dock>> found = new List<KeyValuePair<NavCmd, Dock>> { };

private static List<NavCmd> notFound = new List<NavCmd> { };

private static string command;

public static void ProcessCmd(Program p, string cmd) {

Serializer.InitUnpack(cmd);

retStr = ExecuteCmd(Serializer.UnpackShipCommand());

if(retStr == "") {

return;

}

p.IGC.SendBroadcastMessage<string>(CMD_RES_TAG, retStr);

}

public static string ExecuteCmd(ShipCommand shipCommand) {

if(!Block.GetProperty(GridBlocks.MasterProgrammableBlock.EntityId, "Name", ref myName)) {

myName = GridBlocks.MasterProgrammableBlock.CubeGrid.CustomName;

}

if(shipCommand.ShipName != myName) {

return "";

}

if(shipCommand.Command < 0 || shipCommand.Command > Terminal.COMMANDS.Count - 1) {

return "Received a command that does not exist: " + shipCommand;

}

Logger.Info("Remote command received.");

command = Terminal.COMMANDS[shipCommand.Command];

if(command.Contains("start")) {

Pilot.Start();

return "Start success.";

} else if(command.Contains("stop")) {

Pilot.Stop();

return "Stop success.";

}

found.Clear();

notFound.Clear();

foreach(NavCmd nc in shipCommand.navCmds) {

match = false;

foreach(Dock d in DockData.docks) {

if(nc.Connector == d.blockName) {

if(nc.Grid == "" || nc.Grid == d.gridName) {

found.Add(new KeyValuePair<NavCmd, Dock>(nc, d));

match = true;

break;

}

}

}

if(!match) {

notFound.Add(nc);

}

}

if(notFound.Count != 0) {

return "Not found: " + notFound.Count.ToString();

}

Pilot.Stop();

DockData.selectedDocks.Clear();

DockData.selectedDockNAV = 0;

foreach(KeyValuePair<NavCmd, Dock> kp in found) {

kp.Value.job = kp.Key.Action;

DockData.selectedDocks.Add(kp.Value);

}

DockData.BalanceDisplays();

if(command.Contains("step")) {

SetMode(Mode.SINGLE);

} else if(command.Contains("run")) {

SetMode(Mode.LIST);

} else if(command.Contains("loop")) {

SetMode(Mode.LOOP);

}

if(!command.Contains("conf")) {

Pilot.Start();

}

return "Command executed";

}

private static string newCustomName;

private static void SetMode(Mode newMode) {

mode = newMode;

newCustomName = GridBlocks.MasterProgrammableBlock.CustomName;

if(newMode == Mode.LIST) {

newCustomName = newCustomName.Replace(" LOOP", "");

newCustomName = newCustomName.Replace("[" + TAG, "[" + TAG + " LIST");

Block.UpdateProperty(GridBlocks.MasterProgrammableBlock.EntityId, "LIST", "");

Block.RemoveProperty(GridBlocks.MasterProgrammableBlock.EntityId, "LOOP");

} else if(newMode == Mode.LOOP) {

newCustomName = newCustomName.Replace(" LIST", "");

newCustomName = newCustomName.Replace("[" + TAG, "[" + TAG + " LOOP");

Block.UpdateProperty(GridBlocks.MasterProgrammableBlock.EntityId, "LOOP", "");

Block.RemoveProperty(GridBlocks.MasterProgrammableBlock.EntityId, "LIST");

} else {

newCustomName = newCustomName.Replace(" LIST", "");

newCustomName = newCustomName.Replace(" LOOP", "");

Block.RemoveProperty(GridBlocks.MasterProgrammableBlock.EntityId, "LOOP");

Block.RemoveProperty(GridBlocks.MasterProgrammableBlock.EntityId, "LIST");

}

GridBlocks.MasterProgrammableBlock.CustomName = newCustomName;

}

}

public static class Logistics {

private static string valStr;

private static float valFloat;

public static void Dampeners(bool enable) {

RemoteControl.block.DampenersOverride = enable;

}

private static List<IMyGasTank> tempTanks = new List<IMyGasTank>();

public static bool CargoFull() {

tempTanks.Clear();

foreach(IMyGasTank tankBlock in GridBlocks.tankBlocks) {

if(!Block.HasProperty(tankBlock.EntityId, "CARGO")) {

continue;

}

tempTanks.Add(tankBlock);

tankBlock.Stockpile = true;

}

foreach(IMyGasTank tankBlock in tempTanks) {

valFloat = 95.0f;

if(Block.GetProperty(tankBlock.EntityId, "Full", ref valStr)) {

if(!float.TryParse(valStr, out valFloat)) {

valFloat = 95.0f;

}

}

if(tankBlock.FilledRatio < valFloat / 100.0f) {

return false;

}

}

foreach(IMyCargoContainer cargoBlock in GridBlocks.cargoBlocks) {

valFloat = 90f;

if(Block.GetProperty(cargoBlock.EntityId, "Full", ref valStr)) {

if(!float.TryParse(valStr, out valFloat)) {

valFloat = 90f;

}

}

IMyInventory inventory = cargoBlock.GetInventory();

if(inventory.CurrentVolume.RawValue < (inventory.MaxVolume.RawValue * valFloat / 100.0f)) {

return false;

}

}

return true;

}

public static bool CargoEmpty() {

tempTanks.Clear();

foreach(IMyGasTank tankBlock in GridBlocks.tankBlocks) {

if(!Block.HasProperty(tankBlock.EntityId, "CARGO")) {

continue;

}

tempTanks.Add(tankBlock);

tankBlock.Stockpile = false;

}

foreach(IMyGasTank tankBlock in tempTanks) {

valFloat = 0.0f;

if(Block.GetProperty(tankBlock.EntityId, "Empty", ref valStr)) {

if(!float.TryParse(valStr, out valFloat)) {

valFloat = 0.0f;

}

}

if(tankBlock.FilledRatio > valFloat / 100.0f) {

return false;

}

}

foreach(IMyCargoContainer cargoBlock in GridBlocks.cargoBlocks) {

valFloat = 0.0f;

if(Block.GetProperty(cargoBlock.EntityId, "Empty", ref valStr)) {

if(!float.TryParse(valStr, out valFloat)) {

valFloat = 0.0f;

}

}

IMyInventory inventory = cargoBlock.GetInventory();

if(inventory.CurrentVolume.RawValue > (inventory.MaxVolume.RawValue * valFloat / 100.0f)) {

return false;

}

}

return true;

}

public static bool ChargeFull() {

foreach(IMyGasTank tankBlock in GridBlocks.tankBlocks) {

if(Block.HasProperty(tankBlock.EntityId, "CARGO")) {

continue;

}

valFloat = 95.0f;

tankBlock.Stockpile = true;

if(Block.GetProperty(tankBlock.EntityId, "Full", ref valStr)) {

if(!float.TryParse(valStr, out valFloat)) {

valFloat = 95.0f;

}

}

if(tankBlock.FilledRatio < valFloat / 100.0f) {

return false;

}

}

foreach(IMyBatteryBlock batteryBlock in GridBlocks.batteryBlocks) {

valFloat = 95f;

if(Block.GetProperty(batteryBlock.EntityId, "Full", ref valStr)) {

if(!float.TryParse(valStr, out valFloat)) {

valFloat = 95f;

}

}

if(batteryBlock.CurrentStoredPower < (batteryBlock.MaxStoredPower * valFloat / 100.0f)) {

return false;

}

}

Logger.Info("Everything is charged!");

return true;

}

private static bool forceCharge;

public static void Charge(bool enable) {

foreach(IMyBatteryBlock batteryBlock in GridBlocks.batteryBlocks) {

forceCharge = Block.HasProperty(batteryBlock.EntityId, "FORCE");

batteryBlock.ChargeMode = enable && forceCharge ? ChargeMode.Recharge : ChargeMode.Auto;

}

foreach(IMyGasTank tankBlock in GridBlocks.tankBlocks) {

if(Block.HasProperty(tankBlock.EntityId, "CARGO")) {

continue;

}

forceCharge = Block.HasProperty(tankBlock.EntityId, "FORCE");

tankBlock.Stockpile = enable && forceCharge;

}

}

}

public static class Pilot {

public static bool running = false;

public static List<Dock> dock = new List<Dock>();

public static void Tick() {

if(!running) {

return;

}

if(!RemoteControl.Present()) {

if(!ErrorState.Get(ErrorState.Type.NoRemoteController)) {

Logger.Err(MSG_NO_REMOTE_CONTROL);

}

ErrorState.Set(ErrorState.Type.NoRemoteController);

Stop();

return;

}

if(Navigation.Done()) {

if(dock.Count != 0 && dock[0].gridEntityId != 0) {

CalculateApproach();

dock.Clear();

return;

}

Logger.Info(MSG_NAVIGATION_SUCCESSFUL);

Signal.Send(Signal.SignalType.NAVIGATION);

Commander.PilotDone();

ConnectorControl.AttemptConnect();

running = false;

return;

}

Navigation.Tick();

}

private static Quaternion qInitialInverse, qFinal, qDiff;

private static Vector3D connectorToCenter, rotatedConnectorToCenter, newUp, newForward, up, referenceUp, direction, balancedDirection;

private static IMyShipConnector connector;

private static bool revConnector;

private static float connectorDistance;

private static void CalculateApproach() {

connector = ConnectorControl.GetConnector(dock[0]);

if(connector == null) {

Logger.Warn(MSG_NO_CONNECTORS_AVAILABLE);

return;

}

Situation.RefreshParameters();

connectorToCenter = Situation.position - connector.GetPosition();

if(Math.Abs(Vector3D.Dot(dock[0].stance.forward, Situation.gravityUpVector)) < 0.5f) {

up = Situation.gravityUpVector;

referenceUp = connector.WorldMatrix.GetDirectionVector(connector.WorldMatrix.GetClosestDirection(up));

referenceUp = (referenceUp == connector.WorldMatrix.Forward || referenceUp == connector.WorldMatrix.Backward) ? connector.WorldMatrix.Up : referenceUp;

} else {

up = dock[0].stance.up;

referenceUp = connector.WorldMatrix.Up;

}

revConnector = Block.HasProperty(connector.EntityId, "REV");

qInitialInverse = Quaternion.Inverse(Quaternion.CreateFromForwardUp(revConnector ? connector.WorldMatrix.Backward : connector.WorldMatrix.Forward, referenceUp));

qFinal = Quaternion.CreateFromForwardUp(-dock[0].stance.forward, up);

qDiff = qFinal * qInitialInverse;

rotatedConnectorToCenter = Vector3D.Transform(connectorToCenter, qDiff);

newForward = Vector3D.Transform(RemoteControl.block.WorldMatrix.Forward, qDiff);

newUp = Vector3D.Transform(RemoteControl.block.WorldMatrix.Up, qDiff);

connectorDistance = (dock[0].cubeSize == VRage.Game.MyCubeSize.Large) ? 2.6f / 2.0f : 0.5f;

connectorDistance += (GridBlocks.MasterProgrammableBlock.CubeGrid.GridSizeEnum == VRage.Game.MyCubeSize.Large) ? 2.6f / 2.0f : 0.5f;

newPos = dock[0].stance.position + rotatedConnectorToCenter + (connectorDistance * dock[0].stance.forward);

Navigation.AddWaypoint(newPos, newForward, newUp, DOCKING_SPEED, Waypoint.wpType.DOCKING);

newPos = dock[0].stance.position + rotatedConnectorToCenter + ((DOCK_DISTANCE + connectorDistance) * dock[0].stance.forward);

Navigation.AddWaypoint(newPos, newForward, newUp, TAXIING_SPEED, Waypoint.wpType.APPROACHING);

dock[0].approachPath.Reverse();

foreach(VectorPath vp in dock[0].approachPath) {

newPos = vp.position + (vp.direction * (APPROACH_SAFE_DISTANCE + Situation.radius));

Navigation.AddWaypoint(newPos, Vector3D.Zero, Vector3D.Zero, TAXIING_SPEED, Waypoint.wpType.TAXIING);

}

dock[0].approachPath.Reverse();

}

private static Vector3D newPos, undockPos;

private static Dock disconnectDock;

private static void SetEndStance(Dock dock) {

Situation.RefreshParameters();

if(dock.blockEntityId == 0) {

Navigation.AddWaypoint(dock.stance, MAX_SPEED, Waypoint.wpType.ALIGNING);

Navigation.AddWaypoint(dock.stance.position, Vector3D.Zero, Vector3D.Zero, MAX_SPEED, Waypoint.wpType.CONVERGING);

newPos = dock.stance.position;

} else {

if(dock.approachPath.Count == 0) {

newPos = dock.stance.position + ((APPROACH_DISTANCE + Situation.radius) * dock.stance.forward);

} else {

newPos = dock.approachPath[0].position + ((APPROACH_DISTANCE + Situation.radius) * dock.approachPath[0].direction);

}

Navigation.AddWaypoint(newPos, Vector3D.Zero, Vector3D.Zero, MAX_SPEED, Waypoint.wpType.CONVERGING);

}

if(Situation.linearVelocity.Length() >= 2.0f) {

return;

}

disconnectDock = ConnectorControl.DisconnectAndTaxiData();

direction = Vector3D.Normalize(newPos - Situation.position);

balancedDirection = Vector3D.ProjectOnPlane(ref direction, ref Situation.gravityUpVector);

if(disconnectDock == null) {

Navigation.AddWaypoint(Situation.position, balancedDirection, Situation.gravityUpVector, MAX_SPEED, Waypoint.wpType.ALIGNING);

return;

}

if(disconnectDock.approachPath.Count > 0) {

foreach(VectorPath vp in disconnectDock.approachPath) {

newPos = vp.position + (vp.direction * (APPROACH_SAFE_DISTANCE + Situation.radius));

Navigation.AddWaypoint(newPos, Vector3D.Zero, Vector3D.Zero, TAXIING_SPEED, Waypoint.wpType.TAXIING);

}

}

undockPos = disconnectDock.stance.forward;

undockPos *= (Situation.radius + UNDOCK_DISTANCE);

undockPos += Situation.position;

Navigation.AddWaypoint(undockPos, balancedDirection, Situation.gravityUpVector, DOCKING_SPEED, Waypoint.wpType.ALIGNING);

Navigation.AddWaypoint(undockPos, Situation.forwardVector, Situation.upVector, DOCKING_SPEED, Waypoint.wpType.UNDOCKING);

}

public static void Start() {

Start(DockData.GetSelected());

}

public static void Start(Dock d) {

if(d == null) {

return;

}

if(!RemoteControl.PresentOrLog()) {

return;

}

Stop();

Logger.Info(MSG_NAVIGATION_TO + "[" + d.gridName + "] " + d.blockName);

dock.Add(d);

SetEndStance(d);

running = true;

Commander.Activate(d);

Signal.Send(Signal.SignalType.START);

}

public static void Start(Waypoint w) {

if(!RemoteControl.PresentOrLog()) {

return;

}

if(w.stance.position == Vector3D.Zero) {

Logger.Err(MSG_INVALID_GPS_TYPE);

return;

}

Stop();

Logger.Info(MSG_NAVIGATION_TO_WAYPOINT);

Navigation.AddWaypoint(w);

running = true;

Commander.Activate();

}

public static void Stop() {

Navigation.Stop();

dock.Clear();

running = false;

Commander.Deactivate();

}

public static void Toggle() {

if(running) {

Stop();

return;

}

Start();

}

}

private IMyBroadcastListener listener;

private IMyBroadcastListener cmdListener;

private IMyBroadcastListener cmdResListener;

private bool clearStorage = false;

public Program() {

try {

if(this.Load()) {

Logger.Info("Loaded previous session");

}

} catch(Exception e) {

Logger.Warn("Unable to load previous session: " + e.Message);

Storage = "";

}

Runtime.UpdateFrequency = UpdateFrequency.Update100 | UpdateFrequency.Update10 | UpdateFrequency.Once;

listener = IGC.RegisterBroadcastListener(TAG);

listener.SetMessageCallback(TAG);

cmdListener = IGC.RegisterBroadcastListener(CMD_TAG);

cmdListener.SetMessageCallback(CMD_TAG);

cmdResListener = IGC.RegisterBroadcastListener(CMD_RES_TAG);

cmdResListener.SetMessageCallback(CMD_RES_TAG);

}

public bool Load() {

if(Storage.Length != 0) {

Logger.Info("Loading session size: " + Storage.Length);

if(StorageData.Load(Storage)) {

return true;

}

Logger.Warn("Unable to Load previous session due to different version");

}

return false;

}

public void Save() {

string str = clearStorage ? "" : StorageData.Save();

Logger.Info("Saving session size: " + str.Length);

Storage = str;

}

public static class StorageData {

public static string Save() {

Serializer.InitPack();

Serializer.Pack(STORAGE_VERSION);

Serializer.Pack(DockData.currentDockCount);