Clone of Kadin's autopilot2

set k_autopilot2_version to "0.7".
print "Kadin's autopilot2 V" + k_autopilot2_version:tostring.


print "There are three versions of this code.".
print "Unstable/development version, newest features, newest bugs: !runscript NHwvPAp3".
print "Recent working version, seems like it is fairly stable: !runscript 0pgtewL5".
print "Oldest version, least features but also fairly stable: !runscript dHtuDeGx".


//latest change: hl() for hold line.: hl(runway_west,runway_east).
//latest change: blurb about version of the code.
//latest change: added hg() for hold glideslope: hg(target_slope, ground_target, voffset, hoffset).

set bank_change_rate to 3.				// How fast we change between bank angles .
set rudder_rate to 0.4.					// How much rudder to kick in to help increase the turn rate.
set default_bank_limit to 40.			// This is how many degrees of bank for a turn if none are specified.
set hud_message to "Want to have a go? Speak up in chat! I'm just testing code, and will happily stop and let you have a go!".

if ship:name:startswith("Mallard") or ship:name:startswith("kadin landed here") {
	set vs to 65.				// stall speed in level flight - complete guess.
	set vsr to 60.	//normal stall speed.
	set vs0 to 40.	//stall speed with flaps out
	set vto to 100.	//take off speed
	set vfe to 65.	//speed above which you would remove flaps.
	set vlo to 60.	//speed below which you CAN extend/retract gear.
	set vc to 80.	//cruise speed.
	set vt to 45.	//threshold speed.
	set vtd to 40.	//touchdown speed.
	set alt_radar_adj to 3.0433.
	//for p in ship:partsnamed("smallCtrlSrf")    p:getmodule("ModuleControlSurface"):setfield("deploy direction",true).
	for p in ship:partsnamed("airlinerCtrlSrf") set p:tag to "flaps".
	for p in ship:partsnamed("airlinerCtrlSrf") p:getmodule("ModuleControlSurface"):setfield("deploy direction",choose false if p:position:mag < 8 else true).
if not (bodyexists("Lua")) {
	for p in ship:partsnamed("airlinerCtrlSrf") {
		if p:position:mag > 6 and p:position:mag <8 p:getmodule("ModuleControlSurface"):setfield("deploy direction",true).
	}
}
	for p in ship:partsnamed("airlinerCtrlSrf") p:getmodule("ModuleControlSurface"):setfield("deploy angle",choose 23 if p:position:mag < 8 else 15).
	for p in ship:partstagged("flaps") p:getmodule("ModuleControlSurface"):setfield("deploy",false).
	for p in ship:partsnamed("airbrake1") set p:tag to "airbrakes".
	for p in ship:partstagged("airbrakes")  p:getmodule("ModuleAeroSurface"):setfield("deploy",false).
	for p in ship:partsnamed("tailfin") set p:tag to "spoilers".
	for p in ship:partsnamed("tailfin") p:getmodule("ModuleControlSurface"):setfield("deploy angle",15).
	for p in ship:partstagged("spoilers") p:getmodule("ModuleControlSurface"):setfield("deploy",false).
	set bank_limit to 40.

	for p in ship:partsnamed("GearMedium") p:getmodule("ModuleWheelBrakes"):setfield("brakes",200).

	for p in ship:partsnamed("JetEngine") set p:tag to "jets".

} else if ship:name:startswith("Aeris 4A")  {
	set alt_radar_adj to 1.6.	// has wheels on the ground at a reported alt:radar of 1.6
	local rude_word is "thru" + "st limiter".	//thank you so much Pastebin!
	//differential_thrust is a possibility partsnamed("turboFanEngine") getmodule("ModuleEnginesFX")  setfield(rude_word,0).
	//differential braking is a possibility partsnamed("SmallGearBay") getModule("ModuleWheelBase", "ModuleWheelBrakes" "ModuleWheelSteering"....
	// speed boost using the aerospike maybe? partsnamed("toroidalAerospike") getmodule("ModuleEnginesFX") setfield(rude_word,0)
	set bank_limit to 50.
	for p in ship:partsnamed("elevon2") p:getmodule("ModuleControlSurface"):setfield("deploy direction",true).
	for p in ship:partsnamed("StandardCtrlSrf") p:getmodule("ModuleControlSurface"):setfield("deploy direction",true).
	for p in ship:partsnamed("elevon2") p:getmodule("ModuleControlSurface"):setfield("deploy angle",10).
	for p in ship:partsnamed("StandardCtrlSrf") p:getmodule("ModuleControlSurface"):setfield("deploy angle",10).
	for p in ship:partsnamed("R8winglet") p:getmodule("ModuleControlSurface"):setfield("deploy angle",15).
	for p in ship:partsnamed("elevon2") set p:tag to "flaps".
	for p in ship:partsnamed("StandardCtrlSrf") set p:tag to "flaps".
	for p in ship:partsnamed("R8winglet") set p:tag to "flaps".
	ship:partsnamed("SmallGearBay")[0]:getmodule("ModuleWheelBrakes"):setfield("brakes",25).
	ship:partsnamed("SmallGearBay")[0]:getmodule("ModuleWheelBrakes"):setfield("brakes",150).
	ship:partsnamed("SmallGearBay")[0]:getmodule("ModuleWheelBrakes"):setfield("brakes",150).
	set vs to 55.
	set vsr to 55.
	set vs0 to 55.
	set vfe to 65.
	set vlo to 65.
	set vto to 100.
	set vc to 80.
	set vtd to 55.
	set vt to 60.
} else if ship:name:startswith("Aeris 3A") {
	set alt_radar_adj to 1.410271525383.
	for p in ship:partsnamed("R8winglet") if p:position:mag < 4 p:getmodule("ModuleControlSurface"):setfield("deploy angle",10).
	for p in ship:partsnamed("R8winglet") if p:position:mag < 4 set p:tag to "flaps".
	for p in ship:partsnamed("elevon3") p:getmodule("ModuleControlSurface"):setfield("deploy angle",15).
	for p in ship:partsnamed("elevon3") set p:tag to "flaps".
	for p in ship:partsnamed("R8winglet") { if p:position:mag > 4 p:getmodule("ModuleControlSurface"):setfield("deploy angle",15). }
	for p in ship:partsnamed("R8winglet") if p:position:mag > 4 set p:tag to "spoilers".
	set bank_limit to 60.
	set vs to 32.
	set vsr to 32.
	set vs0 to 32.
	set vfe to 35.
	set vlo to 35.
	set vto to 80.
	set vc to 60.
	set vtd to 32.
	set vt to 35.
} else if ship:name:startswith("Lockheed Jetstar") {
	set alt_radar_adj to 1.6479332447952.
	ship:partsnamed("SmallGearBay")[0]:getmodule("ModuleWheelBrakes"):setfield("brakes",25).
	ship:partsnamed("SmallGearBay")[0]:getmodule("ModuleWheelBrakes"):setfield("brakes",150).
	ship:partsnamed("SmallGearBay")[0]:getmodule("ModuleWheelBrakes"):setfield("brakes",150).
	set bank_limit to 50.
	set vs to 60.
	set vsr to 60.
	set vs0 to 60.
	set vfe to 65.
	set vlo to 65.
	set vto to 100.
	set vc to 80.
	set vtd to 60.
	set vt to 60.
} else if ship:name:startswith("stratoprop") {
	set alt_radar_adj to 1.09926223754883.
	set vs to 120.
	set vsr to 120.
	set vs0 to 120.
	set vfe to 120.
	set vlo to 120.
	set vto to 120.
	set vc to 120.
	set vtd to 120.
	set vt to 120.
} else if ship:name:startswith("Reusable Moons Rocketship") {
	set alt_radar_adj to 4.58227.
	set vs to 80.
	set vsr to 80.
	set vs0 to 80.
	set vfe to 100.
	set vlo to 100.
	set vto to 150.
	set vc to 125.
	set vtd to 80.
	set vt to 90.
} else if ship:name:startswith("SG Rescue 2") {
	//AG1  = rapier engine
	//ag2 = toggle rapier mode
	//ag3 = airbrakes.
	//ag5 = cargobay
	//ag7,9,10 do drill stuff.
	// enable nuclear engines with:  list engines in es. for e in es if e:name:contains("nuc") e:activate
	//show engine mode for rapiers: list engines in es. for e in es if e:multimode print e:mode
	//All engines on: list engines in es. for e in es e:activate
	//use execute_node_old for any multi egine craft with some engines on or off or jet engines in space etc...
	//extend antenna: for a in ship:modulesnamed("ModuleDeployableAntenna") a:DoAction("extend antenna", true)
	set alt_radar_adj to 6.
	set vs to 120.
	set vsr to 120.
	set vs0 to 120.
	set vfe to 120.
	set vlo to 120.
	set vto to 120.
	set vc to 120.
	set vtd to 120.
	set vt to 120.
}  else {
	set vs to 120.
	set vsr to 120.
	set vs0 to 120.
	set vfe to 120.
	set vlo to 120.
	set vto to 120.
	set vc to 120.
	set vtd to 120.
	set vt to 120.
set alt_radar_adj to 5.
}

