var targetAltitude = 80   // target orbit's altitude in km
var ascentHeading  = 90   // ascent heading (0 = north, 90 = equitorial)
var ascentRoll = math.round((ship.pitch < 80 ? ship.roll :
  180 - ship.up.angle(ship.north.rotate ascentHeading, ship.away)) / 45)*45
var circleRoll = ascentRoll
var ascentProfile  = 5    // profile (1-9, less means less aggresive turn)
var speedRatio     = 5    // speed/altitude ratio to base the curve on (higher is for speed, more than 10 means switch earlier to full speed-based)
var maxSpeed       = .9   // target escape speed (fraction of minimal or in m/s if above 2)
var safeAltitude   = 1 + math.max atmosphere.depth/1000, 7 // safe altitude above atmosphere and mountains
var startAngle     = math.round(ship.pitch,1) // usually 90 but e.g. DynaWing starts with 81.2
var firstAngle     = 60   // angle upto which to use simple ratio
var firstAltitude  = math.max safeAltitude/10, math.max 3, ship.altitude/1000+2 // altitude (in km) of first phase (to reach pitch=firstAngle)
var narrowAltitude = 10*math.round ship.altitude/10+10 // altitude (in m) upto which to go straight up

if startAngle > 89
  startAngle = 90


//: PARAM GUI

def gui
  var selected = 0
  var wnd = new ui.window false, "Launch " + ship.name
  try // try..finally wnd.dispose
    var lblWidth = 120
    var boxWidth = 60
    var uniWidth = 46
  //move the window to the left side
    wnd.x -= (unity.screen.width - 200) / 3

  //bottom label (above cancel/control/launch buttons) showing additional info
    var hintLabel = new ui.label
    def boxEnter box
      hintLabel.text = box.tag //...when some edit box is entered (focused, key-cursor inside)

  //row-creating helper
    def row text, value, units, hint
      var row = wnd.addHorizontal()
      var lbl = row.addLabel text
      lbl.minWidth = lblWidth
    //string(value) performs to-string conversion:
      var box = row.addTextBox string value
      box.tag = hint
      box.enter.add boxEnter
      if units == null
        box.preferWidth = boxWidth + 3 + uniWidth
      else
        box.preferWidth = boxWidth
        var uni = row.add new ui.label
        uni.text = units
        uni.minWidth = uniWidth
      return box
    
  //parameters
    var ta = row "Target altitude: ",  targetAltitude, "km",    "Final Apoapsis"
    var ah = row "Ascent heading: ",   ascentHeading,  "°",     "Course during ascent (not inclination)"
    var ar = row "Ascent roll: ",      ascentRoll,     "°",     "Roll/bank during ascent"
    var cr = row "Circularize roll: ", circleRoll,     "°",     "Roll/bank during circularization"
    var pf = row "Ascent profile: ",   ascentProfile,  "1-9",   "Lower means less agressive turn"
    var pr = row "Speed/Alt ratio: ",  speedRatio,     "*10%",  "Higher for speed, above 10 means earlier"
    var ms = row "Max. Speed: ",       maxSpeed,       "*|m/s", "<= 2 for fraction of minimal, or m/s"
    var sa = row "Safe altitude: ",    safeAltitude,   "km",    "Above atmposhepre and mountains"
    var sd = row "Start angle: ",      startAngle,     "°",     "Start degrees of pitch"
    var fa = row "First angle: ",      firstAngle,     "°",     "Degrees of pitch for first phase"
    var a1 = row "First altitude: ",   firstAltitude,  "km",    "Altitude for first phase"
    var na = row "Narrow altitude: ",  narrowAltitude,  "m",    "Minimal altitude to go straight up"
    
  //hint label and final buttons
    wnd.add hintLabel
    var brow = wnd.addHorizontal()
    wnd.closed += def => selected = 1
    brow.addButton("Cancel",  def => selected = 1).flexWidth = 1
    brow.addButton("Control", def => selected = 2).flexWidth = 1
    brow.addButton("Launch",  def => selected = 3).flexWidth = 1
    
  //wait until user selects what to do (or closes the window)
    wnd.show
    while selected == 0
      wait
    
    if selected == 3 // launch
      targetAltitude = double ta.text
      ascentHeading  = double ah.text
      ascentRoll     = double ar.text
      circleRoll     = double cr.text
      ascentProfile  = double pf.text
      speedRatio     = double pr.text
      maxSpeed       = double ms.text
      safeAltitude   = double sa.text
      startAngle     = double sd.text
      firstAngle     = double fa.text
      firstAltitude  = double a1.text
      narrowAltitude = double na.text

  finally
    wnd.dispose

  if selected == 2 // control
    run.replace "control.ros"
    //script not continuing here, replaced by control.ros

  return selected == 3
