# S-TEC Thirty Autopilot System
# based on S-TEC 55X by Joshua Davidson (Octal450)

# Reference: https://genesys-aerosystems.com/sites/default/files/files/System%20Twenty_Thirty_%20ThirtyALT.pdf
# Roll Modes:
#	RDY: standby							0
#	ST (Stabilizer): Wings Level					1
#	HD (Heading):	 Holds heading					2
#	LO TRK (Low Track): Tracks nav with low gain (for VOR)		3
#	HI TRK (High Track): Tracks nav with high gain (for LOC)	4
#	Cycle through roll modes by pushing the mode knob
# Pitch Mode:
#	RDY: standby							0
#	ALT (Altitude): Altitude hold					1
# Power up: performs a selftest (see 2-7) then goes into the RDY mode

# Initialize variables
var cdiDefl = 0;
var aoffset = 0;
var vspeed = 0;
var NAV = 0;
var REV = 0;
var CNAV = 0;
var CREV = 0;
var VSError = 0;
var isTrimming = 0;
var NAVFlag = 0;
var GSFlag = 0;
var ALTOffsetDeltaMax = 0;
var NAVGainStd = 1.0;
var NAVGainCap = 0.9;
var NAVGainCapSoft = 0.8;
var NAVGainSoft = 0.6;
var GSNeedleInCapt = 0;
var NAVStep1Time = 0;
var NAVStep2Time = 0;
var NAVStep3Time = 0;
var NAVOver50Time = 0;
var NAVOver50Counting = 0;
var hdgButtonTime = 0;
var powerUpTime = 0;
var powerUpTestAnnun = 0;
var powerUpTestVSAnnun = 0;
var showSoftwareRevision = 0;
var rollMode = 0;
var pitchMode = 0;

# Initialize custom nav inputs
props.globals.initNode("/stec30/custom-nav/in-range-1", 0, "BOOL");
props.globals.initNode("/stec30/custom-nav/in-range-2", 0, "BOOL");
props.globals.initNode("/stec30/custom-nav/nav-needle-1", 0, "DOUBLE");
props.globals.initNode("/stec30/custom-nav/nav-needle-2", 0, "DOUBLE");
props.globals.initNode("/stec30/custom-nav/nav-course-1", 0, "INT");
props.globals.initNode("/stec30/custom-nav/nav-course-2", 0, "INT");