// stall speed increases at high bank angles. Keep this updated so we can refer to this increased stall speed.
lock bank_stall_angle to arccos(vs^2/max(vs,airspeed)^2).

set flaps to false.
set spoilers to false.
set airbrakes to false.
set revthrust to false.

on revthrust {
	for p in ship:partstagged("jets") {
		local m is p:getmodule("ModuleAnimateGeneric").
		if revthrust and m:hasevent("reverse thr" + "ust") m:doevent("reverse thr" + "ust").
		else if m:hasevent("forward thr" + "ust") m:doevent("forward thr" + "ust").
	}
	return true.
}

on flaps {
	for p in ship:partstagged("flaps") p:getmodule("ModuleControlSurface"):setfield("deploy",flaps).
	if flaps set vs to vs0.
	else set vs to vsr.
	return true.
}

//merge airbrakes into spoilers for the moment.
on spoilers {
	for p in ship:partstagged("spoilers") p:getmodule("ModuleControlSurface"):setfield("deploy",spoilers).
	for p in ship:partstagged("airbrakes") p:getmodule("ModuleAeroSurface"):setfield("deploy",spoilers).
	return true.
}

//merge airbrakes into spoilers for the moment.
//on airbrakes {
//	for p in ship:partstagged("airbrakes")  p:getmodule("ModuleAeroSurface"):setfield("deploy",airbrakes).
//	return true.
//}


function alt_radar {
	if defined alt_radar_adj return alt:radar - alt_radar_adj.
	else return alt:radar.
}

//compass heading of our current direction of travel. (compass travel).
function ct {
	if airspeed < 0.1 {
		return body:geopositionof(ship:facing:vector:normalized):heading.
	}   else {
		return body:geopositionof(srfprograde:vector:normalized):heading.
	}
}

