// landon.ks - Deorbit and land on a target or waypoint.
// Copyright © 2022 Teviet Creighton
//
// Usage: landon [(TARGET[, NOFF, EOFF])].
//
// Arguments:
//   TARGET (optional)     - "target", "waypoint", or waypoint name.
//   NOFF, EOFF (optional) - extra north and east offsets in m.
//
// This lands using a deorbit burn and gravity turn, adjusting its
// descent to try to land on a specific target (at the expense of some
// extra delta-V).  Terminal guidance takes over during the gravity
// turn, so the script's success at hitting a target will depend
// significantly on whether the initial deorbit places it on a
// suitable trajectory.  It consists of three steps: Maneuver,
// Deorbit, and Descent.
//
// Step 0 (Maneuver): If there is a maneuver node, the script will run
// brn.ks before starting its own maneuvers.  This is called by the
// script (rather than left to the user) to ensure that there is no
// user delay between maneuver execution and deorbiting, giving
// exactly reproducible trajectories.
//
// Step 1 (Deorbit): If the vessel is traveling too fast or is too low
// for a gravity turn descent, this step kills the vessel's tangential
// velocity, keeping vertical velocity roughly constant.  It ends when
// the orbit intersects the ground and velocity is low enough to stop
// using a gravity turn.  If these conditions are already met
// initially, the script will skip this step.
//
// Step 2 (Descent): This is a gravity turn descent followed by an
// ease-down over the last few metres.  For details, see the
// documentation of the script lnd.ks, including variables that can be
// tweaked for efficiency.  Additional deflections are applied to aim
// the projected impact point towards the designated target or
// waypoint (if any).  Depending on how close an unmodified gravity
// turn would come to the target, this may add substantial delta-V or
// could fail altogether.  (CHORUS: "This may add substantial delta-V
// or could fail.")
@LAZYGLOBAL OFF.
PARAMETER tpar IS "". // Target specifier: one of "target" (current
                      // target), "waypoint" (selected waypoint on
                      // list), or the name of a specific waypoint.
PARAMETER noff IS 0.  // Extra landing point offset north, in m.
PARAMETER eoff IS 0.  // Extra landing point offset east, in m.

// Customization variables; see documentation for lnd.ks.
LOCAL hfinal IS 3.      // Height at end of gravity turn, in m.
LOCAL vfinal IS 2.      // Speed at end of gravity turn, in m/s.
LOCAL hillpeak IS 1000. // Maximum expected hill height, in m.
LOCAL hillslope IS 0.0. // Maximum expected hill slope.
LOCAL throtlow IS 0.9.  // Throttle below which engines switch "off".
LOCAL throthigh IS 1.0. // Throttle at which engines switch back on.
LOCAL throtmin IS 0.0.  // Set to 0.001 to avoid engine restarts.
LOCAL logging IS TRUE.  // Whether to show flight data in terminal.

// General definitions.
SAS OFF.
LOCK vel TO VELOCITY:SURFACE:MAG.
LOCAL bbox IS SHIP:BOUNDS.
LOCK height TO bbox:BOTTOMALTRADAR - hfinal.
LOCK radius TO ALTITUDE + BODY:RADIUS.
LOCK gravity TO BODY:MU/radius^2.
LOCK maxAcc TO AVAILABLETHRUST/MASS.

// Function returns required throttle to come to a stop at the ground
// along a gravity turn, assuming constant acceleration and gravity.
//
// Under these assumptions, an analytic solution exists; e.g. in
// Johnson et al. "Entry, Descent, and Landing Performance for a
// Mid-Lift-To-Drag Ratio Vehicle at Mars" (2018):
//
//   0 = (a/g)^2 + (d/h)*sinP*(a/g) - (d/h)*(1+sinP^2)/2 - 1
//
// where a is the required acceleration, g is the local gravity, sinP
// is the sine of the current trajectory pitch (negative for
// descending trajectories), h is the current height, and d=v^2/2g.
// This can be solved for a/g and rescaled to a throttle level a/amax.
//
// To account for topography, the function estimates the remaining
// downrange distance as cosP*v^2/2amax, and multiplies this by
// hillslope to find the maximum expected rise in ground level
// (limited to no more than hillpeak).  This is subtracted from the
// current altitude when computing the required acceleration.
FUNCTION StoppingThrottle {
  LOCAL sinP IS COS(VANG(UP:VECTOR, VELOCITY:SURFACE)).
  LOCAL stop IS 0.5*VELOCITY:SURFACE:SQRMAGNITUDE/maxAcc.
  LOCAL dh IS stop*SQRT(1 - sinP*sinP)*hillslope.
  LOCAL h IS CHOOSE height - hillpeak IF dh > hillpeak ELSE height - dh.
  IF h <= 0 {
    RETURN 1.
  }.
  LOCAL twrInv IS gravity/maxAcc.
  LOCAL stop2 IS 0.5*stop/h.
  RETURN -sinP*stop2 + SQRT((sinP*sinP*stop2 + twrInv)*(stop2 + twrInv)).
}.

// Set up target location.
IF tpar = "target" AND HASTARGET {
  SET tg TO TARGET.
} ELSE IF tpar:LENGTH > 0 {
  FOR w IN ALLWAYPOINTS {
    IF ( tpar = "waypoint" AND w:ISSELECTED ) OR w:NAME = tpar
      SET tg TO w.
      BREAK.
    }.
  }.
  IF NOT DEFINED tg {
    PRINT "Could not find target \"" + tpar + "\".".
  }.
}.
IF DEFINED tg AND (noff <> 0 OR eoff <> 0) {
  SET tg TO LATLNG(tg:GEOPOSITION:LAT + noff, tg:GEOPOSITION:LON + eoff).
}.
LOCK off TO 0.
LOCK pitchOff TO 0.
LOCK yawOff TO 0.

