FTLToolbox.ks

//   ABANDONED

// Do not run this. Paste via telnet (or TPSKP runscript)
// FTLToolbox.ks
// gist 02e8a05fb7fb02dddb28bb5e59dfb460
// v0.2.5

//TODO:
//implement printing
//fix steeringPrecision raising ftlwarplimit
//fix steering sometimes fluctuating during circularization (appears to be related to crossing periapsis or verticalspeed >/< 0), so probably wrongfully using steeringTangential
//fix looping near tarVel while gbraking
//fix accidentally switching to children soi during maneuvering (gbraking, circularizing)
//change to gbrakemargin based on altitude and up-angle
//fix inclination change not always finishing fully
//fix final transfer and/or circularization can overshoot and/or leave the soi unintentionally
//gbraking: aim at destination when close to tarVel, making sure that we don't overshoot a tight tarVel margin while turning towards destination.
//allow manual trigger of warp while gbraking
//Blueshif sometimes cancles FTL warp when trying to leave a soi in order to "avoid crashing into the body"


//=========
//pre-locks
//=========
lock internalSteering to ship:facing:forevector.
lock internalThrottle to 0.
lock minAngle to "init".
lock currAngle to "init".
lock flip to "init".
lock relvel to v(0,0,0).

//=========
//util functions
//=========

function circSpeed {
	parameter _body, _alt.
	
	return sqrt(_body:mu/(_body:radius + _alt)).
}
function brakingScore {
	parameter bd is body.
	
	//TODO:
	//Find a better method, taking mu, soiradius, safeDist and local velocity into account
	
	//local rad is 1 + getSafeDistInRad(bd).
	//return bd:mu/((bd:radius * rad)^2).
	return bd:mu.
}
function getSafeDist {
	parameter bd is body.
	
	local result is sqrt(bd:mu * 5).
	set result to choose result if (log10(bd:mass) > 27) else max(bd:radius * 2, result).
	
	return result.
}
function getSafeDistInRad {
	parameter bd is body.
	
	return getSafeDist(bd)/bd:radius.
}
function findPartModules {
	set FTLToolbox:controlPoints to list().
	for p in ship:parts for m in p:modules {
		local mod is p:getmodule(m).
		if mod:alleventnames:contains("control from here") FTLToolbox:controlPoints:add(mod).
	}
	local warpEngines is ship:modulesnamed("wbiWarpEngine").
	set FTLToolbox:warpEngine to choose warpEngines[0] if warpEngines:length > 0 else "none".
}
function getOrbitalVelocityRelativeToSun {
	parameter orbitable is ship.
	parameter timestamp is time:seconds.
	
	local result is v(0,0,0).
	until orbitable = sun {
		set result to result + velocityat(orbitable, timestamp):orbit.
		set orbitable to orbitable:body.
	}
	
	return result.
}

function doHasteSteering {
	if not FTLToolbox:hasteSteering return false.
	
	wait 0.		//give steering 1 tick to update
	local tarVec is choose internalSteering if internalSteering:isType("Vector") else choose internalSteering:vector if internalSteering:hassuffix("Vector") else ship:facing:forevector.
	local best is "".
	local topScore is 180.
	
	for cP in FTLToolbox:controlPoints {
		local ang is vang(cP:part:facing:forevector, tarVec).
		if ang < topScore {
			set topScore to ang.
			set best to cP.
		}
	}
	
	if best:isType("PartModule") best:doevent("control from here").
}
function doAlertUser {
	parameter duration is FTLToolbox:alertDuration.
	parameter interval is FTLToolbox:alertInterval.
	
	local restore is terminal:reverse.
	for i in range(ceiling(duration/interval)) {
		set terminal:reverse to not terminal:reverse.
		wait interval.
	}
	set terminal:reverse to restore.
}

function storeVesselState {
	set FTLToolbox:vesselState to lex("engines", lex(), "controlPoint", ship:controlpart).
	for eng in ship:engines {
		FTLToolbox:vesselState["engines"]:add(eng:uid, lex(
			"Ignition", eng:ignition,
			"Thrustlimit", eng:thrustlimit
		)).
	}
}
function restoreVesselState {
	local success is true.
	local engCheck is true.
	local cpCheck is false.
	
	for m in FTLToolbox:vesselState:controlPoint:modules {
		local mod is FTLToolbox:vesselState:controlPoint:getmodule(m).
		if mod:alleventnames:contains("control from here") {
			mod:doevent("control from here").
			cpCheck on.
		}
	}
	if not cpCheck success off.
	
	for en in ship:engines {
		if FTLToolbox:vesselState:engines:keys:contains(en:uid) {
			set en:thrustlimit to FTLToolbox:vesselState:engines[en:uid]:thrustlimit.
			if FTLToolbox:vesselState:engines[en:uid]:ignition {
				en:activate.
			}
			else en:shutdown.
		}
		else engCheck off.
	}
	if not engCheck success off.
	
	if not success print "Failed to restore vessel state!".
	else print "Vessel state restored".
}