//compass heading of the direction we are facing. (compass facing).
function cf {
	return body:geopositionof(ship:facing:vector:normalized):heading.
}

// Use set to change these values. locking steering and THEN doing more locks on the steering variables /seems/ to cause issues.
set x to 90.5.
if alt_radar() > 10 set x to ct().
set p to 10.
set b to 0.
lock steering to heading(x,p,b).
set pitch_mode to "fixed".	// used to know when we are switching from hv() to ha() so we can do a pid:reset().
set hdg to x.
set hdg_mode to "fixed".
set pitch_info to p:tostring + " degrees".
set heading_info to x:tostring + " degrees".

set tpid to pidloop(0.5,0.01,0.1,0,1).		//used for holding speed
set vpid to pidloop(1,0.1,0.2,-45,45).		//used for holding_vertical_speeed
set apid to pidloop(0.5,0.02,0.4,-15,45).	//used for holding altitude
set rpid to pidloop(0.5,0.02,0.5,-10,60).	//used for flying NOE
set lpid to pidloop(0.2,0.02,0.05,-5,5).	//used for following a line

set using_wheelsteer to false.
set turn_dir to 0.
set turn_force_dir to 45.
set m_per_deg to (2 * constant():pi * body:radius) / 360.
set line_distance_target to 100.	// when flying a line, aim 500m further along the line....


if exists("0:/json/kerbin.json") {
	set places to readjson("0:/json/kerbin.json").
} else {
	// assume we are an airplane and on the end of the runway pointing at the far end. pretty big guesses!
	set places to lexicon().
	places:add("runway west",latlng(-0.048551152502, 285.27366496477202)).
	//places:add("runway east",body:geopositionof(ship:geoposition:position + ship:facing:forevector * 2400)).
	places:add("runway east", latlng(-0.0502131097,-74.48863)).
	places:add("vab pad 1",latlng(-0.0967891863704034, 285.382581254365)).
	places:add("vab pad 2",latlng(-0.0967891863704034, 285.3802528234891)).
}

//Work out some key spots for landing at the runway.
set runway_start to places["runway west"].
set runway_end to places["runway east"].

set runway_normalized to ( runway_end :position - runway_start:position ):normalized.
set runway_touchdown to body:geopositionof(runway_start:position + (runway_end:position - runway_start:position) * 0.075).
set runway_final to body:geopositionof(runway_normalized * -2000 + runway_start:position).
set runway_right to body:geopositionof(
		( runway_normalized * -2000) +
		(vcrs(up:vector,runway_normalized):normalized*1000) + runway_start:position
).
set runway_left to body:geopositionof(
		( runway_normalized * -2000) +
		(vcrs(runway_normalized,up:vector):normalized*1000) + runway_start:position
).


// return the angle between two compass headings (second defaults to the direction we are travelling).
// -ve means you need to subtract from the current direction of travel to get to the first headings.
// +ve means you need to turn right (i.e.add to the current direction of travel).
function ang {
	parameter a.
	parameter b is ct().
	//extra 720 added for a bit more resiliency against supplied angles like "-370 degrees" etc.
	return mod(720 + a-b + 180 + 360,360) - 180.
}.


// truncate num so that it is between min and max (inclusive). optionally, do a call back to say you did something.
function constrain {
	parameter num.
	parameter min is -1.
	parameter max is 1.
	parameter action is "".
	if num < min {
		if not (action = "") action(num,min).
		return(min).
	} else if num > max {
		if not (action = "") action(num,max).
		return(max).
	} else  {
		return num.
	}
}

set steeringmanager:pitchtorquefactor to 0.2.
set steeringmanager:yawtorquefactor to 0.5.
set steeringmanager:rollcontrolanglerange to 180.
steeringmanager:resetpids().

// default_bank_limit is how steep to turn if the pilot does not specify...
// bank_limit is how steep to turn (pilot can change this value but it defaults to default_bank_limit).
set bank_limit to default_bank_limit.

set bank_fudge_factor to 3.		// we try to bank this many degrees for every degree of heading we want to change.
								// other constraints will stop us banking that exact ammount, this will have more
								// effect at very small changes in heading angle. heading angle change of 1 deg = 3deg bank.

set current_rudder to 0.

//safetey features.
set dynamic_brakes_enable to false.		// when ground handling, apply/release brakes to help maintain target speeds <= 15.
set electric_throttle_enable to false.	// kick in the throttle if ship:electric charge gets too low.
set wing_leveler_enable to true.		// when lower than 40m off the ground, level the wings.
										// this gives better climb rates, and might mean avoiding touch a wingtip on the ground.
set auto_spoilers_enable to true.		// TODO need an inidcator like DBE

// 1 minute timer
on floor(time:seconds / 60) {
	if hud_message <> "" {
			hudtext(hud_message, 15, 2, 25, purple, false).
	}
	return true.
}

// terrain following radar - lookahead to see what the terrain is like - and peek out to the sides a bit too.
//Try to predict where we are heading by looking at how fast our direction of travel is changing.
set tft_prev_hdg to ct().
set tft_prev_time to time:seconds.

// hold a 2d array of vectors to draw.
set tfr_vecdraws to list().
set tfr_lookahead to 3.			// how many seconds ahead should the tfr radar look.

