Fix CoastBefore behavior

replaces #1675

Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>

MechJeb2/MechJebLib/PVG/Solution.cs

@@ -275,7 +275,7 @@ public double StageTimeLeft(double t)
                 u = x.PV.normalized;
             }
 
-            (double pitch, double heading) = MechJebLib.Core.Maths.ECIToPitchHeading(x.R, u);
+            (double pitch, double heading) = Maths.ECIToPitchHeading(x.R, u);
             return (pitch, heading);
         }
 
@@ -296,14 +296,14 @@ public string TerminalString()
         {
             (V3 rf, V3 vf) = TerminalStateVectors();
 
-            (double sma, double ecc, double inc, double lan, double argp, double tanom, _) = MechJebLib.Core.Maths.KeplerianFromStateVectors(_mu,
+            (double sma, double ecc, double inc, double lan, double argp, double tanom, _) = Maths.KeplerianFromStateVectors(_mu,
                 rf, vf);
 
-            double peR = MechJebLib.Core.Maths.PeriapsisFromKeplerian(sma, ecc);
-            double apR = MechJebLib.Core.Maths.ApoapsisFromKeplerian(sma, ecc);
+            double peR = Maths.PeriapsisFromKeplerian(sma, ecc);
+            double apR = Maths.ApoapsisFromKeplerian(sma, ecc);
             double peA = peR - _rbody;
             double apA = apR <= 0 ? apR : apR - _rbody;
-            double fpa = MechJebLib.Core.Maths.FlightPathAngle(rf, vf);
+            double fpa = Maths.FlightPathAngle(rf, vf);
             double rT = rf.magnitude - _rbody;
             double vT = vf.magnitude;
 
@@ -323,11 +323,14 @@ private int IndexForTbar(double tbar)
             return _tmax.Count - 1;
         }
 
-        public int IndexForKSPStage(int kspStage)
+        public int IndexForKSPStage(int kspStage, bool includeCoasts = true)
         {
             for (int i = 0; i < Phases.Count; i++)
             {
-                if (Phases[i].KSPStage == kspStage)
+                if (Phases[i].KSPStage != kspStage)
+                    continue;
+
+                if (!Phases[i].Coast || includeCoasts)
                     return i;
             }
 

MechJeb2/MechJebModuleGuidanceController.cs

@@ -160,7 +160,16 @@ private void HandleTerminal()
             if (vessel.currentStage != _ascentSettings.OptimizeStage && Solution.Tgo(vesselState.time) > 10)
                 return;
 
-            int solutionIndex = Solution.IndexForKSPStage(vessel.currentStage);
+            // The includeCoast: false flag here is to skip a coast which is in the past in the Solution when
+            // we are ending the coast and the optimizer hasn't run the solution, but CoastBefore is set so
+            // that both the coast and burn have the same KSPStage.  So we want the index of the current burn
+            // and not the index of the first matching KSPStage in the Solution which is the coast.  Might
+            // also consider modifying APIs like IndexForKSPStage to omit stages which are in the past -- but
+            // I have concerns about that with residuals where you may currently be in a burning stage which
+            // is in the "past" in the Solution but you're burning down residuals and you don't know when
+            // the stage will actually run out (assuming it isn't a burn before a coast or an optimized burntime
+            // so that we burn past the end of the stage and into whatever residuals are available).
+            int solutionIndex = Solution.IndexForKSPStage(vessel.currentStage, includeCoasts: false);
             if (solutionIndex < 0)
                 return;