if not gui()
//user selected "Cancel" or closed the window
  return

targetAltitude *= 1000
safeAltitude   *= 1000
firstAltitude  *= 1000
ascentProfile  = 1-math.clamp(ascentProfile, 1, 9)*0.1
speedRatio     = speedRatio*0.1
if maxSpeed <= 2
  maxSpeed *= math.sqrt body.mu/(body.radius+safeAltitude)
if safeAltitude + 1000 >= targetAltitude
  var minSafe = math.max atmosphere.depth, 7000
  safeAltitude = 0.5 * (minSafe + targetAltitude)
  if safeAltitude + 1000 >= targetAltitude
    safeAltitude = minSafe
    if safeAltitude + 1000 >= targetAltitude
      safeAltitude = targetAltitude - 1000


//: STEERING

//autopilot.reset
autopilot.sas = true
autopilot.killRot = true

/*/ change params for all three pids at once
autopilot.pids.P = 1.0
autopilot.pids.I = 0.05
autopilot.pids.D = 0.02
autopilot.pids.R = 0.02
//*/
/* change roll pid params
autopilot.pids.roll.P = 1.0
autopilot.pids.roll.I = 0.1
autopilot.pids.roll.D = 0.2
autopilot.pids.roll.R = 0.3
//*/

var minSpeed = null
var prevSteerReport = nan
def steer
  if ship.altitude <= narrowAltitude
    autopilot.pitch = startAngle
    return
  if ship.periapsis >= safeAltitude
    autopilot.pitch = 0
    return
//limit AoA when Q is high (TODO: limit around staging in atmosphere)
  var srfA = 90 - ship.away.angle ship.surfaceVelocity
  var curr = srfA
  var maxA = 3.0/math.clamp(ship.q, 0.1, 0.75) - 3.0
//smooth transition from surface to orbital speed by ratio of altitude to safe altitude
  var factor = math.clamp (altitude/safeAltitude)^2, 0, 1
  var speed = factor * ship.velocity.size + ship.surfaceVelocity.size * (1-factor)
  if minSpeed == null then minSpeed = speed
  var want
  if ship.altitude <= firstAltitude
    want = startAngle - (startAngle - firstAngle) * ship.altitude/firstAltitude
  else
  //mix of speed-based and altitude-based curve
    var altRatio = math.clamp (ship.altitude-firstAltitude)/(safeAltitude-firstAltitude), 0, 1
    var ratio = math.min 1, speedRatio * altRatio
    var fract = ((1-ratio)*altRatio + ratio *
      math.min(1,(speed-minSpeed)/(maxSpeed-minSpeed)))^ascentProfile
    want = firstAngle * (1 - fract)
//compensate for differences from desired prograde pitch
  var pitch
  curr = factor * (90 - ship.away.angle ship.velocity) + (1-factor) * srfA
  if want < curr and ship.altitude < safeAltitude/3
    pitch = want + 3*ship.altitude/safeAltitude*(want-curr)
  else pitch = 2*want-curr
//final adjustment
  var ctrl = math.clamp pitch, math.max(0, srfA-maxA), math.min(srfA+maxA, 90)
  autopilot.pitch = ctrl
  if time.since(prevSteerReport) >= 1
    prevSteerReport = time()
    print "Ctrl:{0,6:F2} Want:{1,6:F2} Have:{2,6:F2} Q:{3,4:F2} ", ctrl, want, ship.pitch, ship.q
    print "ReqA:{0,6:F2} Curr:{1,6:F2} SrfA:{2,6:F2} MaxA:{3,6:F2} ", pitch, curr, srfA, maxA


//: THROTTLE

var lastApoAbove = 0.0
def throttle
  if ship.apoapsis >= targetAltitude
    ship.throttle = 0
    if lastApoAbove == 0.0
      print "Apoapsis reached"
    lastApoAbove = time()
  else if lastApoAbove != 0 and time() - lastApoAbove < 10
    ship.throttle = 0
  else
    var power = 1
    if ship.apoapsis > safeAltitude*0.9 and ship.altitude < safeAltitude
      power = math.max 0.1, (targetAltitude-ship.apoapsis) / math.max 1.0, targetAltitude-safeAltitude*0.9
    if ship.apoapsis > targetAltitude * 0.95
      power = math.min power, 0.01+20*(1-ship.apoapsis/targetAltitude)
    ship.throttle = math.min power, user.throttle