function tfr {
	parameter lookahead is 3.
	local max_ht is ship:geoposition:terrainheight.
	local alpha is ang(tft_prev_hdg,ct()) * 50 * groundspeed *(time:seconds - tft_prev_time ).
	set tft_prev_time to time:seconds.
	set tft_prev_hdg to ct().
	from { local dt is 1. } until dt > lookahead step { set dt  to dt + 1. } do {
		from { local base_rot is -5. } until base_rot > 5 step { set base_rot to base_rot +5. } do {
			local actual_rot is base_rot - alpha * dt.
			//local th is body:geopositionof(vxcl(ship:up,velocity:surface * dt * r(actual_rot,0,0))):terrainheight.
			local th is body:geopositionof(vxcl(up:vector,velocity:surface) * dt * r(actual_rot,0,0)):terrainheight.
			//local th is body:geopositionof(positionat(ship,time:seconds + dt )*r(actual_rot,0,0)):terrainheight.
			if dt -1 >= tfr_vecdraws:length {
print "add row = " + (dt -1).
				tfr_vecdraws:add(list()).
			}
			if base_rot/5 + 1 >= tfr_vecdraws[dt -1]:length {
print "add index = " + (dt -1) + "," + (base_rot/5 + 1).
				tfr_vecdraws[dt-1]:add( vecdraw(v(0,0,0),v(0,0,0),yellow,"",1,true)).
			}
			if th > max_ht {
				set max_ht to th.
				set tfr_vecdraws[dt -1][base_rot/5 +1]:color to red.
			} else   {
				set tfr_vecdraws[dt -1][base_rot/5 +1]:color to yellow.
			}
			//set tfr_vecdraws[dt -1][base_rot /5 + 1]:vec to vxcl(ship:up,velocity:surface * dt * r(0,0,actual_rot)).
			set tfr_vecdraws[dt -1][base_rot /5 + 1]:vec to vxcl(up:vector,velocity:surface) * dt * r(0,0,actual_rot).
			tfr_vecdraws[dt -1][base_rot/5 +1]:show.
		}
	}
	return max_ht .
}.

set the_gui to gui(300).
set gui_fields to lexicon().
{
	// hide some local veriables that can be thrown away.
	gui_fields:add("gui",the_gui).
	local vl is the_gui:addvlayout().
	local hl1 is vl:addhlayout().
	hl1:addlabel("Throttle:").
	gui_fields:add("speed",hl1:addtextfield(tpid:setpoint:tostring)).
	hl1:addlabel("m/s").
	gui_fields:add("stall",hl1:addlabel("<color=black>Stall</color>")).
	local hl2 is vl:addhlayout().
	hl2:addlabel("Pitch:").
	gui_fields:add("pitch_mode",hl2:addtextfield(pitch_mode)).
	set gui_fields["pitch_mode"]:style:fontsize to 10.
	gui_fields:add("pitch_info",hl2:addtextfield(p:tostring)).
	set gui_fields["pitch_info"]:STYLE:HSTRETCH TO True.
	set gui_fields["pitch_info"]:STYLE:FONTSIZE TO 10.
	local hl3 is vl:addhlayout().
	hl3:addlabel("Heading:").
	gui_fields:add("heading_mode",hl3:addtextfield(hdg_mode)).
	set gui_fields["heading_mode"]:style:fontsize to 10.
	gui_fields:add("heading_info",hl3:addtextfield(hdg:tostring)).
	set gui_fields["heading_info"]:STYLE:HSTRETCH TO True.
	set gui_fields["heading_info"]:STYLE:FONTSIZE TO 10.
	local hl4 is vl:addhlayout().
	hl4:addlabel("Bank Ang:").
	gui_fields:add("bank",hl4:addtextfield(b:tostring)).
	local lab is hl4:addlabel("Limit:").
	set lab:style:align to "RIGHT".
	gui_fields:add("bank_limit",hl4:addtextfield(bank_limit:tostring)).
	local hl5 is vl:addhlayout().
	gui_fields:add("brakes", hl5:addlabel((choose "<color=green>Brakes</color>" if brakes else "<color=black>Brakes</color>"))).
	gui_fields:add("gear", hl5:addlabel((choose "<color=green>Gear</color>" if gear else "<color=black>Gear</color>"))).
	gui_fields:add("flaps", hl5:addlabel((choose "<color=green>Flaps</color>" if flaps else "<color=black>Flaps</color>"))).
	gui_fields:add("spoilers", hl5:addlabel( (choose "<color=green>Spoilers</color>" if spoilers else "<color=black>Spoilers</color>"))).
	local hl6 is vl:addhlayout().
	gui_fields:add("revthrust",hl6:addlabel( (choose "<color=green>Reverse</color>" if revthrust else "<color=black>Reverse</color>"))).
	gui_fields:add("electric_throttle",hl6:addlabel( (choose "<color=green>ETE</color>" if electric_throttle_enable else "<color=black>ETE</color>"))).
	gui_fields:add("dynamic_brakes",hl6:addlabel( (choose "<color=green>DBE</color>" if dynamic_brakes_enable else "<color=black>DBE</color>"))).
	gui_fields:add("wing_leveler",hl6:addlabel( (choose "<color=green>WLE</color>" if wing_leveler_enable else "<color=black>WLE</color>"))).
	set the_gui:x to -100.
	set the_gui:y to 100.
	the_gui:show().
}

set main_loop_timestamp to 0.

