Prototype analytical gyro controller

/*
 * R e a d m e
 * -----------
 * klassekatze was here
 *
 * Prototype analytical gyro control class. No PID! ONLY ANGLE
 */


public static Program gProgram;

public Program()
{
    gProgram = this;
    //wsinit();
    Runtime.UpdateFrequency = UpdateFrequency.Update1;

    log("test", LT.LOG_N);
}


class logmsg
{
    public logmsg(string m, LT l) { msg = m; level = l; }
    public string msg = "";
    public int c = 1;
    public LT level = LT.LOG_N;
}

List<logmsg> loggedMessages = new List<logmsg>();
int MAX_LOG = 50;
bool loggedMessagesDirty = true;


public enum LT
{
    LOG_N = 0,
    LOG_D,
    LOG_DD
}

public static LT LOG_LEVEL = LT.LOG_N;

public static void log(string s, LT level)
{
    if (level > LOG_LEVEL) return;

    if (s.Length > 50)
    {
        List<string> tok = new List<string>();
        while (s.Length > 50)
        {
            int c = 0;
            if (tok.Count > 0) c = 2;
            tok.Add(s.Substring(0, 50 - c));
            s = s.Substring(50 - c);
        }
        tok.Add(s);
        s = string.Join("\n ", tok);
    }
    var p = gProgram;
    logmsg l = null;
    if (p.loggedMessages.Count > 0)
    {
        l = p.loggedMessages[p.loggedMessages.Count - 1];
    }
    if (l != null)
    {
        if (l.msg == s) l.c += 1;
        else p.loggedMessages.Add(new logmsg(s, level));
    }
    else p.loggedMessages.Add(new logmsg(s, level));
    if (p.loggedMessages.Count > p.MAX_LOG) p.loggedMessages.RemoveAt(0);
    p.loggedMessagesDirty = true;
}


string loggedMessagesCache = "";
string renderLoggedMessages()
{
    if (!loggedMessagesDirty) return loggedMessagesCache;

    string o = "";
    foreach (var m in loggedMessages)//(int i = 0; i < loggedMessages.Count; i++)
    {
        o += m.msg;
        if (m.c > 1) o += " (" + m.c + ")";
        o += "\n";
    }
    loggedMessagesDirty = false;
    loggedMessagesCache = o;
    return o;
}

List<IMyShipController> controllers = new List<IMyShipController>();
List<IMyGyro> gyros = new List<IMyGyro>();
IMyTextSurface consoleLog = null;
IMyTextSurface statusLog = null;
IMyTextSurface profileLog = null;

public bool isThis(IMyTerminalBlock b)
{
    return b.OwnerId == Me.OwnerId && b.CubeGrid == Me.CubeGrid;
}

bool needsetup = true;
public void loadBlocks()
{
    List<IMyTerminalBlock> LCDs = new List<IMyTerminalBlock>();
    GridTerminalSystem.GetBlocksOfType<IMyTerminalBlock>(LCDs, b => (b is IMyTextSurface) && b.CubeGrid == Me.CubeGrid);
    foreach (var b in LCDs)
    {
        IMyTextSurface s = b as IMyTextSurface;
        if (b.CustomData.Contains("radarLog")) statusLog = s;
        else if (b.CustomData.Contains("consoleLog")) consoleLog = s;
        else if (b.CustomData.Contains("profileLog")) profileLog = s;
    }

    controllers.Clear();
    GridTerminalSystem.GetBlocksOfType(controllers, isThis);
    gyros.Clear();
    GridTerminalSystem.GetBlocksOfType(gyros, isThis);

    gyroECU = new GyroECU4(gyros, controllers[0], controllers[0]);
}
GyroECU4 gyroECU = null;

public void Save()
{
    // Called when the program needs to save its state. Use
    // this method to save your state to the Storage field
    // or some other means.
    //
    // This method is optional and can be removed if not
    // needed.
}
static Profiler initP = new Profiler("init");
static Profiler mainP = new Profiler("main");