function updateTarget {
	set FTLToolbox:tarBody to choose target if (hastarget and target:isType("Body")) else (choose target:body if (hastarget and target:isType("Vessel") and target:status <> "Escaping") else body).
	set FTLToolbox:oldTarget to FTLToolbox:tarBody.
}
function updateTarBody {
	parameter cmin is -1.	//custom minAlt
	parameter ctar is -1.	//custom tarAlt
	parameter cmax is -1.	//custom maxAlt
	
	if cmin < 0 or ctar < 0 or cmax < 0 {
		set FTLToolbox:minAlt to max(FTLToolbox:tarBody:atm:height + 1e3, choose getSafeDist(FTLToolbox:tarBody) if (log10(FTLToolbox:tarBody:mass) > 27) else FTLToolbox:tarBody:radius).
		if FTLToolbox:tarBody <> sun set FTLToolbox:maxAlt to (FTLToolbox:tarBody:soiRadius - FTLToolbox:tarBody:radius) * 0.9.
		else set FTLToolbox:maxAlt to sun:orbitingChildren[sun:orbitingChildren:length - 1]:orbit:apoapsis * 1.1.
		set FTLToolbox:tarAlt to min(FTLToolbox:maxAlt, max(FTLToolbox:minAlt, FTLToolbox:tarAlt)).
	}
	
	if cmin > 0 set FTLToolbox:minAlt to cmin.
	if ctar > 0 set FTLToolbox:tarAlt to ctar.
	if cmax > 0 set FTLToolbox:maxAlt to cmax.
}
function updateFlightplan {
	hold().
	set FTLToolbox:flightplan to stack().
	local tarSpeed is circSpeed(FTLToolbox:tarBody, FTLToolbox:tarAlt).
	local minSpeed is circSpeed(FTLToolbox:tarBody, min(FTLToolbox:maxAlt, max(FTLToolbox:tarAlt * (1+FTLToolbox:altMinFactor), FTLToolbox:tarALt + FTLToolbox:altMargin))).
	local maxSpeed is circSpeed(FTLToolbox:tarBody, max(FTLToolbox:minAlt, min(FTLToolbox:tarAlt * (1-FTLToolbox:altMinFactor), FTLToolbox:tarAlt - FTLToolbox:altMargin))).
	
	
	//Building the flightplan stack in reverse:
	FTLToolbox:flightplan:push(lex("mode", 0)).		//Holding at the end
	FTLToolbox:flightplan:push(lex(
		"mode", 6,									//change inclination
		"orbitType", "absolute",
		"inclination", 0,
		"LAN", "any"
	)).
	FTLToolbox:flightplan:push(lex(
		"mode", 5,									//Establish a circular orbit
		"body", FTLToolbox:tarBody,
		"tarAlt", FTLToolbox:tarAlt,
		"minAlt", FTLToolbox:minAlt,
		"maxAlt", FTLToolbox:maxAlt
	)).
	
	if FTLToolbox:tarBody = body {
		if velocity:orbit:mag < minSpeed or velocity:orbit:mag > maxSpeed {
			//outside of altitude target range, need to adjust velocity
			FTLToolbox:flightplan:push(lex(
				"mode", 3,							//GravityBraking
				"orbitType", "equatorial",
				"body", FTLToolbox:tarBody,
				"destination", FTLToolbox:tarBody,
				"tarSpeed", tarSpeed,
				"minSpeed", minSpeed,
				"maxSpeed", maxSpeed
			)).
		}
	}
	else {
		//find route to tarBody
		local route is stack(FTLToolbox:tarBody).
		local subRoute is stack(body).
		until route:peek() = sun route:push(route:peek():body).
		until route:contains(subRoute:peek()) subRoute:push(subRoute:peek():body).
		until route:peek() = subRoute:peek() route:pop().
		route:pop().
		until subRoute:empty route:push(subRoute:pop()).
		
		//find most potent body in route for gravity brakingScore
		local best is body.
		local topScore is -1.
		for bd in route {
			local score is brakingScore(bd).
			if score >= topScore {
				set topScore to score.
				set best to bd.
			}
		}
		
		if best <> FTLToolbox:tarBody FTLToolbox:flightplan:push(lex(
			"mode", 4,								//Transfer to tarBody
			"destination", FTLToolbox:tarBody
		)).
		
		FTLToolbox:flightplan:push(lex(
			"mode", 3,								//GravityBraking at most potent body on route
			"orbitType", "equatorial",
			"body", best,
			"destination", FTLToolbox:tarBody,
			"tarSpeed", tarSpeed,
			"minSpeed", minSpeed,
			"maxSpeed", maxSpeed
		)).
		
		if best <> body FTLToolbox:flightplan:push(lex(
			"mode", 4,								//Transfer to most potent body on route
			"destination", best
		)).
		
		if periapsis < body:radius {
			FTLToolbox:flightplan:push(lex(
				"mode", 2							//Escape to above body:radius
			)).
			
			if apoapsis < body:radius and apoapsis > 0 FTLToolbox:flightplan:push(lex(
				"mode", 1							//Raise apoapsis above body:radius
			)).
		}
	}
}
function updateWarpdrive {
	parameter ignition is false.
	
	local evGen is choose "Activate Main Power" if ignition else "Deactivate Main Power".
	local evAux is choose "Activate Auxilary Power" if ignition else "Deactivate Auxilary Power".
	local evEng is choose "Activate Engine" if ignition else "Shutdown Engine".
	
	for m in ship:modulesnamed("wbiModuleGeneratorFX") {
		if m:hasevent(evGen) m:doevent(evGen).
		if m:hasevent(evAux) m:doevent(evAux).
	}
	for m in ship:modulesnamed("wbiWarpEngine") {
		if m:hasevent(evEng) m:doevent(evEng).
	}
}