// 1 second timer
on time:second {
	//check on the current goal - change if need be. this is a big TODO, and encompasses the flight plan ideas I've had.

	//display useful information - currently just using the console for debugging, but a GUI might be better.
	//print (("Heading: " + round(ct(),1)):padright(15) + "target: " + round(hdg,1)):padright(terminal:width) at (0,0).
	//print (
	//	(
	//		"Pitch: " + round(p,1)
	//	):padright(15) + "Tgt: " + pitch_mode +"("+
	//	(
	//		choose p if pitch_mode = "fixed"
	//			else choose vpid:setpoint if pitch_mode = "vert"
	//			else choose apid:setpoint if pitch_mode = "alt"
	//			else choose rpid:setpoint if pitch_mode = "radar" else "Unknown"
	//	)
	//	+ ")"
	//):padright(terminal:width) at (0,1).
	//print ("Bank: " + round(b,1)):padright(terminal:width) at (0,2).
	//print (("Throttle: " + round(throttle,1)):padright(15) + "TgtSpd:" + round(tpid:setpoint) +"m/s"):padright(terminal:width) at (0,3).
	//print ("Bank Stall angle: " + round(bank_stall_angle,1) + "deg"):padright(terminal:width) at(0,4).
	//print (("VertSpd: " + round(verticalspeed,1) + "m/s"):padright(15) + "Current GR: " + (choose "---" if verticalspeed < 0.001 else round(-groundspeed/verticalspeed,1) + ":1")):padright(terminal:width) at (0,5).

	//local str is "".
	//if electric_throttle_enable {
	//	if not (str = "") set str to str + ", ".
	//	set str to str + "Electric Throttle".
	//}
	//if dynamic_brakes_enable {
	//	if not (str = "") set str to str + ", ".
	//	set str to str + "Dynamic Brakes".
	//}
	//if wing_leveler_enable {
	//	if not (str = "") set str to str + ", ".
	//	set str to str + "Wing Leveler".
	//}
	//print str:padright(terminal:width) at (0,6).
	//print " ":padright(terminal:width) at (0,7).


	// finally some flight related code that needs to go in here.
	//Nice in theory - but flaps tend to come on at inopportune times, and that then messes with stall speeds too!
	//if airspeed < vfe {
	//	if alt_radar() < 120 if not flaps flaps on.
	//} else {
	//	if alt_radar() > 120 if flaps flaps off.
	//}

	//Gear at least does not mess with stallspeeds in KSP (I think!? If so it does not have much effect).
	if alt_radar() < 80 and verticalspeed < 0 if not gear gear on.

	if gui_fields["gui"]:visible {
		local blink is false.
		if mod(time:second,2) = 0 set blink to true.
		set gui_fields["speed"]:text to tpid:setpoint:tostring.
		if ship:status <> "LANDED" and (airspeed <= vs or b >= bank_stall_angle) set gui_fields["stall"]:text to "<color=" + (choose "red" if blink else "yellow") + ">Stall</color>".
		else set gui_fields["stall"]:text to "<color=black>Stall</color>".
		set gui_fields["pitch_mode"]:text to pitch_mode.
		set gui_fields["pitch_info"]:text to pitch_info.
		set gui_fields["heading_mode"]:text to hdg_mode.
		set gui_fields["heading_info"]:text to heading_info.
		set gui_fields["bank"]:text to round(abs(90 - vang(ship:facing:starvector,up:vector)),1):tostring + " deg".
		set gui_fields["bank_limit"]:text to bank_limit:tostring + " deg".
		if brakes set gui_fields["brakes"]:text to "<color=green>Brakes</color>".
		else set gui_fields["brakes"]:text to "<color=black>Brakes</color>".
		if gear {
			if defined vlo and  airspeed > vlo set gui_fields["gear"]:text to "<color=" + (choose "red" if blink else "yellow") + ">Gear</color>".
			else set gui_fields["gear"]:text to "<color=green>Gear</color>".
		} else {
			set gui_fields["gear"]:text to "<color=black>Gear</color>".
		}
		if flaps {
			if defined vfe and airspeed > vfe set gui_fields["flaps"]:text to "<color=" + (choose "red" if blink else "yellow") + ">Flaps</color>".
			else set gui_fields["flaps"]:text to "<color=green>Flaps</color>".
		} else {
			set gui_fields["flaps"]:text to "<color=black>Flaps</color>".
		}
		if spoilers set gui_fields["spoilers"]:text to "<color=green>Spoilers</color>".
		else set gui_fields["spoilers"]:text to "<color=black>Spoilers</color>".
		if revthrust set gui_fields["revthrust"]:text to "<color=green>Reverse</color>".
		else set gui_fields["revthrust"]:text to "<color=black>Reverse</color>".

		if electric_throttle_enable set gui_fields["electric_throttle"]:text to "<color=green>ETE</color>".
		else set gui_fields["electric_throttle"]:text to "<color=black>ETE</color>".
		if dynamic_brakes_enable set gui_fields["dynamic_brakes"]:text to "<color=green>DBE</color>".
		else set gui_fields["dynamic_brakes"]:text to "<color=black>DBE</color>".
		if wing_leveler_enable set gui_fields["wing_leveler"]:text to "<color=green>WLE</color>".
		else set gui_fields["wing_leveler"]:text to "<color=black>WLE</color>".
	}

	if main_loop_timestamp > 0 and main_loop_timestamp +5 < time:seconds {
		print "MAIN LOOP IS DEAD".
	}.
	return true.
}

//set vd_line to vecdraw(v(0,0,0),v(0,0,0),yellow,"",1,true,10).
//set vd_runway to vecdraw(v(0,0,0),v(0,0,0),red,"",1,true,100).

set runway_vec to places["runway east"]:position - places["runway west"]:position.