static int tick = -1;

public void Main(string argument, UpdateType updateSource)
{
    tick += 1;
    mainP.start();
    main(argument, updateSource);
    mainP.stop();
    if (tick % 5 == 0)
    {
        Echo(tick.ToString());
        if (profileLog != null) profileLog.WriteText("name:ms1t:ms60t\n" + Profiler.getAllReports());
    }
    if (consoleLog != null)// && tick % 6 == 0)
    {
        consoleLog.WriteText(renderLoggedMessages());
    }
}
static Profiler gyroP = new Profiler("gyro");
private void main(string arg, UpdateType upd)
{
    if (needsetup)
    {
        needsetup = false;
        loadBlocks();
    }

    if (arg == "test")
    {
        gyroECU.rotateToPosition(new Vector3D(4372.71,2861.67,-2820.26));
    }else if (arg == "flip")
    {
        gyroECU.rotateToHeading(controllers[0].WorldMatrix.Forward * -1);
    }
    else if (arg == "left")
    {
        gyroECU.rotateToHeading(controllers[0].WorldMatrix.Left * -1);
    }
    gyroP.start();
    gyroECU.update();
    gyroP.stop();
}





/// <summary>
        /// Whip's GetRotationAnglesSimultaneous - Last modified: 07/05/2020
        /// Gets axis angle rotation and decomposes it upon each cardinal axis.
        /// Has the desired effect of not causing roll oversteer. Does NOT use
        /// sequential rotation angles.
        /// Set desiredUpVector to Vector3D.Zero if you don't care about roll.
        /// Dependencies:
        /// SafeNormalize
        /// </summary>
public static void GetRotationAnglesSimultaneous(Vector3D desiredForwardVector, Vector3D desiredUpVector, MatrixD worldMatrix, out double yaw, out double pitch, out double roll)
{
    desiredForwardVector = SafeNormalize(desiredForwardVector);

    MatrixD transposedWm;
    MatrixD.Transpose(ref worldMatrix, out transposedWm);
    Vector3D.Rotate(ref desiredForwardVector, ref transposedWm, out desiredForwardVector);
    Vector3D.Rotate(ref desiredUpVector, ref transposedWm, out desiredUpVector);

    Vector3D leftVector = Vector3D.Cross(desiredUpVector, desiredForwardVector);
    Vector3D axis;
    double angle;
    if (Vector3D.IsZero(desiredUpVector) || Vector3D.IsZero(leftVector))
    {
        axis = new Vector3D(desiredForwardVector.Y, -desiredForwardVector.X, 0);
        angle = Math.Acos(MathHelper.Clamp(-desiredForwardVector.Z, -1.0, 1.0));
    }
    else
    {
        leftVector = SafeNormalize(leftVector);
        Vector3D upVector = Vector3D.Cross(desiredForwardVector, leftVector);

        // Create matrix
        MatrixD targetMatrix = MatrixD.Zero;
        targetMatrix.Forward = desiredForwardVector;
        targetMatrix.Left = leftVector;
        targetMatrix.Up = upVector;

        axis = new Vector3D(targetMatrix.M23 - targetMatrix.M32,
                            targetMatrix.M31 - targetMatrix.M13,
                            targetMatrix.M12 - targetMatrix.M21);

        double trace = targetMatrix.M11 + targetMatrix.M22 + targetMatrix.M33;
        angle = Math.Acos(MathHelper.Clamp((trace - 1) * 0.5, -1, 1));
    }

    if (Vector3D.IsZero(axis))
    {
        angle = desiredForwardVector.Z < 0 ? 0 : Math.PI;
        yaw = angle;
        pitch = 0;
        roll = 0;
        return;
    }

    axis = SafeNormalize(axis);
    yaw = -axis.Y * angle;
    pitch = axis.X * angle;
    roll = -axis.Z * angle;
}

