// functions for calculating steering values.

// Create a node that will circularize the orbit.
// 'where' can be one of:
// the special string "APO", for the next Apoapsis.
// the special string "PERI", for the next Periapsis.
// a time value (either a Time struct or a scalar), representing a target time.
function CreateCircularizationNode {
  parameter where is "APO".

  local t is TIME.
  if where:IsType("String") {
    if where = "APO" {
      set t to TIME + SHIP:ORBIT:ETA:APOAPSIS.
    } else if where = "PERI" {
      set t to TIME + SHIP:ORBIT:ETA:PERIAPSIS.
    } else {
      print "WARNING: Invalid string passed to CreateCirculazationNode(). Node is invalid.".
    }
  } else {
    // we've been passed a time at which to circularize.
    set t to where.
  }

  local Vc is sqrt(SHIP:BODY:MU/(PositionAt(SHIP, t) - SHIP:BODY:POSITION):MAG).
  local dV is Vc - VelocityAt(SHIP, t):ORBIT:MAG.
  local n is Node(t, 0, 0, dV).

  return n.
}

// The distance at which to start burning to reach a target on the ground.
// REQUIRES a circular orbit.
// TODO: it would be great to semi-automate this...
function TargetBurnDistance {
  if not HASTARGET {
    return -1.
  }
  return Sqrt(StoppingDistance()^2 + (SHIP:ORBIT:SEMIMAJORAXIS - SHIP:BODY:RADIUS - TARGET:ALTITUDE)^2).
}

// Stopping distance at current velocity. For a circular orbit this is valid at any point in the orbit.
function StoppingDistance {
  local dV is SHIP:VELOCITY:SURFACE:MAG.
  return dV*BurnTime(dV)/2.
}

function ExecNode {
  if not HASNODE {
    print "No node to execute.".
    return.
  }

  SAS off.

  // begin the burn at leadT seconds before the node.
  local leadT is BurnTime(NEXTNODE:DELTAV:MAG / 2).
  local t is BurnTime(NEXTNODE:DELTAV:MAG).

  if WillStage(NEXTNODE:DELTAV:MAG) {
    print "WARNING: kOS will stage during this node execution. Safe cancellation requires reboot.".
    when FlameOut() then {
      print "Flameout detected. Staging.".
      stage.
    }
  }
  
  print "Adjusting heading".
  lock STEERING to LookDirUp(NEXTNODE:DELTAV, SHIP:FACING:TOPVECTOR).
  wait until VAng(SHIP:FACING:FOREVECTOR, STEERINGMANAGER:TARGET:FOREVECTOR) <= 0.1.

  print "Warping to node.".
  KUNIVERSE:TIMEWARP:WarpTo(NEXTNODE:TIME - leadT - 2).
  wait until SHIP:UNPACKED.
  wait until NEXTNODE:ETA <= leadT.

  print "Executing burn.".
  local dvMin is NEXTNODE:DELTAV:MAG.
  lock THROTTLE to 1.0.
  wait t.
  lock THROTTLE to 0.0.

  unlock THROTTLE.
  unlock STEERING.
  SAS on.
  print "Node execution complete.".
}

// currently only works for testing against *current* stage.
function WillStage {
  parameter dV.
  if not HASNODE { return false. }
  return dV > SHIP:StageDeltaV(SHIP:STAGENUM):VACUUM.
}

// Calculate the time required to burn a given dV.
// Assumes a perfectly spherical Kerbal in a vacuum.
function BurnTime {
  parameter totaldV, s is STAGE:NUMBER.
  local totalT is 0.0.

  local lastStage is false.
  // We allow a small tolerance to deal with potential floating point errors.
  until totaldV <= 0.001 {
    local F is stageThrust(s).
    local Isp is stageISP(s).
    local m is stageMass(s).
    // TODO: handle node execution in atmosphere?
    local dV is min(totaldV, SHIP:StageDeltaV(s):VACUUM).
    local t is calcBurnTime(dV, m, Isp, F).

    set totaldV to totaldV - dV.
    set s to s - 1.
    set totalT to totalT + t.
  }

  return totalT.
}

// Convenience function to wrap the actual calculation for burn time.
function calcBurnTime {
  parameter dV, m, Isp, F.

  if F = 0 or Isp = 0 {
    print "WARNING: Tried to calculate burn time with a denominator value of 0. Returning 0. Your calculations are probably wrong.".
    print "F: " + F .
    print "Isp: " + Isp.
    return 0.
  }

  local g0 is CONSTANT:G0.
  return g0 * m * Isp * (1 - CONSTANT():E^(-dV/(g0*Isp))) / F.
}

// Calculate the ISP for a given stage.
// Defaults to current stage. Assumes your ship is designed so that
// engines are discarded immediately when they flame out.
function stageISP {
  parameter s is STAGE:NUMBER.

  local en is list().
  list ENGINES in en.

  local ispSum is 0.
  local eCount is 0.
  for e in en {
    if e:STAGE >= s and e:DECOUPLEDIN < s {
      set ispSum to ispSum + e:VACUUMISP.
      set eCount to eCount + 1.
    }
  }
  if eCount = 0 { return 0. }
  
  return ispSum / eCount.
}

// Calculates the total thrust for the given stage, in kN.
// Defaults to current stage. Assumes your ship is designed so that
// engines are discarded immediately when they flame out.
function stageThrust {
  parameter s is STAGE:NUMBER.

  local en is list().
  list ENGINES in en.

  local sum is 0.
  for e in en {
    if e:STAGE >= s and e:DECOUPLEDIN < s {
      set sum to sum + e:POSSIBLETHRUST.
    }
  }

  return sum.
}

// Determine mass at start of target stage.
// This can handle Delta V-style launchers but
// only if the central rocket remains in the stack
// for exactly two stages, one of which is the current stage.
// More complex staging with partially depleted tanks may produce
// undefined behavior.
function stageMass {
  parameter s is STAGE:NUMBER.

  local m is SHIP:MASS.
  if s = SHIP:STAGENUM { return m. }

  local ps is List().
  list PARTS in ps.
  for part in ps {
    if part:DECOUPLEDIN >= s {
      set m to m - part:MASS.
    }
  }

  list ENGINES in ps.
  for part in ps {
    if part:DECOUPLEDIN < s and part:AVAILABLETHRUST > 0 {
      set m to m - (part:MAXMASSFLOW * SHIP:StageDeltaV(SHIP:STAGENUM):DURATION).
    }
  }

  return m.
}

// TODO: This would be better in throttle.ks or perhaps some sort of ship status library,
// but we want to avoid too many inter-dependencies for now.
function FlameOut {
  local ens is List().
  list engines in ens.
  for en in ens {
    if en:FLAMEOUT {
      return true.
    }
  }
  return false.
}

// function PredictGeo {
//   parameter t.

//   local pos is PositionAt(SHIP,t).
//   local rDir is VDOT(SHIP:BODY:NORTH:FOREVECTOR,SHIP:BODY:ANGULARVEL). //the number of radians the body will rotate in one second (negative if rotating counter clockwise when viewed looking down on north
//   local dT is t - TIME:SECONDS.
//   local geoPos is SHIP:BODY:GeoPositionOf(pos).
//   local drift is rDir * dT * CONSTANT:RADTODEG.
//   local long is Mod(geoPos:LNG + drift, 360).
//   if long < -180 {
//     set long to long + 360.
//   }
//   if long > 180 {
//     set long TO long - 360.
//   }

//   return LatLng(geoPos:LAT, long).
// }