// Fast timer
when true then {
	set main_loop_timestamp to time:seconds.
//set vd_runway:start to places["runway west"]:position + up:vector * 10.
//set vd_runway:vec to places["runway west"]:position + up:vector*10 + runway_vec.
	if pitch_mode = "vert" {
		// pilot has asked to try to maintain climb/sink rate
		set p to vpid:update(time:seconds,verticalspeed).
	} else if pitch_mode = "alt" {
		// pilot has asked to try to maintain altitude
		set p to apid:update(time:seconds,altitude).
	} else if pitch_mode = "radar" {
		// pilot has ask to to fly NOE
		set p to rpid:update(time:seconds,altitude - tfr(tfr_lookahead)).
	} else if pitch_mode = "slope" {
		local actual_dist is (slope_target:position - ship:geoposition:position):mag + slope_distance_adj.
		local actual_vert is altitude - slope_target:terrainheight - slope_height_adj.
		local actual_slope is arctan(actual_vert / actual_dist).
		local new_dist is actual_dist / 3.
		local new_vert is altitude - slope_target:terrainheight - slope_height_adj - ( actual_dist * tan(slope_angle)*2/3).
		set vpid:setpoint to ((-new_vert)/ (new_dist / airspeed)).
		set p to vpid:update(time:seconds,verticalspeed).
//print "SL TGT: " + round(slope_angle,1) + "  ACT: " + round(actual_slope,1) + " NEW: " + round(arctan(new_vert/new_dist),1) + " VS TG=" + round(vpid:setpoint,1).
		if auto_spoilers_enable {
			if tpid:setpoint * 1.1 < airspeed and actual_slope > slope_angle spoilers on.
			else spoilers off.
		}
	} // else pitch_mode = "fixed" => just leave the pitch alone.
	if hdg_mode = "circle" {
		if circle_target:altitudeposition(altitude):mag > circle_radius {
			local alpha is arcsin( circle_radius / circle_target:altitudeposition(altitude):mag ).
			set hdg to circle_target:heading + (choose alpha if circle_dir = -1 else -alpha).
		} else {
			set hdg to circle_target:heading + 92 * -circle_dir.
		}
	} else if hdg_mode = "target" {
		if hdg_target:altitudeposition(altitude):mag < (3 * groundspeed) {
			// if we get "close", just keep flying that heading TODO: this should be more rigorous shouldn't it?
			set hdg_mode to "heading".
			set heading_info to round(hdg,1):tostring + " degrees".
		}     else {
			set hdg to hdg_target:heading.
		}
	} else if hdg_mode = "line" {
		// We are going to make a few of big assumptions and approximations.
		// LAT/LNG give us a catersian plane. This is an issue near the poles. And its not really a plane - it's a sphere.
		// As you move away from the equator degrees of longitude get smaller by a factor of: sin(90 - abs(latitude)).
		// We will use the latitude of the ship for that scaling factor, and hope that distances are small and
		// spherical nature of the planet will not be very apparent at the scales we are working with.

		// We will leave this code commented out - and instead try to stick with using LAT/LNG coordinates.
		// first convert lat/lng to x,y in meters. take account the curvature of the planet.
		//  local x_ship is 0. local y_ship is 0.
		// local x_near is (line_tgt_from:lng - ship:geoposition:lng) * m_per_deg * sin(90 - abs(ship:geoposition:lat)).
		// local y_near is (line_tgt_from:lat - ship:geoposition:lat) * m_per_deg.
		// local x_far is (line_tgt_to:lng - ship:geoposition:lng) * m_per_deg * sin(90 - abs(ship:geoposition:lat)).
		// local y_far is (line_tgt_to:lat - ship:geoposition:lat) *m_per_deg.

		if (line_tgt_to:lng - line_tgt_from:lng) = 0 {
			// line has formula x = line_tgt_from:lng   i.e. line runs directly north/south through line_tgt_from:lng.
			if line_tgt_to:lat > line_tgt_from:lat {
				//we need to heard north
				// fly towards a point on the line, (line_distance_target) meters north.
				local spot is body:geopositionlatlng(ship:geoposition:lat + line_distance_target/m_per_deg, line_tgt_from:lng).
				set hdg to spot:heading.
//set vd_line:start to ship:geoposition:position + spot:position.
//set vd_line:vec to ship:geoposition:position + spot:position + up:vector * 50.
			} else {
				// we need to head south.
				local spot is body:geopositionlatlng(ship:geoposition:lat - line_distance_target/m_per_deg, line_tgt_from:lng).
				set hdg to spot:heading.
//set vd_line:start to ship:geoposition:position + spot:position.
//set vd_line:vec to ship:geoposition:position + spot:position + up:vector * 50.
			}
		} else {
			local m to (line_tgt_to:lat - line_tgt_from:lat) / (sin(90 - abs(line_tgt_to:lat))*line_tgt_to:lng - sin(90 - abs(line_tgt_from:lat))*line_tgt_from:lng).
			local c is line_tgt_from:lat - m * sin(90 - abs(line_tgt_from:lat)) * line_tgt_from:lng.
			// line has formula lat = m * sin(90-abs(lat)*lng + c.

			local new_x is 0.
			local a is c * cos(arctan(m))*cos(90-arctan(m)).
			local b is cos(arctan(m)) + (line_distance_target/m_per_deg).
			if line_tgt_to:lng > line_tgt_from:lng {
				set new_x to (ship:geoposition:lng -a + b).
			} else {
				set new_x to (ship:geoposition:lng -a -b).
			}
			local spot is body:geopositionlatlng(m * new_x + c, new_x / sin(90-abs(m * new_x + c))).
			set hdg to spot:heading.
//set vd_line:start to ship:geoposition:position + spot:position.
//set vd_line:vec to ship:geoposition:position + spot:position + up:vector * 50.
		}
	}
	if hdg_mode = "fixed" or hdg_mode = "target" or hdg_mode = "circle" or hdg_mode = "line" {
		if ship:status = "PRELAUNCH" or ship:status = "LANDED" {
			set b to 0.
			set x to hdg.
			//still on the ground - try to steer, but as speed increases do a LOT less wheelsteering.
			if abs(ang(hdg,cf())) < 1 {
				set ship:control:wheelsteer to 0.
				set using_wheelsteer to false.
			} else     {
				set using_wheelsteer to true.
				set ship:control:wheelsteer to constrain(
					constrain(
						-ang(hdg,cf())/10,
						-100/(groundspeed+1)^2,
						100/(groundspeed+1)^2
					),
					-1,
					1
				).
			}.
			// we might be using throttle to maintain ship:electriccharge... so using brakes on the ground could be really important.
			if dynamic_brakes_enable {
				if tpid:setpoint < 0.1 {
					brakes on.
				} else {
					if groundspeed > tpid:setpoint * 1.1 brakes on.
					if groundspeed < tpid:setpoint  brakes off.
				}
			}
		}     else     {
			//in the air
			//if brakes brakes off.
			if using_wheelsteer {
				set ship:control:wheelsteer to 0.
				set using_wheelsteer to false.
			}.
			local bank_dir is 0.
			if hdg_mode = "circle" {
				set bank_dir to circle_dir.
				set turn_force_dir to 90.
			}
			if hdg_mode = "fixed" set bank_dir to turn_dir.

			// force a turn in a particular direction if the angle is over turn_force_dir degrees.
			// otherwise turn which ever way is smaller.
			if abs(ang(hdg,ct())) < turn_force_dir set bank_dir to 0.
			local hdg_error is ang(hdg,ct()).
			// force us to turn the other way by faking how much we need to turn :-/
			if bank_dir = -1 and hdg_error > 0 {
				set hdg_error to hdg_error - 180.
			} else if bank_dir = 1 and hdg_error < 0 {
				set hdg_error to hdg_error + 180.
			}

			// don't bank so much you stall,
			// don't exceed normal bank limits,
			// don't change bank too rapidly,
			set b to constrain(
				-hdg_error * bank_fudge_factor,
				max(-bank_stall_angle,max(-bank_limit,b - bank_change_rate)),
				min(bank_stall_angle,min(bank_limit,b + bank_change_rate))
			).
			set target_rudder to rudder_rate * -b.
			if wing_leveler_enable {
				// don't bank steeply close to the ground.
				set b to constrain(b,-alt_radar(), alt_radar()).
			}
			set current_rudder to constrain(target_rudder,current_rudder -1, current_rudder + 1).
			set x to mod(720 + ct() + current_rudder,360).
		}
	}
	return true.
}