public static Vector3D SafeNormalize(Vector3D a)
{
    if (Vector3D.IsZero(a))
        return Vector3D.Zero;

    if (Vector3D.IsUnit(ref a))
        return a;

    return Vector3D.Normalize(a);
}
static double d180bypi = (180 / Math.PI);
public static double ConvertRadiansToDegrees(double radians)
{
    double degrees = d180bypi * radians;
    return (degrees);
}
static double dpiby180 = (Math.PI / 180);
public static double ConvertDegreesToRadians(double degrees)
{
    double radians = dpiby180 * degrees;
    return (radians);
}
public static string v2ss(Vector3D v)
{
    return "<" + v.X.ToString("0.0000") + "," + v.Y.ToString("0.0000") + "," + v.Z.ToString("0.0000") + ">";
}
static public double VectorAngleBetween(Vector3D a, Vector3D b) //returns radians
{
    if (a.LengthSquared() == 0 || b.LengthSquared() == 0)
        return 0;
    else
        return Math.Acos(MathHelper.Clamp(a.Dot(b) / a.Length() / b.Length(), -1, 1));
}

public static void ApplyGyroOverride(double pitch_speed, double yaw_speed, double roll_speed, List<IMyGyro> gyro_list, IMyTerminalBlock reference)
{
    var rotationVec = new Vector3D(-pitch_speed, yaw_speed, roll_speed); //because keen does some weird stuff with signs
    var shipMatrix = reference.WorldMatrix;
    var relativeRotationVec = Vector3D.TransformNormal(rotationVec, shipMatrix);

    foreach (var thisGyro in gyro_list)
    {
        var gyroMatrix = thisGyro.WorldMatrix;
        var transformedRotationVec = Vector3D.TransformNormal(relativeRotationVec, Matrix.Transpose(gyroMatrix));
        //log("t:"+ transformedRotationVec.X.ToString("0.00") + "," + transformedRotationVec.Y.ToString("0.00") + "," + transformedRotationVec.Z.ToString("0.00"), LT.LOG_N);
        thisGyro.Pitch = (float)transformedRotationVec.X;
        thisGyro.Yaw = (float)transformedRotationVec.Y;
        thisGyro.Roll = (float)transformedRotationVec.Z;
        thisGyro.GyroOverride = true;
    }
}

public class GyroECU4
{
    public static double gyroMaxRPM = 3.1415;

    public double angleMultiplier = 1;

    List<IMyGyro> gyros = new List<IMyGyro>();
    IMyTerminalBlock fwd_ref;
    IMyShipController ctrl;

    public GyroECU4(List<IMyGyro> g, IMyTerminalBlock refer, IMyShipController c) : base()
    {
        angleMultiplier = c.CubeGrid.GridSizeEnum == VRage.Game.MyCubeSize.Small ? 2 : 1;
        gyroMaxRPM *= angleMultiplier;
        fwd_ref = refer;
        gyros = g;
        ctrl = c;
        lastAngleRoll = lastAnglePitch = lastAngleYaw = 0;
        lMPTRoll = lMPTPitch = lMPTYaw = 0;

        //debug telemetry
        ybrakes=pbrakes=rbrakes = 0;
    }
    double lastAngleRoll, lastAnglePitch, lastAngleYaw;
    double lMPTRoll, lMPTPitch, lMPTYaw;
    double modr, modp, mody = 0;

    int ybrakes, pbrakes, rbrakes = 0;



    bool active = false;
    Vector3D targetPosition;
    Vector3D targetVelocity;

    Vector3D targetHeading;

    int startTick;