//: STAGING

def staging
  if not stage.ready
    return
  var nextDecoupler = ship.parts.nextDecoupler
  if nextDecoupler == null
  //nothing to separate
    if stage.pending
    //but there appears to be inactive engine
      print "Pending last stage"
      stage
    return
  if nextDecoupler.tags.has "noauto"
    return
  if nextDecoupler.type == "LaunchClamp"
    print "Releasing launch clamps"
    stage
    return
  if stage.nofuel
    print stage.engines.anyOperational ? "Stage out of fuel" : "No operational engine"
    stage
    return


//: VECTORS

var vd = [
  new vector.draw,
  new vector.draw,
  new vector.draw,
  new vector.draw]
vd[0].scale = 15
vd[0].width = .5
vd[1].color = ui.color.blue
vd[1].scale = 10
vd[2].color = ui.color.green
vd[2].width = .3
vd[2].scale = 10
vd[3].color = ui.color.red
vd[3].width = .2
vd[3].scale = 12
def vectors
  vd[0].vector = ship.forward
  vd[1].vector = autopilot.direction.normalized
  vd[2].vector = ship.velocity.normalized
  vd[3].vector = ship.srfVel.normalized
vectors
for var d in vd
  d.show


//: ASCENT

ship.throttle = 0
user.throttle = 1
ship.heading = ascentHeading
ship.roll = ascentRoll
var ctlsub = system.update def
  throttle()
  steer()
var stgsub = system.idle staging
var vdraws = system.update vectors

try
  print "Target apoapsis: " + targetAltitude
  print "Safe altitude:   " + safeAltitude
  until ship.altitude >= narrowAltitude+(safeAltitude-narrowAltitude)/2 and (
    ship.altitude >= safeAltitude and ship.apoapsis >= targetAltitude*.99
    or ship.timeToAp > ship.timeToPe) // this is here until I find better way for weak orbital engines (TODO: use stage.burntime)
    wait

  ctlsub.remove()
  ctlsub = null
  user.throttle = 0
  autopilot.disable
  if ship.altitude >= safeAltitude
    print "Safe altitude reached"
  else if ship.timeToAp > ship.timeToPe
    print "Emergency circularization"
  else
    print "Uknown reason for circularization"
  vd[3].hide //srfvel

