GyroECU5

using Sandbox.ModAPI.Ingame;
using System;
using System.Collections.Generic;
using System.Data.Common;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using VRageMath;

namespace IngameScript
{
    partial class Program : MyGridProgram
    {
        public class GyroECU5
        {
            public static double gyroMaxRPM = Math.PI;

            public double angleMultiplier = 1;

            public GyroECU5() : base()
            {
                angleMultiplier = gProgram.Me.CubeGrid.GridSizeEnum == VRage.Game.MyCubeSize.Small ? 2 : 1;
                gyroMaxRPM *= angleMultiplier;
                lastAngleRoll = lastAnglePitch = lastAngleYaw = 0;
                lMPTRoll = lMPTPitch = lMPTYaw = 0;
            }
            double lastAngleRoll, lastAnglePitch, lastAngleYaw;
            double lMPTRoll, lMPTPitch, lMPTYaw;

            public bool active = false;
            Vector3D targetPosition;
            Vector3D targetVelocity;//currently unused

            Vector3D targetHeading;
            Vector3D targetUp;

            public int startTick;
            public int ticksOnTarget = 0;
            private void flush()
            {
                if (!active)
                {
                    active = true;
                    foreach (var g in gyros) g.GyroOverride = false;
                    lG = null;
                    startTick = tick;
                    lastAngleRoll = lastAnglePitch = lastAngleYaw = 0;
                    lMPTRoll = lMPTPitch = lMPTYaw = 0;
                    ticksOnTarget = 0;
                }
            }

            public void rotateToPosition(Vector3D tp, Vector3D tv = new Vector3D())
            {
                if (!active) log("GECU test init rtp", LT.LOG_N);
                flush();
                targetPosition = tp;
                targetVelocity = tv;
                targetUp = getCtrl().WorldMatrix.Up;
            }
            public void rotateToHeading(Vector3D forward, Vector3D up = new Vector3D())
            {
                if (up == Vector3D.Zero) up = getCtrl().WorldMatrix.Up;
                if (!active)
                {
                    log("GECU test init rth", LT.LOG_N);
                    log("forward:" + v2ss(forward), LT.LOG_N);
                    log("up:" + v2ss(up), LT.LOG_N);
                }
                flush();
                targetPosition = targetVelocity = Vector3D.Zero;
                targetHeading = forward;
                targetUp = up;
            }

            double error_thresholdLocked = ConvertDegreesToRadians(1);//we must be within this much on each axis
            double minVelThresholdLocked = ConvertDegreesToRadians(1d);
            //these only set qualifiers for ticksOnTarget.

            private void calculateAxisSpecificData(double now, ref double prior, ref double lastMPT, out bool braking, out double lastMPTActual, string p = "")//, out double ticksToStop, out double ticksToTarget)
            {

                var radMovedPerTick = Math.Abs(prior - now);
                var ticksToTarget = Math.Abs(now) / radMovedPerTick;
                var initVel = radMovedPerTick;
                var rateOfDecel = Math.Abs(lastMPT - radMovedPerTick);
                //if (rateOfDecel > mod) mod = rateOfDecel;

                //if (Math.Abs(now) > nobrake_threshold) rateOfDecel *= 1.5;//overestimating here did not improve timings
                var ticksToStop = initVel / rateOfDecel;
                //mod - maximum observed decel - saved 0.1s on large sluggish ship but lost .3s on sg snappy ship.
                //sticking to the conservative metric

                bool closing = Math.Abs(now) < Math.Abs(prior);
                lastMPTActual = radMovedPerTick;
                if (!closing)
                {
                    lastMPT = 0.0001;
                }
                else lastMPT = radMovedPerTick;

                if (closing)
                {
                    if (ticksToStop > ticksToTarget + 1) braking = true;
                    else braking = false;
                }
                else braking = false;

                prior = now;
            }

            //for distances under 90 degrees, it returns a value between 0 and gyromaxRPM, sharpened with sine so it levels off a bit in a nice way at the end.
            //slightly black magic, but if it works, it works
            static double amp_threshold = ConvertDegreesToRadians(100);//125.0d);
            static double deAmp(double i)//, double spd)
            {
                if (i == 0) return i;
                var abs = Math.Abs(i);
                var ig = i / abs * gyroMaxRPM;
                //spd = 0;
                if (abs > amp_threshold) return ig;

                i = i / (amp_threshold);
                i = Math.Abs(i);
                i = Math.Sin(i);
                return i * ig;
            }

            public static void setUpdateRate(int ups)
            {
                updateRate = ups;
                updr_mult = 60 / updateRate;
                updr_mult_div = updr_mult / 60.0d;
            }