# Initialize all used property nodes
var elapsedSec = props.globals.getNode("/sim/time/elapsed-sec");
var powerSrc = props.globals.getNode("/systems/electrical/outputs/autopilot", 1); # Autopilot power source
var serviceable = props.globals.initNode("/stec30/serviceable", 1, "BOOL");
var systemAlive = props.globals.initNode("/stec30/internal/system-alive", 0, "BOOL");
var hdg = props.globals.initNode("/stec30/input/hdg", 360, "DOUBLE");
var hdgButton = props.globals.initNode("/stec30/input/hdg-button", 0, "BOOL");
var alt = props.globals.initNode("/stec30/input/alt", 0, "DOUBLE"); # Altitude is in static pressure, not feet
var altOffset = props.globals.initNode("/stec30/input/alt-offset", 0, "DOUBLE"); # Altitude offset
var masterAPSW = props.globals.initNode("/stec30/input/ap-master-sw", 0, "BOOL");
var trimSW = props.globals.initNode("/stec30/input/trim-sw", 0, "BOOL");	# shows up/dn lamps for trim when on
var discSW = props.globals.initNode("/stec30/input/disc", 0, "BOOL");
var hasPower = props.globals.initNode("/stec30/internal/hasPower", 0, "BOOL");
var roll = props.globals.initNode("/stec30/output/roll", -1, "INT");
var pitch = props.globals.initNode("/stec30/output/pitch", -1, "INT");
var ST_annun = props.globals.initNode("/stec30/annun/st", 0, "BOOL");
var HD_annun = props.globals.initNode("/stec30/annun/hd", 0, "BOOL");
var LO_annun = props.globals.initNode("/stec30/annun/lo", 0, "BOOL");
var HI_annun = props.globals.initNode("/stec30/annun/hi", 0, "BOOL");
var RDY_annun = props.globals.initNode("/stec30/annun/rdy", 0, "BOOL");
var ALT_annun = props.globals.initNode("/stec30/annun/alt", 0, "BOOL");
var UP_annun = props.globals.initNode("/stec30/annun/up", 0, "BOOL");
var DN_annun = props.globals.initNode("/stec30/annun/dn", 0, "BOOL");
var minTurnRate = props.globals.initNode("/stec30/internal/min-turn-rate", -0.9, "DOUBLE");
var maxTurnRate = props.globals.initNode("/stec30/internal/max-turn-rate", 0.9, "DOUBLE");
var manTurnRate = props.globals.initNode("/stec30/internal/man-turn-rate", 0, "DOUBLE");
var NAVGain = props.globals.initNode("/stec30/internal/nav-gain", NAVGainStd, "DOUBLE");
var powerUpTest = props.globals.initNode("/stec30/internal/powerup-test", -1, "INT"); # -1 = Powerup test not done, 0 = Powerup test complete, 1 = Powerup test in progress
var ALTOffsetDelta = props.globals.getNode("/stec30/internal/static-20ft-delta");
var masterSW = props.globals.initNode("/stec30/internal/master-sw", 0, "BOOL");
var servoRollPower = props.globals.initNode("/stec30/internal/servo-roll-power", 0, "BOOL");
var servoPitchPower = props.globals.initNode("/stec30/internal/servo-pitch-power", 0, "BOOL");
var pressureRate = props.globals.getNode("/stec30/internal/pressure-rate", 1);
var NAVIntercept = props.globals.getNode("/stec30/internal/intercept-angle", 1);
var discSound = props.globals.initNode("/stec30/sound/disc", 0, "BOOL");
var HDGIndicator = props.globals.getNode("/instrumentation/heading-indicator/indicated-heading-deg");
var NAVCourse = props.globals.getNode("/stec30/nav/nav-course", 1);
var OBSNAVNeedle = props.globals.getNode("/stec30/nav/nav-needle", 1);
var OBSGSNeedle = props.globals.getNode("/stec30/nav/gs-needle", 1);
var OBSActive = props.globals.initNode("/stec30/nav/in-range", 0, "BOOL");
var NAV0Power = props.globals.getNode("/systems/electrical/outputs/nav[0]");
var turnRate = props.globals.getNode("/instrumentation/turn-indicator/indicated-turn-rate");
var turnRateSpin = props.globals.getNode("/instrumentation/turn-indicator/spin");
var staticPress = props.globals.getNode("/systems/static[0]/pressure-inhg");
var gain_factor = 0.8;

setlistener("/sim/signals/fdm-initialized", func {
	ITAF.init();
});