//: CIRCULARIZE

  def dt => ship.eccentricity < 0.001 or
    ship.periapsis < safeAltitude and ship.timeToAp > ship.timeToPe ?
    0 : ship.timeToAp
  var minE = ship.eccentricity
  def dv
    minE = math.min minE, ship.eccentricity
    if minE < 0.001 and (minE < 0.00001 or ship.eccentricity > 1.1*minE)
      return vector.zero
    var p = ship.position-body.position
    var v1 = ship.velocity
    var v2 = math.sqrt(body.mu/p.size) * (p.cross v1.cross p).normalized
    return v2-v1

  autopilot.roll = circleRoll
  ctlsub = system.update def
    autopilot.direction = ship.away.cross ship.velocity.cross ship.away

  var maxThrottle = 0.0
  var dvLast = dv().size
  var dvMin = dvLast
  var tta = ship.timeToAp
  var pitchDown = 0.0
  def circle
    tta = ship.timeToAp
    var pwr = 1
    var dir = dv()
    dvMin = math.min dvMin, dvLast = dir.size
    if tta <= ship.timeToPe
    //minimize velocity vector difference when before apoapsis
      pwr = (0.1 / math.clamp tta - 0.5 * stage.burnTime(dvMin), 0.1, 10.0) ^ 2.0
    else
      tta -= ship.period
    //pitch-up when past apoapsis
      var a = math.clamp(-tta, 0, 30)
      dir = ship.velocity.rotate a, ship.velocity.cross ship.away
      pwr = 1 + math.tan a
    if maxThrottle <= 0.5 // almost there
      var tan = ship.away.cross ship.velocity.cross ship.away
      if tan.angle(dir) > 1 // fix the wobble
        dir = tan.rotate(tan.angle(dir, ship.away.cross ship.velocity) >= 0 ? 1 : -1, ship.velocity.cross ship.away)
    if pitchDown != 0.0
      dir = dir.rotate pitchDown, ship.away.cross ship.velocity
    autopilot.direction = dir
    var d = dir.angle ship.forward // direction / face-angle error
    var e = d <= 1 ? 1 : math.deg.cos math.min 89.4, d^2.0 // min 0.0105
    autopilot.throttle = pwr * e * math.min maxThrottle,
      dvMin * ship.mass / math.max 1, ship.engines.getThrust()

  var prevEtaReport = nan
  var switched = false
  var warped = false
  for
    var bt = stage.burnTime dv().size
    if bt != bt; bt = 120 // bt != bt is test for nan
    var eta = time.delta dt()-0.5*bt
    if time.since(prevEtaReport) >= (eta > 10 and warped ? 10 : eta > 5 ? 2 : 1)
      prevEtaReport = time.now
      print "ETA: " + eta
      if eta > 20 and !warped and time.warp.ready
        warped = true
        time.warp.index = 0
        time.warp.high = true
        time.warp.to prevEtaReport + eta - 10
    if eta <= 5 and not switched
      switched = true
      time.warp.cancel
      ctlsub.remove
      tta = ship.timeToAp
      ctlsub = system.update circle
    if eta <= 3
      maxThrottle = 0.5*(3-eta)
    if eta <= 1; break
    wait
  maxThrottle = 1.0

  var almost = null
  var almost2 = null
  var choked = null
  var warned = 0
  var maxHeight = ship.apoapsis*1.1+1000
  var prevReport = nan
  var lastTta = null
  var pitchDownFactor = 1.0
  print "Burn!"
  until dvLast < 0.01 or ship.eccentricity < 0.00001
    if minE < 0.001 and ship.timeToPe < ship.period*0.3
    //happens with good accuracy, not a failure
      print "Apoapsis too far"
      break
    if ship.apoapsis > maxHeight and ship.periapsis > math.max(atmosphere.depth,1000)+1000
      print "Overshot apoapsis"
      break
    if ship.apoapsis > maxHeight*1.5+5000
      print "Something went wrong!"
      break

  //progress printout + pitch-down logic
    if time.since(prevReport) >= 1
      prevReport = time()
    //this is to handle tilted/off-center engines like on Dynawing
      var currTta = tta // to avoid hazards, tta is updated asynchronously
      if lastTta != null
        var diff = lastTta-currTta
        pitchDown = math.clamp pitchDown + pitchDownFactor * math.clamp(ship.throttle - diff,
          -0.5, currTta < 0 ? 0.0 : 0.5), 0.0, 7.0
      lastTta = currTta
      print "TTA: {0,6:F1} DV: {1,6:F1} BT: {2,6:F1} E: {3,7:F5} PD: {4,3:F1}",
        currTta, dvLast, stage.burnTime(dvLast), ship.eccentricity, pitchDown
      
  //check that staging is working (and we have fuel)
    if ship.engines.getThrust() == 0
      if choked == null
        choked = time()
      if time.since(choked) >= 3 and time.since(warned) >= 1
        warned = time()
        print "No acceleration!"
      if time()-choked >= 30
        print "No acceleration!!"
        break
      continue
    choked = null

  //time limit for fine-tuning
    if ship.periapsis < safeAltitude
      continue
    if dvMin < 0.2 or ship.eccentricity < 0.01
      if almost == null
        almost = time()
        maxThrottle = 0.3
        print "AT1: {0,6:F1} DV: {1,6:F1} BT: {2,6:F1} E: {3,7:F5} PD: {4,3:F1}",
          tta, dvLast, stage.burnTime(dvLast), ship.eccentricity, pitchDown
        pitchDownFactor = 0.5
      else if time.since(almost) > 30
        break
      if dvMin < 0.05 or ship.eccentricity < 0.001
        if almost2 == null
          almost2 = time()
          maxThrottle = 0.1
          print "AT2: {0,6:F1} DV: {1,6:F1} BT: {2,6:F1} E: {3,7:F5} PD: {4,3:F1}",
            tta, dvLast, stage.burnTime(dvLast), ship.eccentricity, pitchDown
          pitchDownFactor = 0.1
        else if time.since(almost2) > 10
          break

  maxThrottle = 0.0
finally
  if ctlsub != null
    ctlsub.remove
  stgsub.remove
  vdraws.remove
  for var d in vd
    d.dispose()
  vd = null
  user.throttle = 0
  autopilot.disable
  //autopilot.reset

print "DONE! e={0:F5} Ap:{1:F3} Pe:{2:F3}", ship.eccentricity, ship.apoapsis*0.001, ship.periapsis*0.001
global.control_dir = "obt.prograde"
run.replace "control.ros"