    private void flush()
    {
        if (!active)
        {
            active = true;
            startTick = tick;
            lastAngleRoll = lastAnglePitch = lastAngleYaw = 0;
            lMPTRoll = lMPTPitch = lMPTYaw = 0;
            modr = modp = mody = 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;
    }
    public void rotateToHeading(Vector3D heading)
    {
        if (!active) log("GECU test init rth", LT.LOG_N);
        flush();
        targetPosition = targetVelocity = Vector3D.Zero;
        targetHeading = heading;
    }

    /*public void activate(Vector3D tp)//NOTE: targp too close to the reference block can cause problems, due to CoM fluctuations.
            {
                active = true;
                startTick = tick;
                targetPosition = tp;

                targetHeading = Vector3D.Normalize(targetPosition - ctrl.GetPosition());

                lastAngleRoll = lastAnglePitch = lastAngleYaw = 0;
                lMPTRoll = lMPTPitch = lMPTYaw = 0;
                modr = modp = mody = 0;
                log("test init", LT.LOG_N);
                gProgram.Me.CustomData = "";
            }*/
    string dbg = "";
    private void calculateAxisSpecificData(double now, ref double prior, ref double lastMPT, ref double mod, out bool ontarg, out bool braking, string p="")//, out double ticksToStop, out double ticksToTarget)
    {
        ontarg = false;

        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);

        if (!closing)
        {
            lastMPT = 0.0001;
            mod = MathHelper.EPSILON;
        }
        else lastMPT = radMovedPerTick;

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

        if (Math.Abs(now) < error_threshold)
        {
            braking = true;
            if(radMovedPerTick < minVelThreshold)ontarg = true;
        }
        /*
                dbg += p + "rmpt=" + ConvertRadiansToDegrees(radMovedPerTick) + "\n";
                dbg += p + "lmpt=" + ConvertRadiansToDegrees(lastMPT) + "\n";
                dbg += p + "deg=" + ConvertRadiansToDegrees(now) + "\n";
                dbg += p + "closing=" + closing + "\n";
                dbg += p + "braking=" + braking + "\n";
                dbg += p + "ticksToTarget:" + ticksToTarget.ToString("0.00") + "\n";
                dbg += p + "initVel:" + ConvertRadiansToDegrees(initVel).ToString("0.00") + "\n";
                dbg += p + "rateOfDecel:" + ConvertRadiansToDegrees(rateOfDecel).ToString("0.00") + "\n";
                dbg += p + "ticksToStop:" + ticksToStop.ToString("0.00") + "\n";*/

        prior = now;
    }

    double error_threshold = ConvertDegreesToRadians(0.25);
    double minVelThreshold = ConvertDegreesToRadians(0.01);

    double nobrake_threshold = ConvertDegreesToRadians(45);
    double amp_threshold = ConvertDegreesToRadians(5);


    int minTicksOnTarget = 5;

    int ticksOnTarget = 0;

    public void update(bool up=false)
    {
        if (active)
        {
            dbg = "";

            Vector3D heading = fwd_ref.WorldMatrix.Forward;


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

            double pitch, yaw, roll;
            GetRotationAnglesSimultaneous(targetHeading, up ? fwd_ref.WorldMatrix.Up : Vector3D.Zero, fwd_ref.WorldMatrix, out yaw, out pitch, out roll);
            //string dbg = "";
            //dbg += pitch.ToString("0.0")+","+yaw.ToString("0.0"),roll.ToString("0.0")
            bool yT, pT, rT, yB, pB, rB;
            calculateAxisSpecificData(roll, ref lastAngleRoll, ref lMPTRoll, ref modr, out rT, out rB, "r-");
            calculateAxisSpecificData(pitch, ref lastAnglePitch, ref lMPTPitch, ref modp, out pT, out pB,"p-");
            calculateAxisSpecificData(yaw, ref lastAngleYaw, ref lMPTYaw, ref mody, out yT, out yB,"y-");

            /*ybrakes += yB?1:0;
                    pbrakes += pB ? 1 : 0;
                    rbrakes += rB ? 1 : 0;

                    dbg += "ybrakes:" + ybrakes + "\n";
                    dbg += "pbrakes:" + pbrakes + "\n";
                    dbg += "rbrakes:" + rbrakes + "\n";*/

            Vector3D a_impulse = new Vector3D(pB ? 0 : pitch, yB ? 0 : yaw, rB ? 0 : roll);
            dbg += v2ss(d180bypi * a_impulse) + "\n";
            if (a_impulse != Vector3D.Zero)
            {
                var m = a_impulse.AbsMax();
                if (m > amp_threshold) m = gyroMaxRPM;
                else m = m / amp_threshold * gyroMaxRPM;


                a_impulse = a_impulse / a_impulse.AbsMax() * m;
            }
            dbg += v2ss(d180bypi * a_impulse) + "\n";

            ApplyGyroOverride(a_impulse.X, a_impulse.Y, a_impulse.Z, gyros, fwd_ref);

            if (yT && pT && rT)
            {
                ticksOnTarget += 1;
            }
            else ticksOnTarget = 0;

            if (ticksOnTarget > minTicksOnTarget)
            {
                active = false;
                foreach (var g in gyros) g.GyroOverride = false;
                log("GECU shutdown", LT.LOG_N);
                log("time: "+(tick - startTick) + "t (" + ((double)(tick - startTick) / 60).ToString("0.0") + "s)", LT.LOG_N);
            }

            //debug nonsense
            //if(gProgram.statusLog != null)gProgram.statusLog.WriteText(dbg);
        }
    }
}