function printLine {
	parameter str, line is FTLToolbox:currentLine.
	
	print str:toString():padright(terminal:width) at(0, line).
	if line = FTLToolbox:currentLine set FTLToolbox:currentLine to FTLToolbox:currentLine + 1.
}
function printCore {
	printLine("[ ]FTLToolbox " + FTLToolbox:version + " ||| " + FTLToolbox:modes[FTLToolbox:activeLeg:mode] + " |||").
	printLine("Flightplan steps remaining: " + FTLToolbox:flightplan:length).
	printLine("Throttle: " + round(FTLToolbox:outputThrottle,2)).
	printLine("pilotThr: " + round(ship:control:pilotmainthrottle,2)).
	printLine("sl      : " + round(internalThrottle,2)).
	printLine("").
}
function printMode0 {
	if FTLToolbox:flightplan:empty {
		printLine("Flightplan is empty. Thank you for travelling with FTL Drives!").
	}
	
	for leg in FTLToolbox:flightplan {
		if leg:mode = 0 {
			printLine("HOLD       : Let the user decide how to continue from here.").
		}
		else if leg:mode = 1 {
			printLine("Raise AP   : Raise the apoapsis above the FTL altitude limit.").
		}
		else if leg:mode = 2 {
			printLine("ESCAPE     : Escape the gravity well until FTL travel becomse possible.").
		}
		else if leg:mode = 3 {
			printLine("GBRAKE     : Use the gravity of " + leg:body:name + " to match " + leg:destination:name + "'s velocity.").
		}
		else if leg:mode = 4 {
			printLine("TRANSFER   : Use FTL travel to transfer to " + leg:destination:name + "'s soi.").
		}
		else if leg:mode = 5 {
			printLine("CIRCULARIZE: Establish a circular orbit at " + round(leg:tarAlt/1000) + " km around " + leg:body:name + ".").
		}
		else if leg:mode = 6 {
			printLine("INCLINATION: Change the inclination to " + leg:orbitType + ".").
		}
		else {
			printLine("ERROR      : Mode #" + leg:mode + " is not defined. Flightplan might be corrupted!!").
		}
	}
}
function printMode1 {
	//TODO: implement this properly
	for k in FTLToolbox:activeLeg:keys printLine(k + ": " + FTLToolbox:activeLeg[k]).
}
function printMode2 {
	//TODO: implement this properly
	for k in FTLToolbox:activeLeg:keys printLine(k + ": " + FTLToolbox:activeLeg[k]).
}
function printMode3 {
	//TODO: implement this properly
	for k in FTLToolbox:activeLeg:keys printLine(k + ": " + FTLToolbox:activeLeg[k]).
	printLine("Flip: " + flip).
	printLine("Relvel: " + relvel:mag).
	printLine("Angle: " + vang(relvel, body:position)).
	printLine("minAngle : " + minAngle).
	printLine("currAngle: " + currAngle).
}
function printMode4 {
	//TODO: implement this properly
	for k in FTLToolbox:activeLeg:keys printLine(k + ": " + FTLToolbox:activeLeg[k]).
	printLine("vMax: " + FTLToolbox:vMax).
}
function printMode5 {
	//TODO: implement this properly
	for k in FTLToolbox:activeLeg:keys printLine(k + ": " + FTLToolbox:activeLeg[k]).
	printLine("minAngle : " + minAngle).
	printLine("currAngle: " + currAngle).
}
function printMode6 {
	//TODO: implement this properly
	for k in FTLToolbox:activeLeg:keys printLine(k + ": " + FTLToolbox:activeLeg[k]).
}

function hold {
	lock internalThrottle to 0.
	set FTLToolbox:outputThrottle to internalThrottle.
	set ship:control:pilotmainthrottle to 0.
	
	lock internalSteering to ship:facing.
	set FTLToolbox:outputSteering to internalSteering.
	
	updateWarpdrive(false).
	
	if FTLToolbox:activeLeg:mode <> 0 {
		if FTLToolbox:activeLeg:keys:contains("active") FTLToolbox:activeLeg:active off.
		if FTLToolbox:activeLeg:keys:contains("waitForUser") FTLToolbox:activeLeg:waitForUser off.
		
		FTLToolbox:flightplan:push(FTLToolbox:activeLeg).
	}
	set FTLToolbox:activeLeg to lex("mode", 0).
}
function go {
	if FTLToolbox:activeLeg:mode = 0 {
		if FTLToolbox:flightplan:length > 0 set FTLToolbox:activeLeg to FTLToolbox:flightplan:pop().
		else print "Flightplan is empty!".
	}
	else if FTLToolbox:activeLeg:hassuffix("waitForUser") and FTLToolbox:activeLeg:waitForUser FTLToolbox:activeLeg:waitForUser off.
	else print "Already GO!".
}
function proceed {
	updateWarpdrive(false).
	set warp to 0.
	wait until kuniverse:timewarp:issettled.
	wait 0.
	if FTLToolbox:activeLeg:mode = 0 {
		print "Will not proceed during a HOLD! Did you mean 'GO'?".
	}
	else if FTLToolbox:flightplan:empty {
		set FTLToolbox:activeLeg to lex("mode", 0).
	}
	else set FTLToolbox:activeLeg to FTLToolbox:flightplan:pop().
	wait 0.
}