set valid_dirs to list("left","right","either").
// try to hold a circle - if not landmark to reference is given, circle the current location.
//if "either" direction is given (or no direction given) then a direction has to be determined here.
function circle {
	parameter tgt is ship:geoposition.
	parameter radius is 1000.
	parameter dir is "either".
	if not (tgt:hassuffix("distance") and tgt:hassuffix("heading")) and not places:haskey(tgt) {
		print char(7) + "ERROR: cannot circle - unrecognised target".
		return.
	}
	if valid_dirs:find(dir) = -1 {
		print char(7) + "ERROR: cannot circle - unrecognised direction".
		return.
	}
	if places:haskey(tgt) {
		set tgt to places[tgt].
		set heading_info to tgt + " radius=" + round(circle_radius) + "m ".
	} else {
	set heading_info to "LAT:" + round(tgt:lat,3) + " LNG:" + round(tgt:lng,3).
}
	set hdg_mode to "circle".
	set circle_target to tgt.
	set circle_radius to radius.

	if dir = "either" {
		if circle_target:altitudeposition(altitude):mag > circle_radius {
			set circle_dir to (choose 1 if ang(circle_target:heading,ct()) > 0 else -1).
		} else {
			set circle_dir to (choose 1 if abs(ang(circle_target:heading -90 ,ct())) < 90 else -1).
		}
	} else {
		set circle_dir to (choose -1 if dir = "left" else 1 ).
	}
	set heading_info to heading_info + (choose "left" if circle_dir  < 0 else "right").
}.


// provide target heading, a direction to force a turn in, and a limit of how far off a heading must be before forced dir will be used.
// i.e. if the current heading is 45, the new target heading is 50 and the force_dir is left, and force_angle is 10 we will turn right.
// be careful setting small force angles, any perturbation of the heading could result in complete 360degree turns.
set force_dirs to list("closest","left","right").
function tt {
	parameter tgt.
	parameter force_dir is "closest".
	parameter force_angle is 45.
	if force_dirs:find(force_dir) = -1 {
		print char(7) + "ERROR: " + force_dir + " is not a recognised direction".
	}       else        {
		set turn_force_dir to force_angle.
		if force_dir = "closest" or abs(ang(tgt,ct())) < force_angle {
			set hdg_mode to "fixed".
			set hdg to tgt.
			set turn_dir to 0.
		} else if force_dir = "left" {
			set hdg_mode to "fixed".
			set hdg to tgt.
			set turn_dir to -1.
		} else if force_dir = "right" {
			set hdg_mode to "fixed".
			set hdg to tgt.
			set turn_dir to 1.
		}
		set heading_info to hdg:tostring + " deg".
	}
	return true.
}.