public class Profiler
{


    static bool PROFILING_ENABLED = true;
    static List<Profiler> profilers = new List<Profiler>();
    const int mstracklen = 60;
    double[] mstrack = new double[mstracklen];
    double msdiv = 1.0d / mstracklen;
    int mscursor = 0;
    DateTime start_time = DateTime.MinValue;
    string Name = "";
    string pre = "";
    string post = "";
    int _ticks_between_calls = 1;
    int ltick = int.MinValue;
    //..int callspertick = 1;

    static int base_sort_position_c = 0;
    int base_sort_position = 0;

    bool nevercalled = true;
    //bool closed = true;
    public int getSortPosition()
    {
        if (nevercalled) return int.MaxValue;
        int mult = (int)Math.Pow(10, 8 - (depth * 2));
        if (parent != null) return parent.getSortPosition() + (base_sort_position * mult);

        return base_sort_position * mult;
        //if (parent != null) return parent.getSortPosition() + (base_sort_position / (10 * depth));
        //return base_sort_position;// * 1000;
    }
    /*public Profiler(string name, int ticks_between_calls) : this(name)
            {
                _ticks_between_calls = ticks_between_calls;
            }*/
    static int basep = (int)Math.Pow(10, 5);
    public Profiler(string name)
    {
        if (PROFILING_ENABLED)
        {
            Name = name;
            profilers.Add(this);
            base_sort_position = base_sort_position_c;
            base_sort_position_c += 1;
        }
    }
    public void s()
    {
        start();
    }
    public void e()
    {
        stop();
    }


    static List<Profiler> stack = new List<Profiler>();

    Profiler parent = null;
    int depth = 0;

    bool adding = false;
    public void start()
    {
        if (PROFILING_ENABLED)
        {
            //closed = false;
            nevercalled = false;
            if (tick != ltick)
            {
                if (_ticks_between_calls == 1 && ltick != int.MinValue)
                {
                    _ticks_between_calls = tick - ltick;
                }
                ltick = tick;
                //callspertick = 1;
                adding = false;
            }
            else
            {
                adding = true;
            }
            if (depth == 0) depth = stack.Count;
            if (depth > 11) depth = 11;
            if (stack.Count > 0 && parent == null) parent = stack[stack.Count - 1];
            stack.Add(this);
            start_time = DateTime.Now;
        }
    }
    double lastms = 0;
    double average = 0;


    /// <summary>
            /// records a fake ms consumption for this timeframe - for tests or demo
            /// </summary>
    public double FAKE_stop(double fakems)
    {
        return stop(fakems);
    }
    /// <summary>
            /// adds the elapsed time since start() to the records
            /// </summary>
    public double stop()
    {
        double time = 0;
        if (PROFILING_ENABLED)
        {
            //closed = true;
            time = (DateTime.Now - start_time).TotalMilliseconds;
        }
        return stop(time);
    }

