land

@lazyglobal on.
@CLOBBERBUILTINS on.
//Settings
set Control to ship:partsnamed("probeStackSmall")[0]:getmodule("ModuleCommand").
Control:doevent("Control from here").
clearscreen.
unlock all.
set errorScalling to 1.
set preset to "BFT". // Your preset where to land
if preset = "LZ-1" {
  set landingZone to latlng(-0.195836429065209,-74.484855364316).
} else if preset = "LZ-2" {
  set landingZone to latlng(-0.195836429065209,-74.484855364316).
} else if preset = "LZ-4" {
  set landingZone to latlng(-0.556128,-88.461235).
} else if preset = "BFT" {
  set landingZone to vessel("Big Floating Thing"):geoposition.
} else if preset = "MMWAM" {
  set landingZone to vessel("My Mum Wonders About Me"):geoposition.
} else if preset = "LIP" {
  set landingZone to vessel("Land In One Piece"):geoposition.
}

lock altit to alt:radar - 22.6.
set entryburnalt to 32500.
set speedcancelonentry to 416.
if preset = "LZ-1" or preset = "LZ-4" or preset = "LZ-2" {
  set tlng to speedcancelonentry*(ship:apoapsis/12000)+500.
} else {
  set tlng to speedcancelonentry*(ship:apoapsis/7000)+500.
}

lock lngoff to (landingZone:LNG - ADDONS:TR:IMPACTPOS:LNG)*10472.
lock latoff to (landingZone:LAT - ADDONS:TR:IMPACTPOS:LAT)*10472.
lock myvel to ship:velocity:surface:mag.
lock grav to constant:g * body:mass / body:radius^2.
lock maxAccel to (ship:availablethrust / ship:mass) - grav.
lock LBAlt to ship:verticalspeed^2 / (2 * maxAccel).
lock Thr to LBAlt / altit.
sas off.
rcs on.
set rm to 1.
//Boostback
if rm = 1 {
  toggle ag7. //Turn off all engines, but left the center
  toggle ag6. //Turn on two aditional engines
  set deg to (latoff/70)/3.
  lock steering to heading(270+deg, 5).
  wait 9.
  lock throttle to 1.
  print "Stage 1 Boostback burn has started.".

  until lngoff > -tlng-6000 {
    clearscreen.
    set deg to (latoff/70)/3.
    print "Lngoff: " + lngoff + " M".
    print "Latoff: " + latoff + " M".
    wait 0.2.
  }

  toggle ag6.
  until lngoff > -tlng {
    clearscreen.
    set deg to (latoff/70)/3.
    print "Lngoff: " + lngoff + " M".
    print "Latoff: " + latoff + " M".
    wait 0.2.
  }

  lock throttle to 0.
  wait 2.
  toggle brakes.
  unlock steering.
  unlock throttle.
  print "Bb completed!".
  rcs on.
  set rm to 2.
}
//Ascent to apoapsis and orient for entry burn
if rm = 2 {
  clearscreen.
  print "Welcome to Falcon 9 Landing Software V5.".
  print "Your preset is " +  preset.
  lock steering to heading(90, 90).
  until ship:verticalspeed < -0 {
    clearscreen.
    print "Latoff(M): " + latoff.
    print "Lngoff(M): " + lngoff.
    wait 1.
  }

  print "Stage 1 is in apoapsis.".

  until ship:verticalspeed < -250 {
    clearscreen.
    print "Latoff(M): " + latoff.
    print "Lngoff(M): " + lngoff.
    wait 1.
  }

  lock steering to srfretrograde.
  set rm to 3.
}

//Entry burn performing and descending
if rm = 3 {
  until altit < entryburnalt + 7000 {
    clearscreen.
    print "Latoff(M): " + latoff.
    print "Lngoff(M): " + lngoff.
    wait 1.
  }

  set ship:control:fore to 1.
  wait 4.
  set ship:control:fore to 0.
  until altit < entryburnalt + 50 {
    clearscreen.
    print "Latoff(M): " + latoff.
    print "Lngoff(M): " + lngoff.
    wait 1.
  }

  set targetspeedonentry to ship:velocity:surface:mag - speedcancelonentry.
  lock throttle to 1.
  print "Stage 1 Entry Burn has started.".
  wait 2.
  toggle ag6.

  until myvel < targetspeedonentry {
    clearscreen.
    print "Latoff(M): " + latoff.
    print "Lngoff(M): " + lngoff.
    wait 1.
  }

  toggle ag6.
  wait 2.
  lock throttle to 0.
  wait 0.1.
  toggle ag6.
  print "Stage 1 Entry Burn shutdown.".
  lock aoa to 45.
  lock steering to Steer().

  until ship:velocity:surface:mag < 220 {
    clearscreen.
    print "Latoff(M): " + latoff.
    print "Lngoff(M): " + lngoff.
    wait 1.
  }

  print "Stage 1 is transonic.".
  set rm to 4.
}

//Landing Burn
wait 0.
if rm = 4 {
  wait 0.
  until altit < LBAlt - 600 or altit < 1100 {
    clearscreen.
    print "Latoff(M): " + latoff.
    print "Lngoff(M): " + lngoff.
    wait 1.
  }

  lock throttle to Thr.
  print "Stage 1 Landing Burn has started.".
  lock aoa to -5.
  lock steering to Steer().

  until altit < 80 {
    clearscreen.
    print "Latoff(M): " + latoff.
    print "Lngoff(M): " + lngoff.
    wait 1.
  }

  lock steering to lookdirup(-ship:velocity:surface,ship:facing:topvector).
  toggle gear.
  print "Stage 1 Landing legs deployed.".
  until altit < 30 {
    clearscreen.
    print "Latoff(M): " + latoff.
    print "Lngoff(M): " + lngoff.
    wait 1.
  }

  lock steering to lookdirup(up:vector,ship:facing:topvector).
  set rm to 5.
  wait 0.
}

if rm = 5 {
  wait 0.
  until ship:verticalspeed > -1 or altit < 1 {
    clearscreen.
    print "Latoff(M): " + latoff.
    print "Lngoff(M): " + lngoff.
    wait 1.
  }

  lock throttle to 0.01.
  set latoff to latoff.
  set lngoff to lngoff.
  print "Lngoff(M):  " + lngoff.
  print "Latoff(M): " + latoff.
  wait 1.
  lock throttle to 0.
  print preset + ", stage 1 landing confirmed.".
  wait 20.
  unlock steering.
  unlock throttle.
  sas off.
  rcs off.
  toggle brakes.
  set done to true.
}

//Functions

function lngError {
 return getImpact():lng - landingZone:lng.
}

function latError {
 return getImpact():lat - landingZone:lat.
}

function errorVector {
 return getImpact():position - landingZone:position.
}

function Steer {
 local errorVector is errorVector().
 local velVector is -ship:velocity:surface.
 local result is velVector + errorVector*errorScalling.

 if vang(result, velVector) > aoa
 {
   set result to velVector:normalized + tan(aoa)*errorVector:normalized.
 }

 return lookdirup(result, ship:facing:topvector).
}

function getImpact {
  if addons:tr:hasimpact { return addons:tr:impactpos. }
    return ship:geoposition.
}