var ITAF = {
	init: func() {
		hdg.setValue(360);
		alt.setValue(0);
		altOffset.setValue(0);
		discSW.setBoolValue(0);
		masterAPSW.setBoolValue(0);
		powerUpTest.setValue(-1);
		roll.setValue(0);
		pitch.setValue(0);
		ST_annun.setBoolValue(0);
		HD_annun.setBoolValue(0);
		LO_annun.setBoolValue(0);
		HI_annun.setBoolValue(0);
		RDY_annun.setBoolValue(0);
		ALT_annun.setBoolValue(0);
		UP_annun.setBoolValue(0);
		DN_annun.setBoolValue(0);
		discSound.setBoolValue(0);
		update.start();
		updateFast.start();
	},
	loop: func() {
		
		masterSW.setValue(masterAPSW.getValue());
		
		rollMode = roll.getValue();
		pitchMode = pitch.getValue();
				
		if (hasPower.getBoolValue() and turnRateSpin.getValue() >= 0.2) { # Requires turn indicator spin over 20%
			systemAlive.setBoolValue(1);
		} else {
			systemAlive.setBoolValue(0);
			if (rollMode != 0 or pitchMode != 0) {
				me.killAP();
			}
		}
		
		if (powerSrc.getValue() >= 8 and masterSW.getValue() > 0) {
			hasPower.setBoolValue(1);
			if (powerUpTest.getValue() == -1 and systemAlive.getBoolValue()) { # Begin power on test
				powerUpTest.setValue(1);
				powerUpTime = elapsedSec.getValue();
			}
		} else {
			hasPower.setBoolValue(0);
			if (powerUpTest.getValue() != -1 or systemAlive.getBoolValue() != 1) {
				powerUpTest.setValue(-1);
			}
			if (rollMode != 0 or pitchMode != 0) {
				me.killAP();
			}
		}
		
		# Powerup Test Annunciators
		if (powerUpTest.getValue() == 1) {
			if (powerUpTime + 3 >= elapsedSec.getValue()) {
				powerUpTestAnnun = 1;
				showSoftwareRevision = 0;
			} else if (powerUpTime + 8 >= elapsedSec.getValue()) {
				powerUpTestAnnun = 0;
				showSoftwareRevision = 1;
			} else {
				powerUpTestAnnun = 0;
				showSoftwareRevision = 0;
			}
		} else {
			powerUpTestAnnun = 0;
			showSoftwareRevision = 0;
		}
		
		NAV = rollMode == 3 or rollMode == 4; # Is NAV armed?
		
		if (!systemAlive.getBoolValue()) { # AP Failed when false
			RDY_annun.setBoolValue(0);
		} else {
			if (powerUpTest.getValue() == 1 and powerUpTime + 10 < elapsedSec.getValue()) {
				powerUpTest.setValue(0);
			}
			if (powerUpTestAnnun == 1) {
				RDY_annun.setBoolValue(1);
			} else if (rollMode == 0 and serviceable.getBoolValue() and powerUpTest.getValue() == 0) {
				RDY_annun.setBoolValue(1);
			} else {
				RDY_annun.setBoolValue(0);
			}
			if (serviceable.getBoolValue() != 1) {
				powerUpTest.setValue(0);
				if (rollMode != 0 or pitchMode != -1) {
					me.killAP();
				}
			}
		}
		
		# Mode Annunciators
		# AP does not power up or show any signs of life unless if has power (obviously)
		if ((rollMode == 1 or powerUpTestAnnun == 1) and systemAlive.getBoolValue()) {
			ST_annun.setBoolValue(1);
		} else {
			ST_annun.setBoolValue(0);
		}		
		if ((rollMode == 2 or powerUpTestAnnun == 1) and systemAlive.getBoolValue()) {
			HD_annun.setBoolValue(1);
		} else {
			HD_annun.setBoolValue(0);
		}
		if ((rollMode == 3 or powerUpTestAnnun == 1) and systemAlive.getBoolValue()) {
			LO_annun.setBoolValue(1);
		} else {
			LO_annun.setBoolValue(0);
		}
		if ((rollMode == 4 or powerUpTestAnnun == 1) and systemAlive.getBoolValue()) {
			HI_annun.setBoolValue(1);
		} else {
			HI_annun.setBoolValue(0);
		}
		
		#if ((pitchMode == 0 or powerUpTestAnnun == 1) and systemAlive.getBoolValue()) {
		#	RDY_annun.setBoolValue(1);
		#} else {
		#	RDY_annun.setBoolValue(0);
		#}
		if ((pitchMode == 1 or powerUpTestAnnun == 1) and systemAlive.getBoolValue()) {
			ALT_annun.setBoolValue(1);
		} else {
			ALT_annun.setBoolValue(0);
		}
		
		# Electric Pitch Trim
		if (systemAlive.getBoolValue()) {
			if (powerUpTestAnnun == 1 or (pitchMode > 0 and getprop("/stec30/internal/elevator") < -0.025 and masterSW.getValue() and trimSW.getValue())) {
				UP_annun.setBoolValue(1);
			} else if (pitchMode > 0 and UP_annun.getBoolValue() and getprop("/stec30/internal/elevator") < -0.01 and masterSW.getValue() and trimSW.getValue()) {
				UP_annun.setBoolValue(1);
			} else {
				UP_annun.setBoolValue(0);
			}
			if (powerUpTestAnnun == 1 or (pitchMode > 0 and getprop("/stec30/internal/elevator") > 0.025 and masterSW.getValue() and trimSW.getValue())) {
				DN_annun.setBoolValue(1);
			} else if (pitchMode > -1 and DN_annun.getBoolValue() and getprop("/stec30/internal/elevator") > 0.01 and masterSW.getValue() and trimSW.getValue()) {
				DN_annun.setBoolValue(1);
			} else {
				DN_annun.setBoolValue(0);
			}
		} else {
			UP_annun.setBoolValue(0);
			DN_annun.setBoolValue(0);
		}
		
		cdiDefl = OBSNAVNeedle.getValue();
		
		# NAV modes gain, reduces as the system captures the course
		if (rollMode == 3){
			gain_factor = 0.8;
		}else if(rollMode == 4){
			gain_factor = 1.2;
		}
		
		if (rollMode == 3 or rollMode == 4) {
			if (abs(cdiDefl) <= 1.5 and NAVGain.getValue() == NAVGainStd) { # CAP mode
				NAVGain.setValue(NAVGainCap);
				NAVStep1Time = elapsedSec.getValue();
			} else if (NAVStep1Time + 15 <= elapsedSec.getValue() and NAVGain.getValue() == NAVGainCap) { # CAP SOFT mode
				NAVGain.setValue(NAVGainCapSoft);
				NAVStep2Time = elapsedSec.getValue();
			} else if (NAVStep2Time + 75 <= elapsedSec.getValue() and NAVGain.getValue() == NAVGainCapSoft) { # SOFT mode
				NAVGain.setValue(NAVGainSoft);
				NAVStep3Time = elapsedSec.getValue();
			}
			
			# Return to CAP SOFT if needle deflection is >= 50% for 60 seconds
			if (cdiDefl >= 5 and NAVGain.getValue() == NAVGainSoft) {
				if (NAVOver50Counting != 1) { # Prevent it from constantly updating the time
					NAVOver50Counting = 1;
					NAVOver50Time = elapsedSec.getValue();
				}
				if (NAVOver50Time + 60 < elapsedSec.getValue()) { # CAP SOFT mode
					NAVGain.setValue(NAVGainCapSoft);
					NAVStep2Time = elapsedSec.getValue();
					if (NAVOver50Counting != 0) {
						NAVOver50Counting = 0;
					}
				}
			}
		} else {
			if (NAVGain.getValue() != NAVGainStd) {
				NAVGain.setValue(NAVGainStd);
			}
			if (NAVOver50Counting != 0) {
				NAVOver50Counting = 0;
			}
		}
		
		# Limit the turn rate depending on the mode
		if (rollMode == 3 or rollMode == 4) { # Turn rate in NAV modes
			if (NAVGain.getValue() == NAVGainCapSoft) {
				minTurnRate.setValue(-0.45*gain_factor);
				maxTurnRate.setValue(0.45*gain_factor);
			} else if (NAVGain.getValue() == NAVGainSoft) {
				minTurnRate.setValue(-0.15*gain_factor);
				maxTurnRate.setValue(0.15*gain_factor);
			} else {
				minTurnRate.setValue(-0.9*gain_factor);
				maxTurnRate.setValue(0.9*gain_factor);
			}
		} else { # 90% turn rate in all other modes
			minTurnRate.setValue(-0.9);
			maxTurnRate.setValue(0.9);
		}
		
	},
	loopFast: func() {
		# Roll Servo
		if (masterSW.getValue() and roll.getValue() > -1) {
			if (servoRollPower.getBoolValue() != 1) {
				servoRollPower.setBoolValue(1);
				discSound.setBoolValue(0);
			}
		} else {
			if (servoRollPower.getBoolValue() != 0) {
				servoRollPower.setBoolValue(0);
				setprop("/controls/flight/aileron", 0);
				if (roll.getValue() != -2) {
					discSound.setBoolValue(1);
				}
			}
		}
		
		# Pitch Servo
		if (masterSW.getValue() and pitch.getValue() > -1) {
			if (servoPitchPower.getBoolValue() != 1) {
				servoPitchPower.setBoolValue(1);
			}
		} else {
			if (servoPitchPower.getBoolValue() != 0) {
				servoPitchPower.setBoolValue(0);
				setprop("/controls/flight/elevator", 0);
			}
		}
		
		
	},
	killAP: func() { # Kill all AP modes
		NAVt.stop();
		roll.setValue(0);
		pitch.setValue(0);
	},
	killAPPitch: func() { # Kill only the pitch modes
		pitch.setValue(0);
	},
};