    private double stop(double _ms)
    {
        double time = 0;
        if (PROFILING_ENABLED)
        {
            time = _ms;

            stack.Pop();
            if (parent != null)
            {
                depth = parent.depth + 1;
            }

            //if(!adding)mscursor = (mscursor + 1) % mstracklen;


            if (!adding) mstrack[mscursor] = 0;
            mstrack[mscursor] += time;
            if (!adding) mscursor = (mscursor + 1) % mstracklen;

            average = 0d;
            foreach (double ms in mstrack) average += ms;
            average *= msdiv;
            average /= _ticks_between_calls;
            lastms = time;
        }
        return time;
    }
    /// <summary>
            /// generates a monospaced report text. If called every tick, every 120 ticks it will recalculate treeview data.
            /// </summary>
    //the treeview can be initially inaccurate as some profilers might not be called every tick, depending on program architecture
    public string getReport()
    {
        if (PROFILING_ENABLED)
        {
            if (tick % 120 == 25)//recalculate hacky treeview data, delayed by 25 ticks from program start
            {
                try
                {
                    profilers.Sort(delegate (Profiler x, Profiler y)
                    {
                        return x.getSortPosition().CompareTo(y.getSortPosition());
                    });
                }
                catch (Exception) { }

                for (int i = 0; i < profilers.Count; i++)
                {
                    Profiler p = profilers[i];

                    p.pre = "";
                    if (p.depth > 0 && p.parent != null)
                    {
                        bool parent_has_future_siblings = false;
                        bool has_future_siblings_under_parent = false;
                        for (int b = i + 1; b < profilers.Count; b++)
                        {
                            if (profilers[b].depth == p.parent.depth) parent_has_future_siblings = true;
                            if (profilers[b].depth == p.depth) has_future_siblings_under_parent = true;
                            if (profilers[b].depth < p.depth) break;

                        }
                        while (p.pre.Length < p.parent.depth)
                        {
                            if (parent_has_future_siblings) p.pre += "│";
                            else p.pre += " ";
                        }
                        bool last = false;

                        if (!has_future_siblings_under_parent)
                        {
                            if (i < profilers.Count - 1)
                            {
                                if (profilers[i + 1].depth != p.depth) last = true;
                            }
                            else last = true;
                        }
                        if (last) p.pre += "└";
                        else p.pre += "├";

                        while (p.pre.Length < p.depth) p.pre += "─";

                        //if(!p.closed) p.pre = p.closed + p.pre;
                        //p.pre = p.depth + p.pre;
                    }
                }
                int mlen = 0;
                foreach (Profiler p in profilers) if (p.pre.Length + p.Name.Length > mlen) mlen = p.pre.Length + p.Name.Length;
                foreach (Profiler p in profilers)
                {
                    p.post = "";
                    int l = p.pre.Length + p.Name.Length + p.post.Length;
                    if (l < mlen) p.post = new string('_', mlen - l);
                }
            }
            if (nevercalled) return "!!!!" + Name + "!!!!: NEVER CALLED!";

            /*string bsp = ""+getSortPosition();
                    if (bsp.Length < 8) bsp = new string('0',8 - bsp.Length) + bsp;*/

            // return /*bsp+ */
            return pre + Name + post + ": " + lastms.ToString("0.00") + ";" + average.ToString("0.00");
        }
        return "";
    }
    static public string getAllReports()
    {
        string r = "";
        if (PROFILING_ENABLED)
        {
            foreach (Profiler watch in profilers)
            {
                r += watch.getReport() + "\n";
            }
        }
        if (stack.Count > 0)
        {
            r += "profile stack error:\n";
            r += stack.Count + "\n";
            foreach (var s in stack)
            {
                r += s.Name + ",";
            }
        }
        return r;
    }
}