function leg0 {
	parameter leg is FTLToolbox:activeLeg.
	
	if leg:mode = 0 {
		print "FTLToolbox HOLDING".
		
		lock internalThrottle to 0.
		set FTLToolbox:outputThrottle to 0.
		set ship:control:pilotmainthrottle to 0.
		updateWarpdrive(false).
		
		lock internalSteering to "kill".
		set FTLToolbox:outputSteering to "kill".
		
		restoreVesselState().
	}
	return true.
}
function leg1 {
	parameter leg is FTLToolbox:activeLeg.
	
	if leg:mode = 1 {			//RaisingAP
		//TODO: test
		print "FTLToolbox raising AP".
		
		//validations
		if apoapsis > body:radius or apoapsis < 0 {
			//no need to raise ap, already above FTL limit.
			proceed().
			return true.
		}
		if periapsis < max(body:atm:height + 1e3, 1e4) {
			//periapsis is too low
			hold().
			print " ".
			print "Please raise your PE to a safe altitude.".
			print "At least 1 km above the atmosphere.".
			print "At least 10 km above sea level.".
			print "This might still be too low sometimes.".
			print "I'll blame you if we hit something!".
			doAlertUser().
			return true.
		}
		
		set FTLToolbox:activeLeg:active to true.
		
		//node planning
		for n in allnodes remove n.
		add node(time:seconds + eta:periapsis, 0, 0, sqrt(body:mu * (2/(body:radius + periapsis) - 2/(3 * body:radius + periapsis + 1000))) - sqrt(body:mu * (2/(body:radius + periapsis) - 1/orbit:semimajoraxis))).
		lock internalSteering to nextnode:deltav.
		set FTLToolbox:outputSteering to internalSteering.
		
		
		//is it safe to do the burn in this vessel setup?
		updateWarpdrive(false).
		leg:waitForUser off.
		for en in ship:engines {
			if en:ignition and en:thrustlimit > 0 and vang(ship:facing:vector, en:facing:vector) > 5 {
				leg:waitForUser on.
				break.
			}
		}
		if ship:availablethrust = 0 leg:waitForUser on.
		
		if leg:waitForUser {
			FTLToolbox:activeLeg:waitForUser on.
			print " ".
			print "Please make sure that it is safe to burn this node with the current engine 'setup' and controlPoint. Make any changes neccesary, then call GO() when ready.".
			doAlertUser().
		}
		
		
		//node execution
		when
		(vang(steeringmanager:target:vector, ship:facing:forevector) < 1 and FTLToolbox:activeLeg:hassuffix("waitForUser") and not FTLToolbox:activeLeg:waitForUser)
		or not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active)
		or not FTLToolbox:enabled
		then {
			if not FTLToolbox:enabled return false.
			if not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active) return false.
			if FTLToolbox:activeLeg:mode <> 1 return false.
			
			local acc is ship:availablethrust/ship:mass.
			local dur is nextnode:deltav:mag/acc.
			local igt is nextnode:time - dur/2.
			
			updateWarpdrive(false).
			set warp to 1.
			wait until kuniverse:timewarp:issettled.
			warpto(igt - 60).
			wait until (igt - time:seconds) < 60 and vang(steeringmanager:target:vector, ship:facing:forevector) < 0.05.
			warpto(igt - 5).
			wait until time:seconds >= igt.
			
			lock internalThrottle to 3*nextnode:deltav:mag/acc.
			when
			nextnode:deltav:mag < 0.01
			or vdot(nextnode:deltav, ship:facing:forevector) < 0
			or not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active)
			or not FTLToolbox:enabled
			then {
				if not FTLToolbox:enabled return false.
				if not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active) return false.
				if FTLToolbox:activeLeg:mode <> 1 return false.
				
				lock internalThrottle to 0.
				set FTLToolbox:outputThrottle to 0.
				set ship:control:pilotmainthrottle to 0.
				lock internalSteering to "kill".
				set FTLToolbox:outputSteering to "kill".
				remove nextnode.
				
				wait .5.
				proceed().
			}
		}
	}
	return true.
}
function leg2 {
	parameter leg is FTLToolbox:activeLeg.
	
	if leg:mode = 2 {		//Escaping
		//TODO: test
		//TODO: only raise pe if neccesary
		print "FTLToolbox escaping".
		
		//validations
		if apoapsis < body:radius and apoapsis > 0 {
			//need to raise apoapsis
			FTLToolbox:flightplan:push(activeLeg).
			set FTLToolbox:activeLeg to lex("mode", 1).
			return true.
		}
		if periapsis > body:radius {
			//already high enough
			proceed().
			return true.
		}
		
		set FTLToolbox:activeLeg:active to true.
		
		for en in ship:engines en:shutdown.
		updateWarpdrive(false).
		
		local escapeTime is time:seconds.
		until (positionat(ship, escapeTime) - body:position):mag > body:radius * 2 or escapeTime >= time:seconds + eta:apoapsis set escapeTime to escapeTime + 1.
		
		lock internalSteering to positionat(ship, escapeTime) - body:position.
		if escapeTime < time:seconds lock internalSteering to up.
		doHasteSteering().
		
		when
		vang(steeringmanager:target:vector, ship:facing:forevector) < 2
		or not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active)
		or not FTLToolbox:enabled
		then {
			if FTLToolbox:activeLeg:mode <> 2 return false.
			if not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active) return false.
			if not FTLToolbox:enabled return false.
			
			if altitude < body:radius and escapeTime < time:seconds {
				//fell below FTL altitude limit during turning. restart leg.
				//this should not happen twice in a row.
				hold().
				wait 0.
				go().
			}
			if escapeTime > time:seconds + 15 {
				set warp to 1.
				wait until kuniverse:timewarp:issettled.
				warpto(escapeTime).
			}
			
			for en in ship:engines en:shutdown.
			updateWarpdrive(true).
			//lock internalThrottle to min(1, max(0, (body:radius * 1.5 - periapsis)/FTLToolbox:tailing)) * min(1, max(0, (vdot(steeringmanager:target:vector, up:vector) - 0.5))).
			lock internalThrottle to (body:radius - periapsis)/FTLToolbox:tailing.
			lock internalSteering to up.
			doHasteSteering().
			
			when
			periapsis > body:radius
			or (FTLToolbox:activeLeg:hassuffix("body") and FTLToolbox:activeLeg:body <> body)
			or not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active)
			or not FTLToolbox:enabled
			then {
				if FTLToolbox:activeLeg:mode <> 2 return false.
				if not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active) return false.
				if not FTLToolbox:enabled return false.
				
				lock internalThrottle to 0.
				set FTLToolbox:outputThrottle to 0.
				set ship:control:pilotmainthrottle to 0.
				
				lock internalSteering to "kill".
				set FTLToolbox:outputSteering to "kill".
				
				proceed().
			}
		}
	}
	return true.
}
function leg3 {
	parameter leg is FTLToolbox:activeLeg.
	
	if leg:mode = 3 {		//GravityBraking
		//TODO: test
		print "FTLToolbox gBraking".
		
		//validations
		if leg:body <> body {
			//need to transfer first
			local dest is leg:body.
			hold().
			FTLToolbox:flightplan:push(lex("mode", 4, "destination", dest)).
			go().
			return true.
		}
		
		set FTLToolbox:activeLeg:active to true.
		
		local dest is leg:destination.
		local sMin is leg:minSpeed.
		local sMax is leg:maxSpeed.
		local sTar is leg:tarSpeed.
		
		lock minDist to body:radius + getSafeDist(body).
		//lock relvel to dest:velocity:orbit - ship:velocity:orbit.
		lock relvel to getOrbitalVelocityRelativeToSun(dest, time:seconds) - getOrbitalVelocityRelativeToSun(ship, time:seconds).
		lock flip to choose 1 if relvel:mag > sTar else -1.
		lock gbrakepos to body:position - relvel:normalized * flip * mindist.
		lock minAngle to ceiling(arcsin(max(-1, min(1, getSafeDist(body)/body:position:mag))) + 1).
		lock currAngle to vang(gbrakepos, body:position).
		lock rotAxis to vcrs(body:position, gbrakepos):normalized.
		lock steerTangential to angleaxis(minAngle, rotAxis) * body:position.
		
		lock gSteer to choose gbrakepos if (currAngle > minAngle or vdot(gbrakepos - body:position, vxcl(body:position, gbrakepos)) < 0) else steerTangential.
		//TODO: fix steering fluctuations
		lock internalSteering to gSteer.
		doHasteSteering().
		
		//TODO: remove after debug
		set vdT to vecdraw(v(0,0,0), {return steerTangential.}, red, "Tangential", 1, true).
		set vdG to vecdraw(v(0,0,0), {return gBrakePos.}, yellow, "GBrakePos", 1, true).
		
		for en in ship:engines en:shutdown.
		updateWarpdrive(true).
		
		//lock gThr to min(FTLToolbox:FTLLimit, min(1, gbrakepos:mag/FTLToolbox:tailing) * (FTLToolbox:steeringPrecision - abs(steeringmanager:angleerror))).
		lock gTHR to gBrakePos:mag/FTLToolbox:tailing.
		lock internalThrottle to gThr.
		
		when
		(relvel:mag < sMax and relvel:mag > sMin)
		or not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active)
		or not FTLToolbox:enabled
		then {
			if FTLToolbox:activeLeg:mode <> 3 return false.
			if not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active) return false.
			if not FTLToolbox:enabled return false.
			
			updateWarpdrive(false).
			set ship:control:pilotmainthrottle to 0.
			set warp to 1.
			wait 0.
			set warp to 0.
			wait until kuniverse:timewarp:issettled.
			updateWarpdrive(false).
			
			lock internalThrottle to 0.
			set FTLToolbox:outputThrottle to 0.
			set ship:control:pilotmainthrottle to 0.
			
			lock internalSteering to "kill".
			set FTLToolbox:outputSteering to "kill".
			
			proceed().
		}
		
		local marginOverride is FTLToolbox:gbrakeMargin.
		if gbrakepos:mag <= FTLToolbox:gbrakeMargin set marginOverride to gbrakepos:mag/2.
		
		when
		gbrakepos:mag < min(FTLToolbox:gBrakeMargin, marginOverride)
		or (vang(up:vector, relvel * (sTar - relvel:mag)) < 2 and altitude > minDist)
		or not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active)
		or not FTLToolbox:enabled
		then {
			//TODO: DEBUG
			if FTLToolbox:activeLeg:mode <> 3 return false.
			if not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active) return false.
			if not FTLToolbox:enabled return false.
			
			updateWarpdrive(false).
			
			local tx is time:seconds.
			local dV is relvel.
			local step is 1e4.
			until step < 1e-3 and (dV:mag > sMin and dV:mag < sMax) {
				set tx to tx + step.
				set dV to getOrbitalVelocityRelativeToSun(dest, tx) - getOrbitalVelocityRelativeToSun(ship, tx).
				printline("Finding warp time " + (tx - time:seconds) + "/" + step, FTLToolbox:firstLine).
				if 
				(dV:mag < sTar + 1 and dv:mag > sTar - 1)
				or (orbit:hasnextpatch and eta:transition < tx - time:seconds)
				or (positionat(ship, tx) - body:position):mag < minDist
				or vdot((positionat(ship, tx) - body:position):normalized, (dV:normalized * (sTar - dV:mag)):normalized) < 0.7
				{
					//cannot safely warp any further
					set tx to tx - step.
					set step to step/2.
				}
				if step < 1e-3 break.
			}
			
			if tx - time:seconds > 15 {
				//if dV:mag <= sMax and dV:mag >= sMin {
				//	lock internalSteering to FTLToolbox:activeLeg:destination:position.
				//	doHasteSteering().
				//	print ("Rotating to face destination").
				//	wait until vang(internalSteering, facing:vector) < FTLToolbox:steeringPrecision.
				//}
				set warp to 1.
				wait until kuniverse:timewarp:issettled.
				warpto(tx-5).
			}
			wait until warp = 0 and kuniverse:timewarp:issettled.
			wait 0.
			
			updateWarpdrive(true).
			doHasteSteering().
			return true.
		}
	}
	return true.
}
function leg4 {
	parameter leg is FTLToolbox:activeLeg.
	
	if leg:mode = 4 {		//Transferring
		//TODO: test
		print "FTLToolbox transferring".
		
		//validations
		if altitude < body:radius {
			//too low for FTL drives
			hold().
			flightplan:push(lex("mode", 1)).
			go().
			return true.
		}
		
		set FTLToolbox:activeLeg:active to true.
		
		local dest is leg:destination.
		
		lock path to dest:position.
		lock minAngle to ceiling(arcsin(max(-1, min(1, getSafeDist(body)/body:position:mag))) + 3).
		lock currAngle to vang(path, body:position).
		lock rotAxis to vcrs(body:position, path):normalized.
		lock steerTangential to angleaxis(minAngle, rotAxis) * body:position.
		
		lock internalSteering to choose path if (currAngle > minAngle or vdot(path - body:position, body:position) < 0) else steerTangential.
		doHasteSteering().
		
		//TODO: remove after debug
		set vdT to vecdraw(v(0,0,0), {return steerTangential.}, red, "Tangential", 1, true).
		set vdG to vecdraw(v(0,0,0), {return path.}, yellow, "Path", 1, true).
		
		//if dest:hassuffix("soiradius") lock internalThrottle to min(FTLToolbox:FTLLimit, min(1, (path:mag - dest:soiradius * 0.9)/FTLToolbox:tailing) * (FTLToolbox:steeringPrecision-abs(steeringmanager:angleerror))).
		//else lock internalThrottle to min(FTLToolbox:FTLLimit, min(1, (path:mag - body:soiradius)/FTLToolbox:tailing) * (FTLToolbox:steeringPrecision-abs(steeringmanager:angleerror))).
		//if dest:hassuffix("soiradius") lock internalThrottle to (path:mag - dest:soiradius * 0.9)/FTLToolbox:tailing.
		//else lock internalThrottle to choose ((path:mag - body:soiradius * 1.1)/FTLToolbox:tailing) if body:hassuffix("soiradius") else .1.
		//lock internalThrottle to (path:mag - (choose (dest:soiradius * 0.9) if dest:hassuffix("soiradius") else (choose (body:soiradius * 1.1) if body:hassuffix("soiradius") else 0)))/FTLToolbox:tailing.
		lock internalThrottle to (path:mag - dest:radius * 3)/FTLToolbox:tailing.
		
		for en in ship:engines en:shutdown.
		updateWarpdrive(true).
		
		when
		body = dest
		or not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active)
		or not FTLToolbox:enabled
		then {
			if FTLToolbox:activeLeg:mode <> 4 return false.
			if not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active) return false.
			if not FTLToolbox:enabled return false.
			
			lock internalSteering to "kill".
			lock internalThrottle to 0.
			set FTLToolbox:outputThrottle to 0.
			set ship:control:pilotmainthrottle to 0.
			
			proceed().
		}
	}
	return true.
}
function leg5 {
	parameter leg is FTLToolbox:activeLeg.
	
	if leg:mode = 5 {		//Circularizing
		//TODO: test
		print "FTLToolbox circularizing".
		
		//validations
		if leg:body <> body {
			//not at the desired body
			local dest is leg:body.
			hold().
			FTLToolbox:flightplan:push(lex("mode", 4, "destination", dest)).
			go().
			return true.
		}
		
		set FTLToolbox:activeLeg:active to true.
		
		local dest is leg:body.
		
		lock path to dest:position + vcrs(-dest:angularvel, velocity:orbit - dest:velocity:orbit):normalized * (dest:mu/((velocity:orbit - dest:velocity:orbit):mag)^2).
		lock minAngle to ceiling(arcsin(max(-1, min(1, getSafeDist(body)/body:position:mag))) + 3).
		lock currAngle to vang(path, body:position).
		lock rotAxis to vcrs(body:position, path):normalized.
		lock steerTangential to angleaxis(minAngle, rotAxis) * body:position.
		
		lock internalSteering to choose path if (currAngle > minAngle or vdot(path - body:position, vxcl(body:position, path)) < 0) else steerTangential.
		doHasteSteering().
		
		for en in ship:engines en:shutdown.
		updateWarpdrive(true).
		//lock internalThrottle to min(FTLToolbox:FTLLimit, min(1, path:mag/FTLToolbox:tailing) * (FTLToolbox:steeringPrecision-abs(steeringmanager:angleerror))).
		lock internalThrottle to path:mag/FTLToolbox:tailing.
		
		when
		orbit:eccentricity < FTLToolbox:eccMargin
		or not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active)
		or not FTLToolbox:enabled
		then {
			if FTLToolbox:activeLeg:mode <> 5 return false.
			if not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active) return false.
			if not FTLToolbox:enabled return false.
			
			lock internalThrottle to 0.
			set FTLToolbox:outputThrottle to 0.
			set ship:control:pilotmainthrottle to 0.
			
			lock internalSteering to "kill".
			updateWarpdrive(false).
			
			proceed().
		}
	}
	return true.
}
function leg6 {
	parameter leg is FTLToolbox:activeLeg.
	
	if leg:mode = 6 {		//InclinationChange
		//TODO:
		print "FTLToolbox inclination change".
		
		set FTLToolbox:activeLeg:active to true.
		
		//TODO: implement remaining inclination modes
		if leg:orbitType = "absolute" {
			if leg:LAN = "any" {
				set leg:inclination to min(90, leg:inclination).
				local tx is time:seconds.
				local f is choose 1 if vdot(north:vector, velocity:orbit) > 0 else -1.
				local delta is orbit:inclination - leg:inclination.
				local step is 10^floor(log10(orbit:period/4)).
				
				until step < 1e-3 and vdot(north:vector, f * velocityat(ship, tx):orbit) < 0 {
					set tx to tx + step.
					if
					vdot(north:vector, f * velocityat(ship, tx):orbit) < 0 
					or (orbit:hasnextpatch and eta:transition < tx - time:seconds)
					{
						set tx to tx - step.
						set step to step/10.
					}
					print round(tx - time:seconds, 3) + " || " + step.
					if step < 1e-3 break.
				}
				
				lock internalSteering to lookdirup(velocityat(ship, tx):orbit, positionat(ship, tx) - body:position):starvector * delta * f.
				doHasteSteering().
				
				if FTLToolbox:incMargin > orbit:inclination - leg:inclination {
					proceed().
					return true.
				}
				
				when
				vang(internalSteering, ship:facing:vector) < 1
				or not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active)
				or not FTLToolbox:enabled
				then {
					if FTLToolbox:activeLeg:mode <> 6 return false.
					if not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active) return false.
					if not FTLToolbox:enabled return false.
					
					updateWarpdrive(false).
					if tx - time:seconds > 90 {
						set warp to 1.
						wait until kuniverse:timewarp:issettled.
						warpto(tx - 80).
						wait until warp = 0 and kuniverse:timewarp:issettled.
					}
					wait until vang(internalSteering, ship:facing:forevector) < .5.
					if tx - time:seconds > 30 {
						set warp to 1.
						wait until kuniverse:timewarp:issettled.
						warpto(tx - 15).
						wait until warp = 0 and kuniverse:timewarp:issettled.
					}
					lock internalSteering to prograde:starvector * (orbit:inclination - leg:inclination) * f.
					
					wait until tx - time:seconds < 10.
					updateWarpdrive(true).
					//lock internalThrottle to min(FTLToolbox:FTLLimit, min(1, max(throttleGridSize/10, internalSteering:mag/FTLToolbox:tailing)) * (FTLToolbox:steeringPrecision - abs(steeringmanager:angleerror))).
					lock internalThrottle to internalSteering:mag/FTLToolbox:tailing.
					
					
					when
					internalSteering:mag < FTLToolbox:incMargin
					or latitude * f <= 0
					or not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active)
					or not FTLToolbox:enabled
					then {
						if FTLToolbox:activeLeg:mode <> 6 return false.
						if not (FTLToolbox:activeLeg:hassuffix("active") and FTLToolbox:activeLeg:active) return false.
						if not FTLToolbox:enabled return false.
						
						lock internalThrottle to 0.
						set ship:control:pilotmainthrottle to 0.
						
						//if internalSteering:mag > FTLToolbox:incMargin {
						//	hold().
						//	wait 0.
						//	go().
						//}
						//else proceed().
						proceed().
					}
				}
			}
		}
	}
	return true.
}