var button = {
	DISC: func() {
		ITAF.killAP();
	},
	MODE: func() {
		if (systemAlive.getBoolValue() and powerUpTest.getValue() == 0 and serviceable.getBoolValue()) {
			var rollMode=roll.getValue();
			if(rollMode<4){
				rollMode=rollMode+1;
			}else{
				rollMode=0;
			}
			roll.setValue(rollMode);
		}
	},
	ALT: func() { #Alt Button on the yoke
		if (systemAlive.getBoolValue() and powerUpTest.getValue() == 0 and roll.getValue() != 0 and serviceable.getBoolValue()) {
			if (pitch.getValue() == 0) { 
				pitch.setValue(1);
				altOffset.setValue(0);
				alt.setValue(staticPress.getValue());
			} else {
				altOffset.setValue(0);
				alt.setValue(staticPress.getValue());
				pitch.setValue(0);
			}
		}
	},
};

var NAVchk = func {
	if (roll.getValue() == 3) {
		if (OBSActive.getBoolValue()) { # Only engage NAV if OBS reports in range
			NAVt.stop();
			roll.setValue(1);
			if (NAVGain.getValue() != NAVGainStd) {
				NAVGain.setValue(NAVGainStd);
			}
			if (abs(OBSNAVNeedle.getValue()) <= 1 and abs(HDGIndicator.getValue() - NAVCourse.getValue()) < 5) { # Immediately go to SOFT mode if within 10% of deflection and within 5 degrees of course.
				NAVGain.setValue(NAVGainSoft);
				NAVStep1Time = elapsedSec.getValue() - 90;
				NAVStep2Time = elapsedSec.getValue() - 75;
				NAVStep3Time = elapsedSec.getValue();
			}
		}
	} else if (roll.getValue() == 0 and NAVManIntercept.getBoolValue()) {
		if (abs(NAVIntercept.getValue()) > 0.1 and abs(NAVIntercept.getValue()) < 40) { # Only engage NAV if within capture
			NAVt.stop();
			roll.setValue(1);
			if (NAVGain.getValue() != NAVGainStd) {
				NAVGain.setValue(NAVGainStd);
			}
			if (abs(OBSNAVNeedle.getValue()) <= 1 and abs(HDGIndicator.getValue() - NAVCourse.getValue()) < 5) { # Immediately go to SOFT mode if within 10% of deflection and within 5 degrees of course.
				NAVGain.setValue(NAVGainSoft);
				NAVStep1Time = elapsedSec.getValue() - 90;
				NAVStep2Time = elapsedSec.getValue() - 75;
				NAVStep3Time = elapsedSec.getValue();
			}
		}
	} else {
		NAVt.stop();
	}
}


var NAVt = maketimer(0.5, NAVchk);
var update = maketimer(0.1, ITAF, ITAF.loop);
var updateFast = maketimer(0.05, ITAF, ITAF.loopFast);