//pilot is requesting to fly to a particular location.
function goto {
	parameter where.
	if where:hassuffix("heading") and where:hassuffix("distance") {
		set hdg_mode to "target".
		set hdg_target to where.
		set heading_info to "LAT:" + round(where:lat,3) + " LNG:" + round(where:lng,3).
	} else if places:haskey(where) {
		set hdg_mode to "target".
		set hdg_target to places[where].
		set heading_info to where.
	} else {
		print "Sorry, I don't know where " + where + " is.".
	}
	return true.
}.


// pilot is requesting to fly NOE at a certain height off the ground.
function hr {
	parameter tgt.
	if pitch_mode <> "radar" {
		rpid:reset().
		set pitch_mode to "radar".
	}
	set rpid:setpoint to tgt.
	set pitch_info to "NOE " + round(tgt,1) + " m".
	return true.
}.

//pilot is requesting to hold a particular speed - if we are using throttle to maintain electric charge,
//then when electriccharge is low, kick in enough throttle to maintain some charge!
//in the air electric_throttle_enable might not be noticed, but on the ground a small change in throttle could
//easily cause problems... so might be a good idea to also set dynamic_brakes_enable to true to prevent overspeeds.
function hs {
	parameter tgt is 0.
	if tgt = "" {
		unlock throttle.
	} else {
		set tpid:setpoint to tgt.
		lock throttle to max(tpid:update(time:seconds,airspeed),choose 100 - ship:electriccharge if electric_throttle_enable else 0).
	}
	return true.
}.

//pilot has requested to hold a particular pitch
function hp {
	parameter tgt.
	set pitch_mode to "fixed".
	set p to tgt.
	set pitch_info to round(tgt,1) + " deg".
	return true.
}

//pilot is requesting to hold a particular climb/sink rate
function hv {
	parameter tgt.
	if pitch_mode <> "vert" and pitch_mode <> "slope" {
		vpid:reset().
	}
	set pitch_mode to "vert".
	set vpid:setpoint to tgt.
	if tgt < 0 {
		set pitch_info to "Descend " + round(tgt,1) + " m/s".
	} else if tgt > 0 {
		set pitch_info to "Climb " + round(tgt,1) + " m/s".
	} else {
		set pitch_info to "Level Flight".
	}.
	return true.
}.

//hold glideslope.
function hg {
	parameter slope.
	parameter tgt.
	parameter ht is 0.
	parameter dist is 0.

	set slope_angle to slope.
	set slope_target to tgt.
	set slope_height_adj to ht.
	set slope_distance_adj to dist.
	if pitch_mode <> "vert" and pitch_mode <> "slope" {
		vpid:reset().
	}
	//main flight loop will calculate the vertspd to fly and set p accordingly.
	set pitch_mode to "slope".
	set pitch_info to round(slope,1) + " deg slope".
	return true.
}

//hold line - note this says NOTHING about how close to those points we are - just that we want to be on that line.
function hl {
	parameter tgt_from.
	parameter tgt_to.
	if hdg_mode <> "line" lpid:reset().
	set hdg_mode to "line".
	set line_tgt_from to tgt_from.
	set line_tgt_to to tgt_to.
	set heading_info to "runway 90".
}

//pilot is requesting to hold a particular altitude.
function ha {
	parameter tgt.
	if pitch_mode <> "alt" {
		apid:reset().
		set pitch_mode to "alt".
	}
	set apid:setpoint to tgt.
	set pitch_info to "hold " + round(tgt,1) + " m".
}.


if ship:status <> "flying" {

	wait 2.

set runway_start to places["vab pad 1"].
set runway_end to places["vab pad 2"].
set runway_normalized to ( runway_end:position - runway_start:position ):normalized.
set runway_touchdown to body:geopositionof(runway_start:position - runway_normalized * 5).
set runway_final to body:geopositionof(runway_normalized * -2000 + runway_start:position).
set runway_right to body:geopositionof(
		( runway_normalized * -2000) +
		(vcrs(up:vector,runway_normalized):normalized*800) + runway_start:position
).
set runway_left to body:geopositionof(
		( runway_normalized * -2000) +
		(vcrs(runway_normalized,up:vector):normalized*800) + runway_start:position
).


	set runway_alt to altitude.
	stage.
	brakes off.
	lights on.
	flaps on.
	hs(vto).
	hp(10).
	when alt:radar > 50 then gear off.
	when altitude > (runway_alt + 100) then {
		flaps off.
		hs(vc).
		tt(0).
		when altitude > (runway_alt + 310) then {
			ha(runway_alt + 330).
			circle(runway_left,970,"left").
			when abs(runway_final:altitudeposition(altitude):mag) < 500 and abs(ang(runway_touchdown:heading,ct())) < 10 then {
				hl(runway_start,runway_end).
				flaps on.
				hs(vtd).
				hg(5,runway_touchdown,0,0).
				when airspeed < vlo or alt:radar < 50 then gear on.
				when runway_touchdown:altitudeposition(altitude):mag  < 80 then {
					set auto_spoilers_enable to false.
					hv(-1).
					revthrust on.
					lock throttle to 1.
					brakes on.
					flaps off.
					spoilers on.
					when airspeed < 5 then lock throttle to 0.
				}.
			}
		}
	}
}

if not (bodyexists("Lua")) {
	wait until rcs.
	clearguis().
	clearvecdraws().
}