//=========
//main code
//=========
{
set FTLToolbox to lex().
set FTLToolbox to lex(
	"enabled", true,
	"version", "v0.2.5",
	"modes", list(
		"Holding",					//0 Holding
		"RaisingAP",				//1 RaisingAP
		"Escaping",					//2 Escaping
		"GravityBraking",			//3 GravityBraking
		"Transferring",				//4 Transferring
		"Circularizing",			//5 Circularizing
		"InclinationChange"			//6 InclinationChange
	),
	"tick", 0,
	"flightplan", stack(),
	"activeLeg", lex("mode", 0),
	"controlPoints", list(),
	"warpEngine", "none",
	"vesselState", lex("engines", lex(), "controlPoint", ship:controlpart),
	"alertDuration", 0.4,
	"alertInterval", 0.03,
	"vMax", 0,
	"tarBody", updateTarget(),
	"tarAlt", 0,
	"altMargin", 1e4,				//allowed altitude deviation in meter (overruled by altMinFactor)
	"altMinFactor", 1e-3,			//minimum allowed altitude deviation in percentage
	"FTLLimit", 1e4,				//maximum allowed throttle percent during FTL maneuvers
	"steeringPrecision", 2,			//maximum allowed angleerror before reducing throttle (actually throttle starts to be reduced 1 deg earlier, reaching 0 at the set value)
	"tailing", 1e8,
	"currentLine", 0,
	"firstLine", 0,
	"lastLine", 0,
	"printer", list(printMode0@, printMode1@, printMode2@, printMode3@, printMode4@, printMode5@, printMode6@),
	"legs", list(leg0@, leg1@, leg2@, leg3@, leg4@, leg5@, leg6@),
	"doPrint", true,
	"tps", 20,						//ticks per second for control-loop
	"hasteSteering", true,			//allow switching control point for faster steering during FTL maneuvers
	"gBrakeMargin", 2e5,			//how close do we need to be to the desired spot before using timewarp during GBrake maneuvers.
	"eccMargin", 1e-3,				//maximum eccentricity allowed to complete a circularization
	"incMargin", 1e-2,				//maximum deviation angle allowed to complete a inclination change
	"thrMult", 1,					//throttle multiplier to allow for in-flight adjustments when needed
	"outputThrottle", 0,			//used for throttle lock, updated (tick/2) times per second. Lock throttle back to this after using a custom throttle lock
	"maxThrottle", 1,
	"outputSteering", ship:facing	//used for steering lock, updated (tick/2) times per second. Lock steering back to this after using a custom steering lock
).

lock steering to FTLToolbox:outputSteering.
lock throttle to 0.
unlock throttle.
lock throttleGridSize to min(.1, FTLToolbox:thrMult * body:mu^4/1e75).	//TODO: account for spatial location

storeVesselState().
findPartModules().
updateTarget().
updateTarBody().
updateFlightplan().
hold().

for m in ship:modulesnamed("wbiWarpEngine") {
	if m:hasfield("superCharger") m:setfield("superCharger", 1000).		//should be capped to the max at 200
}

//==================
//loops and triggers
//==================



on ship:parts:length {
	if not FTLToolbox:enabled return false.
	findPartModules().
	return true.
}
on FTLToolbox:tarBody {
	if not FTLToolbox:enabled return false.
	set FTLToolbox:tarAlt to 0.
	updateTarBody().
	return true.
}


//control loop
global function startControlLoop {
on round(time:seconds * FTLToolbox:tps) {
	if not FTLToolbox:enabled {
		set FTLToolbox:outputThrottle to 0.
		set FTLToolbox:outputSteering to "kill".
		set ship:control:pilotmainthrottle to 0.
		updateWarpdrive(false).
		restoreVesselState().
		
		print "FTLTOOLBOX HAS SHUT DOWN".
		return false.
	}
	
	if FTLToolbox:tick = 0 {
		//update steering and target
		set FTLToolbox:tick to 1.
		//wait 0.
		set FTLToolbox:outputSteering to internalSteering.
		if not (hastarget and target = FTLToolbox:oldTarget) updateTarget().
		print "O" at(1,0).
	}
	else {
		//update throttle and print
		set FTLToolbox:tick to 0.
		//wait 0.
		
		
		if FTLToolbox:warpEngine:isType("PartModule") {
			set FTLToolbox:vMax to 0.
			for m in ship:modulesnamed("wbiwarpengine") if m:hasfield("max warp speed") set FTLToolbox:vMax to max(FTLToolbox:vMax, m:getfield("max warp speed")).
			if kuniverse:timewarp:issettled and warp = 0 and FTLToolbox:vMax <> 0 set FTLToolbox:outputThrottle to min(1, max(0, min(FTLToolbox:FTLLimit, internalThrottle)/FTLToolbox:vMax)) * (FTLToolbox:steeringPrecision-abs(steeringmanager:angleerror)).
			else if FTLToolbox:activeLeg:mode = 1 set FTLToolbox:outputThrottle to internalThrottle.
			else set FTLToolbox:outputThrottle to 0.
			
			set ship:control:pilotmainthrottle to ceiling(min(FTLToolbox:maxThrottle, max(0, FTLToolbox:outputThrottle)), (choose 2 if (FTLToolbox:outputThrottle < 0.1) else 1)).
		}
		else {
			hold().
			wait 0.
			FTLToolbox:enabled off.
			print "No WarpEngines found, shutting down!".
			doAlertUser().
		}
		
		if FTLToolbox:doPrint {
			set FTLToolbox:currentLine to FTLToolbox:firstLine.
			printCore().
			FTLToolbox:printer[max(0, choose FTLToolbox:activeLeg:mode if (FTLToolbox:activeLeg:mode < FTLToolbox:modes:length) else 0)]:call().
			printLine("").
			until FTLToolbox:currentLine > FTLToolbox:lastLine printLine("").
			set FTLToolbox:lastLine to FTLToolbox:currentLine - 1.
		}
		print "X" at(1,0).
	}
	
	return true.
}
}
startControlLoop().

//Autopilot trigger
global function startapt {
on FTLToolbox:activeLeg {
	if not FTLToolbox:enabled return false.
	
	lock internalThrottle to 0.
	lock internalSteering to "kill".
	set ship:control:pilotmainthrottle to 0.
	
	local leg is FTLToolbox:activeLeg.
	
	if leg:mode = 1 FTLToolbox:legs[1]:call(leg).
	else if leg:mode = 2 FTLToolbox:legs[2]:call(leg).
	else if leg:mode = 3 FTLToolbox:legs[3]:call(leg).
	else if leg:mode = 4 FTLToolbox:legs[4]:call(leg).
	else if leg:mode = 5 FTLToolbox:legs[5]:call(leg).
	else if leg:mode = 6 FTLToolbox:legs[6]:call(leg).
	else FTLToolbox:legs[0]:call(leg).
	
	return true.
}
}
startapt().

global function revive {
	startapt().
	startcontrolloop().
	lock steering to ftltoolbox:outputsteering.
	lock throttle to ftltoolbox:outputthrottle.
}


clearscreen. for i in range(terminal:height) print " ".
set FTLToolbox:FTLLimit to 1.
set FTLToolbox:thrMult to 4e17.
}