// Elixer Space Company - ASDS Script [version 0.3.1]
// Customised Version



// Landing Parameters

//set impactPos to ADDONS:TR:IMPACTPOS.
//if ABS(impactPos:lng - 49) < ABS(impactPos:lng - 44) {
//    set TARGET to vessel("Of Course I Still Love You").
//}
//else {
//   set TARGET to vessel("Just Read The Instructions").
//}
// North pad is 29.9445367992489, -74.537571128941

set landingZone to LatLng(29.96, -74.534).
ADDONS:TR:SETTARGET (landingZone).


// Initialization


rcs on.
set currentFacing to facing.
lock steering to currentFacing.
wait 2.


// Grid Fins Parameter



print "Falcon Booster - Landing Software Initialized" at (0,0).
print "" at (0,1).
print SHIP:DELTAV:CURRENT + " m/s" at (0,2).
print landingZone:lat at (0,3).
print landingZone:lng at (0,4).
print "" at (0,5).
print "- - - - - - - - - - - - - - - - - - - - - - - - - " at (0,6).
print "                 Booster Coasting                 " at (0,7).
print "                   Awaiting Burn                  " at (0,8).
print "- - - - - - - - - - - - - - - - - - - - - - - - - " at (0,9).

//if ship:partstagged("gridFins")[moduleNum]:GETMODULE("ModuleControlSurface"):GETFIELD("authority limiter") <> 40{print "Grid fins issue. Verify authority limiter".}
//else {print "Grid fins set correctly".}

wait 1.

set steeringManager:maxstoppingtime to 5.
set steeringManager:rollts to 20.
wait 1.

set moduleNum to 0.
until moduleNum = 4 {
    ship:partstagged("gridFins")[moduleNum]:GETMODULE("ModuleControlSurface"):SETFIELD("authority limiter", 30).
    ship:partstagged("gridFins")[moduleNum]:GETMODULE("ModuleControlSurface"):SETFIELD("deploy angle", 50).
    set moduleNum to (moduleNum + 1).
}

// Landing Variables


set radarOffset to 28. // This must be changed to the height of the landing vehicle (on gear)
lock trueRadar to alt:radar - radarOffset.
set g to constant:g * body:mass / body:radius^2.
lock stopDist to (ship:verticalspeed^2) / (2 * (ship:availablethrust / ship:mass - g)).
lock idealThrottle to (stopDist / trueRadar) * 6.
lock impactTime to trueRadar / abs(ship:verticalspeed).
lock aoa to 30.
lock errorScaling to 1.


    // Guidance Functions


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

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

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

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

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

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

    return lookdirup(result, facing:topvector).
}

rcs on.
lock steering to srfretrograde. 
brakes on.
wait until ship:verticalspeed <-700.
    lock throttle to 1.
    lock aoa to -5.
    lock steering to getSteering().
	rcs off.
    print "                    First Burn                    " at (0,7).
    print "                Pad targeting burn                " at (0,8).
    print "- - - - - - - - - - - - - - - - - - - - - - - - - " at (0,9).

when alt:radar <= 5000 then {
    set landingZone to LatLng(29.9445367992489, -74.537571128941).
    ADDONS:TR:SETTARGET (landingZone).
}

wait until ship:verticalspeed > -200.
    lock throttle to 0.
    lock aoa to 17.5. 
    lock steering to getSteering().
    steeringManager:resettodefault().
    rcs off.
    print "                 Booster Coasting                 " at (0,7).
    print "                Impact correction                 " at (0,8).
    print "- - - - - - - - - - - - - - - - - - - - - - - - - " at (0,9).

wait until alt:radar < 12000.
    lock aoa to 10.

wait until alt:radar < 7000.
    lock aoa to 5.
    rcs on.

wait until trueRadar < (stopDist). 
    lock throttle to 1.
    lock aoa to -3.
    lock steering to getSteering().
    rcs on.
    print "                  Booster Landing                 " at (0,7).
    print "                   Landing burn                   " at (0,8).
    print "- - - - - - - - - - - - - - - - - - - - - - - - - " at (0,9).

wait until ship:verticalspeed > -35.
    toggle ag1.
    gear on.
    lock throttle to idealThrottle.
    lock aoa to -2.
    lock steering to heading(90, 90, 180).

    wait 2.

wait until ship:verticalspeed > -0.1.
	set ship:control:pilotmainthrottle to 0.
    unlock steering.
	RCS on.
    sas on.
    print "                   Booster Landed                 " at (0,7).
    print "                    Engines Off                   " at (0,8).
    print "- - - - - - - - - - - - - - - - - - - - - - - - - " at (0,9).
    wait 5.
    rcs off.