plotter prototype

using Sandbox.Game.EntityComponents;
using Sandbox.ModAPI.Ingame;
using Sandbox.ModAPI.Interfaces;
using SpaceEngineers.Game.ModAPI.Ingame;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Collections.Immutable;
using System.Diagnostics;
using System.Linq;
using System.Security.Policy;
using System.Text;
using VRage;
using VRage.Collections;
using VRage.Game;
using VRage.Game.Components;
using VRage.Game.GUI.TextPanel;
using VRage.Game.ModAPI.Ingame;
using VRage.Game.ModAPI.Ingame.Utilities;
using VRage.Game.ObjectBuilders.Definitions;
using VRageMath;
using static IngameScript.Program;

namespace IngameScript
{
    partial class Program : MyGridProgram
    {

        static string extract(string s, string prefix, string suffix="")
        {
            var i = s.IndexOf(prefix);
            if (i >= 0)
            {
                s = s.Substring(i + prefix.Length);
            }
            else
            {
                return "";
            }
            if (suffix != "")
            {
                i = s.IndexOf(suffix);
                if (i >= 0)
                {
                    s = s.Substring(0, i);
                }
                else
                {
                    return "";
                }
            }
            return s;
        }


        public class SpeedSector
        {
            public Vector3D pos;
            public double radius;
            public string name;
            public string color;
        }
        public class GPS
        {
            public string name;
            public Vector3D pos;
            public string color;
            public GPS()
            {

            }
            public GPS(string str)
            {
                gps(str);
            }
            public GPS(GPS c)
            {
                name = c.name;
                pos = c.pos;
                color = c.color;
            }

            public string gps()
            {
                return "GPS:" + name + ":" + pos.X.ToString("0.0") + ":" + pos.Y.ToString("0.0") + ":" + pos.Z.ToString("0.0") + ":"+color;//..""GPS:Mars Station:1200000:120000:500000:#FF90FF00:"
            }
            public void gps(string s)
            {
                var t = s.Split(':');
                if (t.Length > 3)
                {
                    pos = new Vector3D(float.Parse(t[2]), float.Parse(t[3]), float.Parse(t[4]));
                    name = t[1];
                    color = t[5];
                }
            }
        }


        public class Plotter
        {
            public Plotter()
            {
                parsesectors();
                mkspheres();
            }

            string nexus_sectors = "GPS:Far Gate Zone - (R:80km):-9999999:-9999999:9999999:#FFFFFF00:\r\nGPS:Sol Gate - (R:80km):-3900000:3600000:-200000:#FFFFFF00:\r\nGPS:Uranus - (R:500km):-4200000:3000000:-250000:#FFFFFF00:\r\nGPS:M2 - (R:300km):1550000:-800000:0:#FFFFFF00:\r\nGPS:Vesta - (R:300km):-1400000:1000000:-50000:#FFFFFF00:\r\nGPS:Tycho - (R:300km):1400000:1000000:-50000:#FFFFFF00:\r\nGPS:T1 - (R:300km):1725000:0:0:#FFFFFF00:\r\nGPS:Pallas - (R:300km):0:-1725000:100000:#FFFFFF00:\r\nGPS:P1 - (R:300km):-800000:-1550000:0:#FFFFFF00:\r\nGPS:M1 - (R:300km):800000:-1550000:0:#FFFFFF00:\r\nGPS:E1 - (R:300km):-1725000:0:0:#FFFFFF00:\r\nGPS:E2 - (R:300km):-1500000:-900000:0:#FFFFFF00:\r\nGPS:C1 - (R:300km):600000:1600000:0:#FFFFFF00:\r\nGPS:C2 - (R:300km):-600000:1600000:0:#FFFFFF00:\r\nGPS:Earth - (R:400km):-800000:100000:80000:#FFFFFF00:\r\nGPS:Mars - (R:400km):750000:-750000:-80000:#FFFFFF00:\r\nGPS:Trojan Cluster - (R:400km):1600000:-2700000:445350:#FFFFFF00:\r\nGPS:Greek Cluster - (R:400km):-1600000:-2700000:-334490:#FFFFFF00:\r\nGPS:Ceres - (R:400km):0:2350000:100000:#FFFFFF00:\r\nGPS:Saturn - (R:750km):2800000:2000000:-100000:#FFFFFF00:\r\nGPS:Jupiter - (R:750km):0:-3200000:0:#FFFFFF00:\r\nGPS:Inner High Speed Zone - (R:4400km):0:0:0:#FFFFFF00:";
            public List<SpeedSector> sectors = new List<SpeedSector>();
            void parsesectors()
            {
                sectors.Clear();
                string[] lines = nexus_sectors.Replace("\r", "").Split('\n');
                foreach (var l in lines)
                {
                    if (l.ToLower().Contains("high speed")) continue;
                    string n = l.Replace("R:", "R;");

                    SpeedSector s = new SpeedSector();
                    s.pos = new GPS(n).pos;
                    s.radius = 0;
                    double.TryParse(extract(n, "R;", "km"), out s.radius);
                    if (s.radius != 0)
                    {
                        s.radius *= 1000;
                        s.name = n.Split(':')[1].Split('-')[0].Trim();
                        sectors.Add(s);
                    }
                }
            }
            List<BoundingSphereD> sectorSpheres = new List<BoundingSphereD>();
            void mkspheres()
            {
                foreach (var z in sectors)
                {
                    sectorSpheres.Add(new BoundingSphereD(z.pos, z.radius));
                }
            }