            static int updateRate = 60;
            static int updr_mult = 60 / updateRate;
            static double updr_mult_div = 60.0d / updateRate / 60.0d;
            int ltick = -1;
            public void update()
            {

                if (active && tick - ltick > updr_mult)// && tick % 2 == 0)
                {
                    ltick = tick;

                    if (!targetPosition.IsZero()) targetHeading = Vector3D.Normalize(targetPosition - getPosition());

                    double pitch, yaw, roll;
                    GetRotationAnglesSimultaneous(targetHeading, targetUp, getCtrl().WorldMatrix, out yaw, out pitch, out roll);

                    double rA, pA, yA;

                    bool yB, pB, rB;
                    calculateAxisSpecificData(roll, ref lastAngleRoll, ref lMPTRoll, out rB, out rA);
                    calculateAxisSpecificData(pitch, ref lastAnglePitch, ref lMPTPitch, out pB, out pA);
                    calculateAxisSpecificData(yaw, ref lastAngleYaw, ref lMPTYaw, out yB, out yA);

                    //Vector3D a_act = new Vector3D(pA, yA,rA);
                    //var amax = a_act.AbsMax();

                    Vector3D a_impulse = new Vector3D(pB ? 0 : pitch, yB ? 0 : yaw, rB ? 0 : roll);

                    //black magic everywhere
                    a_impulse.X = deAmp(a_impulse.X);//, amax * updr_mult);
                    a_impulse.Y = deAmp(a_impulse.Y);//, amax * updr_mult);
                    a_impulse.Z = deAmp(a_impulse.Z);//, amax * updr_mult);
                    if (Math.Abs(pA) / 60* updateRate > Math.Abs(a_impulse.X)) a_impulse.X = 0;
                    if (Math.Abs(yA) / 60 * updateRate > Math.Abs(a_impulse.Y)) a_impulse.Y = 0;
                    if (Math.Abs(rA) / 60 * updateRate > Math.Abs(a_impulse.Z)) a_impulse.Z = 0;


                    GyroOverride(getCtrl().WorldMatrix, a_impulse.X, a_impulse.Y, a_impulse.Z);

                    if (Math.Abs(roll) < error_thresholdLocked && Math.Abs(pitch) < error_thresholdLocked && Math.Abs(yaw) < error_thresholdLocked)
                    {
                        if (rA < minVelThresholdLocked * updr_mult_div && pA < minVelThresholdLocked * updr_mult_div && yA < minVelThresholdLocked * updr_mult_div)
                        {
                            ticksOnTarget += 1;
                        }
                        else ticksOnTarget = 0;
                    }
                    else ticksOnTarget = 0;
                }
            }

            IMyGyro lG = null;
            static Vector3D state = new Vector3D(9, 9, 9);
            const double E = MathHelper.EPSILON;
            void GyroOverride(MatrixD shipRef, double pitch_speed, double yaw_speed, double roll_speed)
            {
                IMyGyro g = null;
                foreach(var c in gyros)
                {
                    if(c.Enabled && c.IsFunctional && !c.Closed)
                    {
                        g = c;
                        break;
                    }
                }
                if (g == null) return;

                if(g != lG)
                {
                    lG = g;
                    state = new Vector3D(9, 9, 9);
                    g.GyroOverride = true;
                }

                var rotationVec = new Vector3D(-pitch_speed, yaw_speed, roll_speed); //because keen does some weird stuff with signs
                var relativeRotationVec = Vector3D.TransformNormal(rotationVec, shipRef);
                var trv = Vector3D.TransformNormal(relativeRotationVec, Matrix.Transpose(g.WorldMatrix));

                if (trv.X != state.X)// && Math.Abs(trv.X - state.X) > E)
                {
                    state.X = trv.X;
                    g.Pitch = (float)(state.X);
                }
                if (trv.Y != state.Y)// && Math.Abs(trv.Y - state.Y) > E)
                {
                    state.Y = trv.Y;
                    g.Yaw = (float)(state.Y);
                }
                if (trv.Z != state.Z)// && Math.Abs(trv.Z - state.Z) > E)
                {
                    state.Z = trv.Z;
                    g.Roll = (float)(state.Z);
                }
            }

            public void shutdown()
            {
                if (active)
                {
                    active = false;
                    if(lG != null)lG.GyroOverride = false;
                    log("GECU shutdown", LT.LOG_N);
                    log("time: " + (tick - startTick) + "t (" + ((double)(tick - startTick) / 60).ToString("0.0") + "s)", LT.LOG_N);
                }
            }
        }
    }
}