context: using System;
  using KSP.UI.Screens;
  using UnityEngine;
  
  namespace MuMech
  {
      //Todo: -reimplement measurement of LPA
      //      -Figure out exactly how throttle-limiting should work and interact
      //       with the global throttle-limit option
      public class MechJebModuleAscentAutopilot : ComputerModule
      {
          public MechJebModuleAscentAutopilot(MechJebCore core) : base(core) { }
  
          public string status = "";
  
          //input parameters:
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public int ascentPathIdx;
          // this is not persisted, the ascentPathIdx controls it very indirectly (via being set from AscentGuidance)
          public MechJebModuleAscentBase ascentPath;
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public EditableDoubleMult desiredOrbitAltitude = new EditableDoubleMult(100000, 1000);
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public double desiredInclination = 0.0;
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public bool autoThrottle = true;
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public bool correctiveSteering = false;
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public EditableDouble correctiveSteeringGain = 0.6; //control gain
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public bool forceRoll = true;
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public EditableDouble verticalRoll = new EditableDouble(90);
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public EditableDouble turnRoll = new EditableDouble(90);
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public bool autodeploySolarPanels = true;
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public bool autoDeployAntennas = true;
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public bool skipCircularization = false;
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public bool _autostage = true;
          public bool autostage
          {
              get { return _autostage; }
              set
              {
                  bool changed = (value != _autostage);
                  _autostage = value;
                  if (changed)
                  {
                      if (_autostage && this.enabled) core.staging.users.Add(this);
                      if (!_autostage) core.staging.users.Remove(this);
                  }
              }
          }
  
  
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public bool limitAoA = true;
  
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public EditableDouble maxAoA = 5;
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public EditableDoubleMult aoALimitFadeoutPressure = new EditableDoubleMult(2500);
          public bool limitingAoA = false;
  
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public EditableDouble launchPhaseAngle = 0;
  
          [Persistent(pass = (int)(Pass.Type | Pass.Global))]
          public EditableDouble launchLANDifference = 0;
  
          [Persistent(pass = (int)(Pass.Global))]
          public EditableInt warpCountDown = 11;
  
          public bool timedLaunch = false;
          public double launchTime = 0;
  
          public double currentMaxAoA = 0;
  
          public double launchLatitude = 0 ;
  
          // useful to track time since launch event (works around bugs with physics starting early and vessel.launchTime being wildly off)
          // FIXME?: any time we lift off from a rock this should probably be set to that time?
          public double launchStarted;
  
          public double tMinus
          {
              get { return launchTime - vesselState.time; }
          }
  
          //internal state:
          enum AscentMode { PRELAUNCH, ASCEND, CIRCULARIZE };
          AscentMode mode;
          bool placedCircularizeNode = false;
          private double lastTMinus = 999;
  
          // wiring for launchStarted
          public void OnLaunch(EventReport report)
          {
              launchStarted = vesselState.time;
              Debug.Log("[MechJebModuleAscentAutopilot] LaunchStarted = " + launchStarted);
          }
  
          // wiring for launchStarted
          public override void OnStart(PartModule.StartState state)
          {
              launchStarted = -1;
              GameEvents.onLaunch.Add(OnLaunch);
          }
  
          private void FixupLaunchStart()
          {
              // continuously update the launchStarted time if we're sitting on the ground on the water anywhere (once we are not, then we've launched)
              if ( vessel.situation == Vessel.Situations.LANDED || vessel.situation == Vessel.Situations.PRELAUNCH || vessel.situation == Vessel.Situations.SPLASHED )
                  launchStarted = vesselState.time;
          }
  
          // various events can cause launchStarted to be before or after vessel.launchTime, but the most recent one is so far always the most accurate
          // (physics wobbles can start vessel.launchTime (KSP's zero MET) early, while staging before engaging the autopilot can cause launchStarted to happen early)
          // this will only be valid AFTER launching
          public double MET { get { return vesselState.time - ( launchStarted > vessel.launchTime ? launchStarted : vessel.launchTime ); } }
  
          public override void OnModuleEnabled()
          {
              mode = AscentMode.PRELAUNCH;
  
              placedCircularizeNode = false;
  
              launchLatitude = vesselState.latitude;
  
              core.attitude.users.Add(this);
              core.thrust.users.Add(this);
              if (autostage) core.staging.users.Add(this);
          }
  
          public override void OnModuleDisabled()
          {
              core.attitude.attitudeDeactivate();
              if (!core.rssMode)
                  core.thrust.ThrustOff();
              core.thrust.users.Remove(this);
              core.staging.users.Remove(this);
  
              if (placedCircularizeNode) core.node.Abort();
  
              status = "Off";
          }
  
          public void StartCountdown(double time)
          {
              timedLaunch = true;
              launchTime = time;
              lastTMinus = 999;
          }
  
          public override void OnFixedUpdate()
          {
              FixupLaunchStart();
              if (timedLaunch)
              {
                  if (tMinus < 3*vesselState.deltaT || (tMinus > 10.0 && lastTMinus < 1.0))
                  {
                      if (enabled && vesselState.thrustAvailable < 10E-4) // only stage if we have no engines active
                          StageManager.ActivateNextStage();
                      timedLaunch = false;
                  }
                  else
                  {
                      if (core.node.autowarp)
                          core.warp.WarpToUT(launchTime - warpCountDown);
                  }
                  lastTMinus = tMinus;
              }
          }
  
          public override void Drive(FlightCtrlState s)
          {
              limitingAoA = false;
  
              switch (mode)
              {
                  case AscentMode.PRELAUNCH:
                      DrivePrelaunch(s);
                      break;
  
                  case AscentMode.ASCEND:
                      DriveAscent(s);
                      break;
  
                  case AscentMode.CIRCULARIZE:
                      DriveCircularizationBurn(s);
                      break;
              }
  
          }
  
          void DriveDeployableComponents(FlightCtrlState s)
          {
              if (autodeploySolarPanels)
              {
                  if (vesselState.altitudeASL > mainBody.RealMaxAtmosphereAltitude())
                  {
                      core.solarpanel.ExtendAll();
                  }
                  else
                  {
                      core.solarpanel.RetractAll();
                  }
              }
  
              if (autoDeployAntennas)
              {
                  if (vesselState.altitudeASL > mainBody.RealMaxAtmosphereAltitude())
                      core.antennaControl.ExtendAll();
                  else
                      core.antennaControl.RetractAll();
              }
          }
  
          void DrivePrelaunch(FlightCtrlState s)
          {
              if (vessel.LiftedOff() && !vessel.Landed) {
                  status = "Vessel is not landed, skipping pre-launch";
                  mode = AscentMode.ASCEND;
                  return;
              }
  
              if (autoThrottle) {
                  Debug.Log("prelaunch killing throttle");
                  core.thrust.targetThrottle = 0.0f;
              }
  
              core.attitude.AxisControl(false, false, false);
  
              if (timedLaunch && tMinus > 10.0)
              {
                  status = "Pre Launch";
                  return;
              }
  
              if (autodeploySolarPanels && mainBody.atmosphere)
              {
                  core.solarpanel.RetractAll();
                  if (core.solarpanel.AllRetracted()) {
                      Debug.Log("Prelaunch -> Ascend");
                      mode = AscentMode.ASCEND;
                  }
                  else
                  {
                      status = "Retracting solar panels";
                  }
              } else {
                  mode = AscentMode.ASCEND;
              }
          }
  
          void DriveAscent(FlightCtrlState s)
          {
              if (timedLaunch)
              {
                  Debug.Log("Awaiting Liftoff");
                  status = "Awaiting liftoff";
                  core.attitude.AxisControl(false, false, false);
                  return;
              }
  
              DriveDeployableComponents(s);
  
              if ( ascentPath.DriveAscent(s) ) {
                  if (GameSettings.VERBOSE_DEBUG_LOG) { Debug.Log("Remaining in Ascent"); }
                  status = ascentPath.status;
              } else {
                  if (GameSettings.VERBOSE_DEBUG_LOG) { Debug.Log("Ascend -> Circularize"); }
                  mode = AscentMode.CIRCULARIZE;
              }
          }
  
          void DriveCircularizationBurn(FlightCtrlState s)
          {
              if (!vessel.patchedConicsUnlocked() || skipCircularization)
              {
                  this.users.Clear();
                  return;
              }
  
              DriveDeployableComponents(s);
  
              if (placedCircularizeNode)
              {
                  if (vessel.patchedConicSolver.maneuverNodes.Count == 0)
                  {
                      MechJebModuleFlightRecorder recorder = core.GetComputerModule<MechJebModuleFlightRecorder>();
                      if (recorder != null) launchPhaseAngle = recorder.phaseAngleFromMark;
                      if (core.target != null) launchLANDifference = vesselState.orbitLAN - core.target.orbit.LAN;
  
                      //finished circularize
                      this.users.Clear();
                      return;
                  }
              }
              else
              {
                  //place circularization node
                  vessel.RemoveAllManeuverNodes();
                  double UT = orbit.NextApoapsisTime(vesselState.time);
                  //During the circularization burn, try to correct any inclination errors because it's better to combine the two burns.
                  //  For example, if you're about to do a 1500 m/s circularization burn, if you combine a 200 m/s inclination correction
                  //  into it, you actually only spend 1513 m/s to execute combined manuver.  Mechjeb should also do correction burns before
                  //  this if possible, and this can't correct all errors... but it's better then nothing.
                  //   (A better version of this should try to match inclination & LAN if target is specified)
                  // FIXME? this inclination correction is unlikely to be at tha AN/DN and will throw the LAN off with anything other than high
                  // TWR launches from equatorial launch sites -- should probably be made optional (or clip it if the correction is too large).
                  Vector3d inclinationCorrection = OrbitalManeuverCalculator.DeltaVToChangeInclination(orbit, UT, Math.Abs(desiredInclination));
                  Vector3d smaCorrection = OrbitalManeuverCalculator.DeltaVForSemiMajorAxis(orbit.PerturbedOrbit(UT, inclinationCorrection), UT,
                      desiredOrbitAltitude + mainBody.Radius);
                  Vector3d dV = inclinationCorrection + smaCorrection;
                  vessel.PlaceManeuverNode(orbit, dV, UT);
                  placedCircularizeNode = true;
  
                  core.node.ExecuteOneNode(this);
              }
  
              if (core.node.burnTriggered) status = "Circularizing";
              else status = "Coasting to circularization burn";
          }
      }
  
      public abstract class MechJebModuleAscentMenuBase : DisplayModule
      {
          public MechJebModuleAscentMenuBase(MechJebCore core) : base(core) { }
      }
  
      public abstract class MechJebModuleAscentBase : ComputerModule
      {
          public MechJebModuleAscentBase(MechJebCore core) : base(core) { }
  
          public string status { get; set; }
  
          public MechJebModuleAscentAutopilot autopilot { get { return core.GetComputerModule<MechJebModuleAscentAutopilot>(); } }
  
          public abstract bool DriveAscent(FlightCtrlState s);
  
          public Vector3d thrustVectorForNavball;
  
          //data used by ThrottleToRaiseApoapsis
          float raiseApoapsisLastThrottle = 0;
          double raiseApoapsisLastApR = 0;
          double raiseApoapsisLastUT = 0;
          MovingAverage raiseApoapsisRatePerThrottle = new MovingAverage(3, 0);
  
          //gives a throttle setting that reduces as we approach the desired apoapsis
          //so that we can precisely match the desired apoapsis instead of overshooting it
          protected float ThrottleToRaiseApoapsis(double currentApR, double finalApR)
          {
              float desiredThrottle;
  
              if (currentApR > finalApR + 5.0)
              {
                  desiredThrottle = 0.0F; //done, throttle down
              }
              else if (raiseApoapsisLastUT > vesselState.time - 1)
              {
                  //reduce throttle as apoapsis nears target
                  double instantRatePerThrottle = (orbit.ApR - raiseApoapsisLastApR) / ((vesselState.time - raiseApoapsisLastUT) * raiseApoapsisLastThrottle);
                  instantRatePerThrottle = Math.Max(1.0, instantRatePerThrottle); //avoid problems from negative rates
                  raiseApoapsisRatePerThrottle.value = instantRatePerThrottle;
                  double desiredApRate = (finalApR - currentApR) / 1.0;
                  desiredThrottle = Mathf.Clamp((float)(desiredApRate / raiseApoapsisRatePerThrottle), 0.05F, 1.0F);
              }
              else
              {
                  desiredThrottle = 1.0F; //no recent data point; just use max thrust.
              }
  
              //record data for next frame
              raiseApoapsisLastThrottle = desiredThrottle;
              raiseApoapsisLastApR = orbit.ApR;
              raiseApoapsisLastUT = vesselState.time;
  
              return desiredThrottle;
          }
  
          protected double srfvelPitch() {
              return 90.0 - Vector3d.Angle(vesselState.surfaceVelocity, vesselState.up);
          }
  
          // provides AoA limiting and ground track steering to pitch controllers (possibly should be moved into the attitude controller, but
          // right now it collaborates too heavily with the ascent autopilot)
          //
          protected void attitudeTo(double desiredPitch)
          {
              double desiredHeading = OrbitalManeuverCalculator.HeadingForLaunchInclination(vessel, vesselState, autopilot.desiredInclination);
  
              Vector3d desiredHeadingVector = Math.Sin(desiredHeading * UtilMath.Deg2Rad) * vesselState.east + Math.Cos(desiredHeading * UtilMath.Deg2Rad) * vesselState.north;
  
              Vector3d desiredThrustVector = Math.Cos(desiredPitch * UtilMath.Deg2Rad) * desiredHeadingVector
                  + Math.Sin(desiredPitch * UtilMath.Deg2Rad) * vesselState.up;
  
              thrustVectorForNavball = desiredThrustVector;
  
              desiredThrustVector = desiredThrustVector.normalized;
  
              if (autopilot.limitAoA)
              {
                  float fade = vesselState.dynamicPressure < autopilot.aoALimitFadeoutPressure ? (float)(autopilot.aoALimitFadeoutPressure / vesselState.dynamicPressure) : 1;
                  autopilot.currentMaxAoA = Math.Min(fade * autopilot.maxAoA, 180d);
                  autopilot.limitingAoA = vessel.altitude < mainBody.atmosphereDepth && Vector3.Angle(vesselState.surfaceVelocity, desiredThrustVector) > autopilot.currentMaxAoA;
  
                  if (autopilot.limitingAoA)
                  {
                      desiredThrustVector = Vector3.RotateTowards(vesselState.surfaceVelocity, desiredThrustVector, (float)(autopilot.currentMaxAoA * Mathf.Deg2Rad), 1).normalized;
                  }
              }
  
              double pitch = 90 - Vector3d.Angle(desiredThrustVector, vesselState.up);
              double hdg;
  
              if (pitch > 89.9) {
                  hdg = desiredHeading;
              } else {
                  hdg = MuUtils.ClampDegrees360(UtilMath.Rad2Deg * Math.Atan2(Vector3d.Dot(desiredThrustVector, vesselState.east), Vector3d.Dot(desiredThrustVector, vesselState.north)));
              }
  
              if (autopilot.forceRoll)
              {
                  core.attitude.AxisControl(!vessel.Landed, !vessel.Landed, !vessel.Landed && vesselState.altitudeBottom > 50);
  
                  if ( desiredPitch == 90.0)
                  {
                      core.attitude.attitudeTo(hdg, pitch, autopilot.turnRoll, this);
                  }
                  else
                  {
                      core.attitude.attitudeTo(hdg, pitch, autopilot.verticalRoll, this);
                  }
              }
              else
              {
                  core.attitude.attitudeTo(desiredThrustVector, AttitudeReference.INERTIAL, this);
              }
          }
  
      }
  
      public static class LaunchTiming
      {
          //Computes the time until the phase angle between the launchpad and the target equals the given angle.
          //The convention used is that phase angle is the angle measured starting at the target and going east until
          //you get to the launchpad.
          //The time returned will not be exactly accurate unless the target is in an exactly circular orbit. However,
          //the time returned will go to exactly zero when the desired phase angle is reached.
          public static double TimeToPhaseAngle(double phaseAngle, CelestialBody launchBody, double launchLongitude, Orbit target)
          {
              double launchpadAngularRate = 360 / launchBody.rotationPeriod;
  
gt:             double targetAngularRate = 360.0 / target.period;