            public SpeedSector getSector(Vector3D p)
            {
                foreach (var sector in sectors)
                {
                    if (Vector3.DistanceSquared(p, sector.pos) < sector.radius * sector.radius) return sector;
                }
                return null;
            }

            public List<GPS> calcroute(GPS start, GPS end, double pad)
            {
                List<GPS> route = new List<GPS>();
                var sa = getSector(start.pos);
                var sb = getSector(end.pos);
                if (sa != null && sb == sa) return new List<GPS>(new GPS[] { start, end });

                if (sa != null)
                {
                    route.Add(closest_highspeed(start, pad, sa));
                }
                else route.Add(start);

                if (sb != null)
                {
                    route.Add(closest_highspeed(end, pad, sb));
                }
                else route.Add(end);

                for (int i = 0; i < route.Count - 1;)
                {
                    GPS a = route[i];
                    GPS b = route[i + 1];

                    Vector3D hit = Vector3D.Zero;
                    var hitsector = checkObjectIntersect(a.pos, b.pos, out hit);

                    if(hitsector != null)
                    {
                        GPS hitgps = new GPS(a);
                        hitgps.pos = hit;
                        GPS shoved = closest_highspeed(hitgps, pad);
                        clog("intersected " + hitsector.name + " zone, adding divert at closest approach, " + (a.pos - shoved.pos).Length() + "m out at " + (shoved.pos).Length());
                        route.Insert(i + 1, shoved);
                        continue;
                    }
                    i++;
                }

                if(route[0] != start)
                {
                    route.Insert(0, start);
                }
                if (route[route.Count-1] != end)
                {
                    route.Add(end);
                }

                return route;
            }

            Vector3D rayHitPos(Vector3D start, Vector3D end, BoundingSphereD sphere)
            {
                var testLine = new RayD(start, Vector3D.Normalize(end - start));
                double? res = testLine.Intersects(sphere);
                if (res.HasValue)
                {
                    Vector3D hp1 = start + (Vector3D.Normalize(end - start) * res.Value);
                    return hp1;
                }
                return Vector3D.Zero;
            }



            SpeedSector checkObjectIntersect(Vector3D start, Vector3D end, out Vector3D hitpos)
            {
                SpeedSector r = null;
                int i = 0;
                foreach (var z in sectors)
                {

                    var sphere = sectorSpheres[i];

                    Vector3D hp1 = rayHitPos(start, end, sphere);
                    if (hp1 != Vector3D.Zero)
                    {
                        Vector3D hp2 = rayHitPos(end, start, sphere);
                        if (hp2 != Vector3D.Zero)
                        {
                            //not sure what happening here exactly, but it's possible to have a ray hit that isn't reversible, so check hp2
                            //this occurred on the tycho-vesta route impacting "jupiter" despite making no sense
                            hitpos = (hp1 + hp2) * 0.5;
                            clog("hitpos is in " + sectors[i].name);// _getZoneName(AllSpeedZones[i]));// ;
                            clog("hitpos is reporting " + getSector(hitpos)?.name);

                            clog("hp1 m " + hp1.Length());
                            clog("hp2 m " + hp1.Length());
                            clog("hp m " + hitpos.Length());

                            return z;
                        }
                        else
                        {
                            //clog("no hit on " + z.name);
                        }
                    }
                    else
                    {
                        //clog("no hit on " + z.name);
                    }
                    i += 1;
                }
                hitpos = Vector3D.Zero;
                return r;
            }

            static int cnt = 0;
            GPS closest_highspeed(GPS gps, double padding, SpeedSector zone = null)
            {
                SpeedSector s_z = zone;
                if (s_z == null) s_z = getSector(gps.pos);
                if (s_z == null) return null;

                Vector3D pushed_out = s_z.pos + (Vector3D.Normalize(gps.pos - s_z.pos) * (s_z.radius + padding));

                GPS ngps = new GPS(gps);
                ngps.name = cnt + "";
                cnt++;
                ngps.pos = pushed_out;
                return ngps;
            }
        }

        public Plotter plotter = new Plotter();
    }
}