// Set up trajectory loggging to terminal.
IF logging {
  CLEARSCREEN.
  GLOBAL update IS TIME:SECONDS + 0.2.
  WHEN TIME:SECONDS > update THEN {
    LOCAL pitch IS 90 - VANG(UP:VECTOR, VELOCITY:SURFACE).
    PRINT "Speed (m/s):  " + ROUND(VELOCITY:SURFACE:MAG, 2) AT (0,2).
    PRINT "Pitch (deg):  " + ROUND(pitch, 2) AT (0,3).
    PRINT "Altitude (m): " + ROUND(ALTITUDE, 2) AT (0,4).
    PRINT "Height (m):   " + ROUND(bbox:BOTTOMALTRADAR, 2) AT (0,5).
    PRINT "StopThrust:   " + ROUND(100*StoppingThrottle()) + "%   " AT (0,6).
    PRINT "StopTime (s): " + ROUND(VELOCITY:SURFACE:MAG/maxAcc, 2) AT (0,7).
    IF DEFINED tg {
      PRINT "Offset (m):   " + off AT (0,9).
      PRINT "DPitch (deg): " + ROUND(pitchOff, 2) AT (0,10).
      PRINT "DYaw (deg):   " + ROUND(yawOff, 2) AT (0,11).
    }.
    SET update TO TIME:SECONDS + 0.2.
    RETURN TRUE.
  }.
}.

// Step 0.  Perform any upcoming maneuver.
IF HASNODE {
  IF logging {
    PRINT "Maneuvering.                          " AT (0,0).
  } ELSE {
    PRINT "Maneuvering.".
  }.
  RUN brn.
}

// Step 1.  Kill tangential velocity.
IF (StoppingThrottle() >= throthigh OR PERIAPSIS >= 0) AND height > 0 {
  IF logging {
    PRINT "Deorbiting.                           " AT (0,0).
  } ELSE {
    PRINT "Deorbiting.".
  }.
  LOCK sinPitch TO (gravity - VELOCITY:ORBIT:SQRMAGNITUDE/radius)/maxAcc.
  LOCK retroUp TO LOOKDIRUP(RETROGRADE:VECTOR, UP:VECTOR).
  LOCK STEERING TO retroUp + R(0, -ARCSIN(sinPitch), 0).
  WAIT UNTIL VECTORANGLE(FACING:VECTOR, STEERING:VECTOR) < 1.
  LOCK THROTTLE TO 1.
  WAIT UNTIL (StoppingThrottle() < throthigh AND PERIAPSIS < 0) OR height <= 0.
}.

// Define "up" and "downrange" dynamically until ship is nearly
// vertical, then set them to a fixed direction to avoid flipping.
LOCK upvec TO UP:VECTOR.
LOCK drvec TO VXCL(UP:VECTOR, SHIP:SRFPROGRADE:VECTOR):NORMALIZED.
LOCAL fixedDR IS drvec.
WHEN VANG(FACING:VECTOR, UP:VECTOR) < 5 THEN {
  SET fixedDR TO drvec.
  LOCK upvec TO fixedDR.
  LOCK drvec TO fixedDR.
}.

// Set up gravity turn steering with terminal guidance correction.  I
// don't have the actual formula for a gravity turn's downrange
// displacement, so instead I'll just compute the freefall impact
// distance imp and halve it.  Then apply pitch and yaw corrections
// equal to some multiple of the angular distance between impact point
// and target.  Treat ground as flat and level (terminal guidance will
// adjust during approach).
IF DEFINED tg {
  LOCK imp TO (SQRT(VERICALSPEED*VERTICALSPEED + 2*gravity*height)
	       - VERTICALSPEED)*GROUNDSPEED/gravity.
  LOCK off TO VXCL(UP:VECTOR, tg:POSITION) - 0.5*imp*drvec.
  LOCK pitchOff TO 2*off*drvec/tg:DISTANCE.
  LOCK yawOff TO 2*off*FACING:STARVECTOR/tg:DISTANCE.
}.

// Step 2a.  Perform gravity turn, pulsing engines as needed.
IF logging {
  PRINT "Descending.                             " AT (0,0).
} ELSE {
  PRINT "Descending.".
}.
LOCK retroUp TO LOOKDIRUP(SRFRETROGRADE:VECTOR, upvec).
LOCK STEERING TO retroUp + R(yawOff, pitchOff, 0).
SET NAVMODE TO "SURFACE".
GEAR ON.
UNTIL height <= hfinal AND vel <= vfinal {
  LOCK THROTTLE TO StoppingThrottle().
  WAIT UNTIL THROTTLE < throtlow OR vel <= vfinal.
  LOCK THROTTLE TO throtmin.
  WAIT UNTIL StoppingThrottle() >= throthigh OR vel <= vfinal.
}.

// Step 2b.  Ease in to touchdown.
//LOCK vel TO -VELOCITY:SURFACE*UP:VECTOR.
LOCK vel TO VERTICALSPEED.
LOCK THROTTLE TO gravity/maxAcc + (vel - vfinal)/maxAcc.
WAIT UNTIL vel <= 0.
LOCK THROTTLE TO 0.
UNLOCK STEERING.
UNLOCK THROTTLE.
SET SHIP:CONTROL:PILOTMAINTHROTTLE TO 0.