ov_mm4.ks

////////////////////////////////////////////////////////////////////////////////
//	OV		MAJOR MODE 4 (LINEAR GUIDANCE ASCENT)
////////////////////////////////////////////////////////////////////////////////
// SPECIFIC GRAVITY CONSTANT FOR ASCENT BODY
GLOBAL MU IS CONSTANT:G * KERBIN:MASS.
GLOBAL G0 IS 9.80665.

// FIX DEFICIENCIES IN KOS MATH LIBRARY
FUNCTION RAD { PARAMETER X.		RETURN CONSTANT:PI*X/180.0. }
FUNCTION DEG { PARAMETER X.		RETURN 180.0*X/CONSTANT:PI. }
FUNCTION ACOS { PARAMETER X.	RETURN RAD(ARCCOS(MAX(-1, MIN(1, X )) )). }
FUNCTION EXP { PARAMETER X.		RETURN CONSTANT:E^X. }
FUNCTION POSITIVE_NON_ZERO { PARAMETER X. RETURN MAX(0.1, X). }


////////////////////////////////////////////////////////////////////////////////
// GUIDANCE MODE (1: SINGLE STAGE, 2: TWO-STAGE)
GLOBAL GUIDANCE_MODE	IS MM4_NUMBER_OF_STAGES.

// STAGES LIST
GLOBAL STAGES			IS LIST().
STAGES:ADD(0). 			// NULL STAGE (0)
STAGES:ADD(LEXICON()).	// STAGE 1
STAGES:ADD(LEXICON()).	// STAGE 2

// PRE-CALCULATE SOME STAGE PARAMETERS
FUNCTION PRECALC_STAGE { PARAMETER S.
	// EXHAUST VELOCITY (M/S)
	S:ADD("REF_VE", G0*S["REF_ISP"]).
	// FUEL FLOW (KG/SEC)
	S:ADD("REF_FF",	S["REF_F"]/(G0*S["REF_ISP"])).
	// STAGE BURN TIME (SECONDS)
	//S:ADD("REF_BURNTIME", (S["FUEL"]/S["REF_FF"])*1.01).
	
	// SET ALL GUIDANCE VALUES TO REFERENCE ONES (FOR LACK OF BETTER ESTIMATES)
	S:ADD("F", S["REF_F"]).
	S:ADD("VE", S["REF_VE"]).
	//S:ADD("BURNTIME", S["REF_BURNTIME"]).
	
	// GUIDANCE VARIABLES
	S:ADD("A0", 0.1).
	S:ADD("TAU", 0.1).
	S:ADD("T", 0.1).
}

// STAGE 1 SETTINGS
STAGES[1]:ADD("A0CONST",	MM4_STAGE1_A0CONST).						// CONSTANT ACCELERATION
STAGES[1]:ADD("REF_F",		MM4_STAGE1_REF_F*MM4_STAGE1_POWER_SETTING).	// ENGINE REFERENCE THRUST
STAGES[1]:ADD("REF_ISP",	MM4_STAGE1_REF_ISP).						// ENGINE REFERENCE ISP
PRECALC_STAGE(STAGES[1]).
GLOBAL STAGE1 IS STAGES[1].

// STAGE 2 SETTINGS
STAGES[2]:ADD("A0CONST",	MM4_STAGE2_A0CONST).						// CONSTANT ACCELERATION
STAGES[2]:ADD("REF_F",		MM4_STAGE2_REF_F*MM4_STAGE2_POWER_SETTING).	// ENGINE THRUST
STAGES[2]:ADD("REF_ISP",	MM4_STAGE2_REF_ISP).						// ENGINE ISP
PRECALC_STAGE(STAGES[2]).
GLOBAL STAGE2 IS STAGES[2].

// TARGET PARAMETERS
//	TARGET PERIAPSIS
GLOBAL TARGET_PE		IS KERBIN:RADIUS + MM4_TARGET_AP.
//	TARGET APOAPSIS
GLOBAL TARGET_AP		IS KERBIN:RADIUS + MM4_TARGET_PE.
//	TARGET TRUE ANOMALY (IN DEGREES)
GLOBAL TARGET_TA		IS MM4_TARGET_TA.
// == DERIVED PARAMETERS ==
GLOBAL TARGET_E 		IS (TARGET_AP - TARGET_PE) / (TARGET_AP + TARGET_PE).
GLOBAL TARGET_A			IS TARGET_AP/(1+TARGET_E).
GLOBAL TARGET_P			IS 2*TARGET_AP*TARGET_PE/(TARGET_AP+TARGET_PE).
GLOBAL TARGET_H			IS (MU*TARGET_P)^0.5.
GLOBAL TARGET_V			IS ((MU/TARGET_P)^0.5)*TARGET_E*SIN(TARGET_TA).
GLOBAL TARGET_R			IS TARGET_P/(1+TARGET_E*COS(TARGET_TA)).


////////////////////////////////////////////////////////////////////////////////
// STATE VARIABLES
GLOBAL OMEGA			IS 0.					// ANGULAR VELOCITY IN INERTIAL COORDINATES
GLOBAL V0_0				IS 0.					// RADIAL VELOCITY OF THE CURRENT STAGE
GLOBAL R0_0				IS 0.					// RADIAL POSITION OF THE CURRENT STAGE

// GUIDANCE VARIABLES
GLOBAL ON_TRAJECTORY	IS FALSE.				// IS ROCKET ON TRAJECTORY YET
GLOBAL CUTOFF_TIME		IS 0.					// COMPUTED TIME TO FINAL CUTOFF
GLOBAL STAGE_TIME		IS 0.					// COMPUTED TIME TO STAGE CUTOFF
//GLOBAL TBIAS			IS 0.					// TIME OF LAUNCH
GLOBAL TAB				IS 0. 					// TIME OF A/B STEERING CONSTANTS
GLOBAL A				IS 0. 					// CURRENT VALUE OF A STEERING CONSTANT
GLOBAL B				IS 0. 					// CURRENT VALUE OF B STEERING CONSTANT
GLOBAL A_1				IS 0. 					// LAST ESTIMATE FOR A1 STEERING CONSTANT
GLOBAL B_1				IS 0. 					// LAST ESTIMATE FOR B1 STEERING CONSTANT

//GLOBAL PITCH 			IS 0. 					// PITCH COMPUTED FIXME


////////////////////////////////////////////////////////////////////////////////
// THRUST INTEGRALS
FUNCTION A0 { PARAMETER S,T.
	IF S["A0CONST"] > 0 {	RETURN S["A0CONST"].
	} ELSE {				RETURN S["A0"]/(1 - T/S["TAU"]).	
	}
}
FUNCTION B0 { PARAMETER S,T.
	IF S["A0CONST"] > 0 {	RETURN S["A0CONST"] * T.
	} ELSE {				RETURN -S["VE"] * LN(MAX(1 - T/S["TAU"], 0.01)).	
	}
}
FUNCTION B1 { PARAMETER S,T.
	IF S["A0CONST"] > 0 {	RETURN S["A0CONST"] * (T^2)/2.
	} ELSE {				RETURN B0(S,T)*S["TAU"] - S["VE"]*T.				
	}
}
FUNCTION C0 { PARAMETER S,T.
	IF S["A0CONST"] > 0 {	RETURN S["A0CONST"] * (T^2)/2.
	} ELSE {				RETURN B0(S,T)*T - B1(S,T).
	}
}
FUNCTION C1 { PARAMETER S,T.
	IF S["A0CONST"] > 0 {	RETURN S["A0CONST"] * (T^3)/6.
	} ELSE {				RETURN C0(S,T)*S["TAU"] - S["VE"]*(T^2)/2.
	}
}

// THRUST INTEGRALS (PER-STAGE)
FUNCTION A0_1 { PARAMETER T. RETURN A0(STAGE1,T). }
FUNCTION B0_1 { PARAMETER T. RETURN B0(STAGE1,T). }
FUNCTION B1_1 { PARAMETER T. RETURN B1(STAGE1,T). }
FUNCTION C0_1 { PARAMETER T. RETURN C0(STAGE1,T). }
FUNCTION C1_1 { PARAMETER T. RETURN C1(STAGE1,T). }
FUNCTION A0_2 { PARAMETER T. RETURN A0(STAGE2,T). }
FUNCTION B0_2 { PARAMETER T. RETURN B0(STAGE2,T). }
FUNCTION B1_2 { PARAMETER T. RETURN B1(STAGE2,T). }
FUNCTION C0_2 { PARAMETER T. RETURN C0(STAGE2,T). }
FUNCTION C1_2 { PARAMETER T. RETURN C1(STAGE2,T). }

// DISCONTINUITIES (FIXME: OPTIMIZE CALLS TO V0_2, R0_2)
FUNCTION dA { // dA = A2 - A1
	LOCAL STAGE1_T IS STAGE1["T"].
	LOCAL R IS POSITIVE_NON_ZERO( R0_2() ).	
	
	RETURN (MU/(R^2) - (OMEGA^2)*R) * (1/A0_1(STAGE1_T) - 1/A0_2(0)).
}
FUNCTION dB { // dB = B2 - B1
	LOCAL STAGE1_T IS STAGE1["T"].
	LOCAL STAGE1_VE IS STAGE1["VE"].
	LOCAL STAGE2_VE IS STAGE2["VE"].
	LOCAL V IS V0_2().
	LOCAL R IS POSITIVE_NON_ZERO( R0_2() ).	
	
	RETURN -(MU/(R^2) - (OMEGA^2)*R) * ( 1/STAGE1_VE - 1/STAGE2_VE )
		   +((3*(OMEGA^2) - 2*MU/(R^3))*V) * (1/A0_1(STAGE1_T) - 1/A0_2(0)).
}

// RADIAL VELOCITY AT THE BEGINNING OF STAGE 2 BURN
FUNCTION V0_2 {
	IF MINOR_MODE = 1 {
		RETURN V0_0. // CURRENT STAGE 2 VELOCITY
	} ELSE {
		LOCAL STAGE1_T IS STAGE1["T"].
		RETURN V0_0 + B0_1(STAGE1_T)*A_1 + B1_1(STAGE1_T)*B_1. // PREDICTED VELOCITY
	}
}

// RADIAL POSITION AT THE BEGINNING OF STAGE 2 BURN
FUNCTION R0_2 {
	IF MINOR_MODE = 1 {
		RETURN R0_0. // CURRENT STAGE 2 POSITION
	} ELSE {
		LOCAL STAGE1_T IS STAGE1["T"].
		RETURN R0_0 + V0_0*STAGE1_T + C0_1(STAGE1_T)*A_1 + C1_1(STAGE1_T)*B_1. // PREDICTED POSITION
	}
}


////////////////////////////////////////////////////////////////////////////////
// SOLVE LINEAR GUIDANCE EQUATIONS FOR TWO-STAGE FLIGHT
FUNCTION LG_SOLVE_2STAGE {
	LOCAL STAGE1_T IS STAGE1["T"].
	LOCAL STAGE2_T IS STAGE2["T"].

	// V(T) = UV*A1 + VV*B1 + WV
	LOCAL UV IS B0_1(STAGE1_T) + B0_2(STAGE2_T).
	LOCAL VV IS B1_1(STAGE1_T) + 
				B1_2(STAGE2_T) + B0_2(STAGE2_T)*STAGE1_T.
	LOCAL WV IS B0_2(STAGE2_T)*dA() + B1_2(STAGE2_T)*dB() + 
				V0_0.
	
	// R(T) = UR*A1 + VR*B1 + WR
	LOCAL UR IS C0_1(STAGE1_T) + 
				C0_2(STAGE2_T) + B0_1(STAGE1_T)*STAGE2_T.
	LOCAL VR IS C1_1(STAGE1_T) + 
				C1_2(STAGE2_T) + C0_2(STAGE2_T)*STAGE1_T + B1_1(STAGE1_T)*STAGE2_T.
	LOCAL WR IS C0_2(STAGE2_T)*dA() + C1_2(STAGE2_T)*dB() +
				R0_0 + V0_0*(STAGE1_T + STAGE2_T).
	
	// SOLVE LINEAR EQUATION
	LOCAL F1 IS TARGET_V - WV.
	LOCAL F2 IS TARGET_R - WR.

	LOCAL DET_D IS UV*VR-VV*UR.
	LOCAL DET_A IS VR*F1-F2*VV.
	LOCAL DET_B IS UV*F2-F1*UR.
	
	// GET STEERING CONSTANTS
	SET A_1 TO DET_A / DET_D.
	SET B_1 TO DET_B / DET_D.
}

// SOLVE EQUATIONS FOR ONE-STAGE FLIGHT
FUNCTION LG_SOLVE_1STAGE {
	LOCAL STAGE1_T IS STAGE1["T"].

	// V(T) = UV*A1 + VV*B1 + WV
	LOCAL UV IS B0_1(STAGE1_T).
	LOCAL VV IS B1_1(STAGE1_T).
	LOCAL WV IS V0_0.
	
	// R(T) = UR*A1 + VR*B1 + WR
	LOCAL UR IS C0_1(STAGE1_T).
	LOCAL VR IS C1_1(STAGE1_T).
	LOCAL WR IS R0_0 + V0_0*STAGE1_T.
	
	// SOLVE LINEAR EQUATION
	LOCAL F1 IS TARGET_V - WV.
	LOCAL F2 IS TARGET_R - WR.

	LOCAL DET_D IS UV*VR-VV*UR.
	LOCAL DET_A IS VR*F1-F2*VV.
	LOCAL DET_B IS UV*F2-F1*UR.
	
	// GET STEERING CONSTANTS
	SET A_1 TO DET_A / DET_D.
	SET B_1 TO DET_B / DET_D.
}

// SET STAGE A0 AND TAU BASED ON CURRENT MASS
FUNCTION LG_SET_A0_TAU { PARAMETER S.
	IF S["A0CONST"] > 0 {
		SET S["A0"] TO S["A0CONST"].
		SET S["TAU"] TO S["T"].
	} ELSE {
		SET S["A0"] TO S["F"] / (SHIP:MASS*1000).
		SET S["TAU"] TO S["VE"] / S["A0"].
	}
}

// SET STAGE A0 AND TAU BASED ON CURRENT MASS MINUS DELTA
FUNCTION LG_SET_A0_TAU_MINUS_DELTA { PARAMETER S, DELTA_MASS.
	IF S["A0CONST"] > 0 {
		SET S["A0"] TO S["A0CONST"].
		SET S["TAU"] TO S["T"].
	} ELSE {
		SET S["A0"] TO S["F"] / (SHIP:MASS*1000 - DELTA_MASS).
		SET S["TAU"] TO S["VE"] / S["A0"].
	}
}

// ESTIMATE STAGE CUTOFF TIME BY DELTA-V AND PREVIOUS STAGE TIME
FUNCTION LG_ESTIMATE_T_BY_DV { PARAMETER S, DV, T0.
	IF S["A0CONST"] > 0 {
		SET S["T"] TO DV/S["A0CONST"] + T0.
	} ELSE {
		SET S["T"] TO S["TAU"]*(1 - EXP(-DV/S["VE"])) + T0.
	}
}


////////////////////////////////////////////////////////////////////////////////
//
// ENTER MAJOR MODE 4
//
FUNCTION MM4_ENTER {
	// ENABLE THROTTLING AND STEERING
	ENABLE_THROTTLE().
	ENABLE_STEERING().
	
	// RESET CURRENT STEERING TO INITIAL VALUES
	SET CURRENT_STEERING TO HEADING(90, 90).
	SET CURRENT_THROTTLE TO MM4_STAGE1_POWER_SETTING. //1.0.
	GLOBAL MAX_THROTTLE TO 1.0.
	GLOBAL PITCH_MAX TO 90.0.
	
	// FIXME: READ CURRENT PITCH COMMAND FROM BODY ANGLES
	GLOBAL PITCH_CMD TO PITCH_MAX.
	
	// DETECT SHIPS ACCELERATION SENSOR
	GLOBAL HAS_ACCELERATION_SENSOR TO FALSE.
	GLOBAL ACCELERATION_SENSOR TO 0.
	FOR PART IN SHIP:PARTS {
		IF (PART:NAME = "sensorAccelerometer")	{
			SET HAS_ACCELERATION_SENSOR TO TRUE.
			SET ACCELERATION_SENSOR TO PART:GETMODULE("ModuleEnviroSensor").
		}
	}
}

//
// READ CURRENT VALUE OF ACCELERATION
//
FUNCTION MM4_READ_ACCELERATION {
	IF NOT HAS_ACCELERATION_SENSOR {
		RETURN 0.
	}
	
	// GET SENSOR VALUE
	LOCAL VALUE IS ACCELERATION_SENSOR:GETFIELD("display").
	IF VALUE = "OFF" {
		ACCELERATION_SENSOR:DOACTION("toggle display", TRUE ).
		SET VALUE TO "0u".
	}
	
	// CONVERT TO A VALID NUMERICAL VALUE (THANKS kOS)
	RETURN G0*(PVAR_S2N(VALUE:SUBSTRING(0,VALUE:LENGTH-1))).
}

//
// LEAVE MAJOR MODE 4
//
FUNCTION MM4_LEAVE {
	DISABLE_THROTTLE().
	DISABLE_STEERING().
}

//
// TRANSFER TO MAJOR 4
//
FUNCTION MM4_TRANSFER {
	// RESET PERFORMANCE ESTIMATION LOOP
	GLOBAL PREVIOUS_MASS TO 0.
	GLOBAL PREVIOUS_VELOCITY TO 0.
	GLOBAL ESTIMATED_A0 TO 0.
	GLOBAL ESTIMATED_FF TO 0.
	GLOBAL ESTIMATED_THRUST TO 0.
	GLOBAL ESTIMATED_ISP TO 0.
	
	// RESET GOOD ESTIMATE COUNTS
	GLOBAL GOOD_ESTIMATES TO 0.
	GLOBAL INCORPORATE_ESTIMATES TO FALSE.
	
	GLOBAL TEST_TIME TO TIME:SECONDS.
}


////////////////////////////////////////////////////////////////////////////////
// MAJOR LOOP: RECOMPUTES STEERING CONSTANTS AND DOES THE HEAVY CALCULATIONS
////////////////////////////////////////////////////////////////////////////////
FUNCTION MM4_MAJOR_LOOP_TASK { PARAMETER DT.
	IF DT = INIT {
		HORIZ_LINE(0, 29, 9).
		HORIZ_LINE(0, 29, 13).
		VERT_LINE(19, 14, 19).
	}

	//--------------------------------------------------------------------------
	// ESTIMATE CURRENT ROCKET THRUST AND ISP
	IF DT <> INIT {
		// (AIRSPEED - PREVIOUS_VELOCITY)/DT.
		
		LOCAL DT1 IS MAX(0.001, TIME:SECONDS - TEST_TIME).
		SET TEST_TIME TO TIME:SECONDS.
		
		// MAKE ESTIMATES
		LOCAL MASS_KG IS SHIP:MASS*1000.
		LOCAL V_ORB IS SHIP:VELOCITY:ORBIT:MAG.

		IF HAS_ACCELERATION_SENSOR { // CAN MAKE PRECISE ESTIMATE BASED ON ACCELERATION SENSOR
			SET ESTIMATED_FF TO ABS(PREVIOUS_MASS - MASS_KG)/DT1.
			SET ESTIMATED_A0 TO MM4_READ_ACCELERATION(). //(V_ORB - PREVIOUS_VELOCITY)/DT.
			SET ESTIMATED_THRUST TO MASS_KG*ESTIMATED_A0.
			SET ESTIMATED_ISP TO ESTIMATED_THRUST/MAX(0.1, G0*ESTIMATED_FF).
		} ELSE { // MAKE ESTIMATE ONLY BASED ON FUEL FLOW, ASSUMES NO ISP EXCURSIONS
			SET ESTIMATED_FF TO ABS(PREVIOUS_MASS - MASS_KG)/DT1.
			SET ESTIMATED_THRUST TO STAGE1["REF_F"]*(ESTIMATED_FF / STAGE1["REF_FF"]).
			SET ESTIMATED_A0 TO ESTIMATED_THRUST/MASS_KG.
			SET ESTIMATED_ISP TO STAGE1["REF_ISP"]. //ESTIMATED_THRUST/MAX(0.1, G0*ESTIMATED_FF).
		}

		// PREPARE FOR THE NEXT ROUND
		SET PREVIOUS_MASS TO MASS_KG.
		SET PREVIOUS_VELOCITY TO V_ORB.
		
		// CHECK IF ESTIMATE IS OK
		IF ON_TRAJECTORY {
			//SET INCORPORATE_ESTIMATES TO TRUE.
			
			//LOCAL THRUST_PERCENTAGE IS ESTIMATED_THRUST/STAGE1["REF_F"].
			//LOCAL ISP_PERCENTAGE IS ESTIMATED_ISP/STAGE1["REF_ISP"].
			//IF (THRUST_PERCENTAGE > 0.10) AND (THRUST_PERCENTAGE < 4.00) AND
			//(ISP_PERCENTAGE    > 0.50) AND (ISP_PERCENTAGE    < 1.50) {
			//	SET GOOD_ESTIMATES TO GOOD_ESTIMATES + 1.
			//	IF GOOD_ESTIMATES > 5 {
			//		SET INCORPORATE_ESTIMATES TO TRUE. // FIXME
			//	}
			//} ELSE {
			//	SET GOOD_ESTIMATES TO 0.
			//	SET INCORPORATE_ESTIMATES TO FALSE.
			//}
		}
	}	
	
	//--------------------------------------------------------------------------
	// AXES OF THE SOI-CENTRIC INERTIAL COORDINATE SYSTEM (ALL VECS IN SHIP:RAW)
	LOCAL ORIGIN IS SHIP:BODY:POSITION.
	LOCAL IX IS (LATLNG( 0, 0):POSITION - ORIGIN):NORMALIZED. // X AXIS (ZERO)
	LOCAL IY IS (LATLNG( 0,90):POSITION - ORIGIN):NORMALIZED. // Y AXIS (EAST)
	LOCAL IZ IS (LATLNG(90, 0):POSITION - ORIGIN):NORMALIZED. // Z AXIS (NORTH)

	// INERTIAL POSITION AND VELOCITY
	LOCAL VR0 IS SHIP:POSITION - ORIGIN.						// POSITION IN SHIP:RAW
	LOCAL VV0 IS SHIP:VELOCITY:ORBIT.							// VELOCITY IN SHIP:RAW
	LOCAL VR IS V(VDOT(VR0, IX), VDOT(VR0, IY), VDOT(VR0, IZ)). // POSITION IN INERTIAL
	LOCAL VV IS V(VDOT(VV0, IX), VDOT(VV0, IY), VDOT(VV0, IZ)).	// VELOCITY IN INERTIAL
	LOCAL HV IS VCRS(VR, VV).									// ORBITAL ANGULAR MOMENTUM
	
	// CALCULATE AXES AND POSITION VECTORS FOR THE COORDINATE SYSTEM
	LOCAL R IS VR:NORMALIZED.
	LOCAL V IS VV:NORMALIZED.
	LOCAL H IS HV:NORMALIZED.
	LOCAL TG IS VCRS(H,R).		// TANGENTIAL VECTOR
	
	LOCAL VRAD IS VDOT(VV, R).	// RADIAL VELOCITY
	LOCAL VTAN IS VDOT(VV, TG).	// TANGENTIAL VELOCITY
	
	SET R0_0 TO VR:MAG.			// CURRENT POSITION
	SET V0_0 TO VRAD.			// CURRENT RADIAL VELOCITY
	SET OMEGA TO VTAN/VR:MAG.	// ANGULAR VELOCITY (IN INERTIAL COORDINATES)
	LOCAL HMAG IS HV:MAG.		// ANGULAR MOMENTUM

	// ESTIMATE RESIDUAL VELOCITY
	LOCAL DV IS (TARGET_H - HMAG)/((R0_0 + TARGET_R)*0.5).
	
	// INCORPORATE THRUST/ISP ESTIMATES INTO GUIDANCE
	IF INCORPORATE_ESTIMATES {
		IF MINOR_MODE = 0 {
			SET STAGE1["F"] TO STAGE1["F"]*0.50 + 0.50*ESTIMATED_THRUST.
			//SET STAGE1["VE"] TO STAGE1["VE"]*0.50 + 0.50*G0*ESTIMATED_ISP.
			//SET STAGE1["BURNTIME"] TO STAGE1["BURNTIME"]*0.50 + 0.50*(STAGE1["FUEL"]/ESTIMATED_FF)*1.01.
		} ELSE {
			//SET STAGE1["F"] TO STAGE1["REF_F"].
			SET STAGE2["F"] TO STAGE2["F"]*0.50 + 0.50*ESTIMATED_THRUST.
		}
	}
	
	// RUN GUIDANCE
	IF GUIDANCE_MODE = 1 {
		// UPDATE STAGE ACCELERATION AND TAU
		LG_SET_A0_TAU(STAGE1).
		
		// ESTIMATE STAGE 1 CUTOFF TIME BY REMAINING DV
		LG_ESTIMATE_T_BY_DV(STAGE1, DV, 0).
		SET CUTOFF_TIME TO STAGE1["T"].
		SET STAGE_TIME TO STAGE1["T"].
		
		// SOLVE LINEAR GUIDANCE EQUATIONS (ONE-STAGE)
		LG_SOLVE_1STAGE().
	} ELSE { // TWO-STAGE GUIDANCE
	
		// ESTIMATE FUEL LOAD IN STAGE 1
		LOCAL STAGE1_LF IS MAX(0, STAGE:RESOURCESLEX["LIQUIDFUEL"]:AMOUNT - MM4_STAGE1_BURNOUT_LF).
		LOCAL STAGE1_OX IS MAX(0, STAGE:RESOURCESLEX["OXIDIZER"]:AMOUNT   - MM4_STAGE1_BURNOUT_OX).
		LOCAL STAGE1_FUEL_MASS IS (STAGE1_LF + STAGE1_OX)*5.0.
	
		// UPDATE STAGE ACCELERATION AND TAU (BUT DONT UPDATE STAGE1 WHEN IT BURNED OUT)
		IF MINOR_MODE = 0 {
			LG_SET_A0_TAU(STAGE1).
			LG_SET_A0_TAU_MINUS_DELTA(STAGE2, STAGE1_FUEL_MASS + MM4_STAGE1_BURNOUT_DRY).
		} ELSE { // CURRENT MASS KNOWN
			LG_SET_A0_TAU(STAGE1).
			LG_SET_A0_TAU(STAGE2).
		}
		
		// ESTIMATE STAGE 1 CUTOFF TIME BY BURN TIME
		IF MINOR_MODE = 0 {
			// ESTIMATE CURRENT FUEL FLOW
			LOCAL CURRENT_FF IS STAGE1["REF_FF"].
			
			// ESTIMATE BURN TIME
			SET STAGE1["T"] TO STAGE1_FUEL_MASS / CURRENT_FF.
		} ELSE {
			// JUST SET TO ZERO
			SET STAGE1["T"] TO 0.0.
		}
		
		// ESTIMATE STAGE 2 CUTOFF TIME BY REMAINING DV
		LG_ESTIMATE_T_BY_DV(STAGE2, DV, STAGE1["T"]).
		SET CUTOFF_TIME TO STAGE2["T"].

		// SOLVE LINEAR GUIDANCE EQUATIONS (TWO-STAGE)
		LG_SOLVE_2STAGE().
		
		// STAGE TRANSITIONING
		IF (MINOR_MODE = 0) AND (STAGE:NUMBER < MM4_FIRST_STAGE_NO) {
			TRANSFER_MODE(4,1).
		}
		
		// SET CORRECT TIME TO STAGE CUTOFF
		IF MINOR_MODE = 0 {
			SET STAGE_TIME TO STAGE1["T"].
		} ELSE {
			SET STAGE_TIME TO STAGE2["T"].
		}
	}
	
	// UPDATE STEERING CONSTANTS (FREEZE THEM WHEN APPROACHING STAGE CUTOFF)
	IF STAGE_TIME > 7 {
		SET TAB TO TIME:SECONDS.
		IF MINOR_MODE = 0 {
			// STAGE 1 STEERING CONSTANTS
			SET A TO A_1.
			SET B TO B_1.
		} ELSE {
			// STAGE 2 STEERING CONSTANTS
			SET A TO A_1 + dA() + B_1*STAGE1["T"].
			SET B TO B_1 + dB().
		}
	}
	
	//--------------------------------------------------------------------------
	// SHOW UI VARIABLES
	UI_VARIABLE("R   ", "M",		R0_0, 			0,8, NUMBER, 0,0).
	UI_VARIABLE("Rdot", "M/S",		V0_0, 			1,8, SIGNED, 0,1).
	UI_VARIABLE("Tdot", "M/S",		VTAN, 			1,8, SIGNED, 0,2).
	UI_VARIABLE("-dV ", "M/S",		DV, 			1,8, NUMBER, 0,3).
	UI_VARIABLE("RT", "",			TARGET_R, 		0,8, NUMBER, 18,0).
	UI_VARIABLE("VT", "",			TARGET_V, 		0,8, SIGNED, 18,1).

	PRINT_TIME(CUTOFF_TIME,	1,8).
	PRINT_TIME(STAGE_TIME,	12,8).
	
	UI_VARIABLE("1   T", "",		STAGE1["T"], 	1,7, SIGNED, 0,7).
	UI_VARIABLE("1  A0", "",		STAGE1["A0"], 	1,7, SIGNED, 0,8).
	UI_VARIABLE("1 TAU", "",		STAGE1["TAU"], 	1,7, SIGNED, 0,9).
	UI_VARIABLE("2   T", "",		STAGE2["T"], 	1,7, SIGNED, 16,7).
	UI_VARIABLE("2  A0", "",		STAGE2["A0"], 	1,7, SIGNED, 16,8).
	UI_VARIABLE("2 TAU", "",		STAGE2["TAU"], 	1,7, SIGNED, 16,9).
	
	UI_VARIABLE("A", "",			A, 				6,8, SIGNED, 0,11).
	UI_VARIABLE("B", "",			B, 				6,8, SIGNED, 0,12).
	
	//UI_VARIABLE("FF", "KGS",		ESTIMATED_FF, 	2,5, NUMBER, 18,1).
	UI_VARIABLE("N", "",			STAGE:NUMBER, 	2,5, NUMBER, 18,2).
	UI_VARIABLE("N", "",			STAGE:RESOURCESLEX["LIQUIDFUEL"]:AMOUNT, 	2,5, NUMBER, 18,3).
	
	IF MINOR_MODE = 0 { // STAGE 1
		UI_VARIABLE(" FF", "",		100*ESTIMATED_FF/STAGE1["REF_FF"], 			1,5, NUMBER, 20,11).
		UI_VARIABLE("THR", "",		100*ESTIMATED_THRUST/STAGE1["REF_F"], 		1,5, NUMBER, 20,12).
		UI_VARIABLE("ISP", "",		100*ESTIMATED_ISP/STAGE1["REF_ISP"], 		1,5, NUMBER, 20,13).
		UI_VARIABLE(" G", "",		ESTIMATED_A0/G0, 							3,6, NUMBER, 20,14).
		UI_VARIABLE("CTA", "",		INCORPORATE_ESTIMATES, 						0,3, ONOFF,  21,16).
	} ELSE { // STAGE 2
		UI_VARIABLE(" FF", "",		100*ESTIMATED_FF/STAGE2["REF_FF"], 			1,5, NUMBER, 20,11).
		UI_VARIABLE("THR", "",		100*ESTIMATED_THRUST/STAGE2["REF_F"], 		1,5, NUMBER, 20,12).
		UI_VARIABLE("ISP", "",		100*ESTIMATED_ISP/STAGE2["REF_ISP"], 		1,5, NUMBER, 20,13).
		UI_VARIABLE(" G", "",		ESTIMATED_A0/G0, 							3,6, NUMBER, 20,14).
		UI_VARIABLE("CTA", "",		INCORPORATE_ESTIMATES, 						0,3, ONOFF,  21,16).
	}

	// RUN TASK AT HIGHER RATE AS CUTOFF APPROACHES
	//IF CUTOFF_TIME > 60.0 {
		//TASK_SCHEDULE(6, MM4_MAJOR_LOOP_TASK@).
	//} ELSE {
		TASK_SCHEDULE(5, MM4_MAJOR_LOOP_TASK@).
	//}
}


////////////////////////////////////////////////////////////////////////////////
// MINOR LOOP: COMPUTES STEERING COMMAND AND STEERS THE ROCKET
////////////////////////////////////////////////////////////////////////////////
FUNCTION MM4_MINOR_LOOP_TASK { PARAMETER DT.
	// DO NOT RUN MINOR LOOP UNTIL INITIALIZED
	IF TAB < 1 { RETURN. }
	
	// DETERMINE AMOUNT OF SECONDS FROM LAST STEERING CONSTANT UPDATE FOR EXTRAPOLATION
	LOCAL T IS TIME:SECONDS - TAB.

	// DETERMINE COORDINATES. SEE MAJOR LOOP FOR EXPLANATION OF THESE CALCULATIONS
	LOCAL ORIGIN IS SHIP:BODY:POSITION.
	LOCAL IX IS (LATLNG( 0, 0):POSITION - ORIGIN):NORMALIZED.
	LOCAL IY IS (LATLNG( 0,90):POSITION - ORIGIN):NORMALIZED.
	LOCAL IZ IS (LATLNG(90, 0):POSITION - ORIGIN):NORMALIZED.
	LOCAL VR0 IS SHIP:POSITION - ORIGIN.
	LOCAL VV0 IS SHIP:VELOCITY:ORBIT.
	LOCAL VR IS V(VDOT(VR0, IX), VDOT(VR0, IY), VDOT(VR0, IZ)).
	LOCAL VV IS V(VDOT(VV0, IX), VDOT(VV0, IY), VDOT(VV0, IZ)).
	LOCAL HV IS VCRS(VR, VV).
	LOCAL R IS VR:NORMALIZED.
	LOCAL H IS HV:NORMALIZED.
	LOCAL TG IS VCRS(H,R).
	LOCAL VTAN IS VDOT(VV, TG).
	LOCAL RMAG IS VR:MAG.
	SET OMEGA TO VTAN/VR:MAG.
	// (FIXME: OPTIMIZE THESE CALCULATIONS)
	
	// COMPUTE STEERING CONSTANT C (GRAVICOMPONENT)
	LOCAL C IS 0.
	IF MINOR_MODE = 0 { // STAGE 1
		SET C TO (MU/(RMAG^2)-(OMEGA^2)*RMAG)/A0_1(T).
	} ELSE { // STAGE 2
		SET C TO (MU/(RMAG^2)-(OMEGA^2)*RMAG)/A0_2(T).
	}
	
	// COMPUTE STEERING COMMAND. THE GOOD STUFF
	LOCAL F IS A + B*T + C.
	

	// GENERATE PITCHOVER
	LOCAL PITCH_GRADIENT IS MIN(1,MAX(0, (ALTITUDE-7000)/(16000-7000)  )).
	LOCAL PITCH_MAX IS 80 - 20*PITCH_GRADIENT.
	
	
	// GENERATE PITCHOVER LIMIT (ADDS BASIC OPEN LOOP SECTION)
	//LOCAL PITCH_GRADIENT IS MIN(1,MAX(0, (ALTITUDE-7000)/(16000-7000)  )).
	//LOCAL PITCH_MAX IS 80 - 40*PITCH_GRADIENT.
	//LOCAL PITCHOVER_START IS 75.0.
	//LOCAL PITCHOVER_RATE  IS 1.0.
	//LOCAL PITCH_MAX IS 90 - (5*PITCH_GRADIENT1) - PITCHOVER.
	
	//LOCAL PITCH_GRADIENT1 IS MIN(1,MAX(0, (ALTITUDE-500)/(1500-500)  )).
	//LOCAL PITCH_GRADIENT2 IS 1.0 - MIN(1,MAX(0, (F-0.80)/(1.80-0.80) )).
	//LOCAL PITCHOVER IS MIN(40,MAX(0, PITCHOVER_RATE*(MISSIONTIME-PITCHOVER_START)  )).
	//SET PITCH_MAX TO PITCH_MAX*0.90 + 0.10*(90 - 15*PITCH_GRADIENT1 - 30*PITCH_GRADIENT2).
	
	// GENERATE PITCH COMMAND
	SET TGT_PITCH_CMD TO DEG(CONSTANT:PI*0.5 - ACOS(F)).
	
	//IF ALTITUDE < 16000 { 
		//SET PITCH TO PITCH_MAX.
	//}
	IF TGT_PITCH_CMD < PITCH_MAX {
		SET ON_TRAJECTORY TO TRUE.
		//LOCK THROTTLE TO 0.35.
	}
	
	IF MINOR_MODE = 1 {
		SET CURRENT_THROTTLE TO MM4_STAGE2_POWER_SETTING.
	} ELSE {
		SET CURRENT_THROTTLE TO MM4_STAGE1_POWER_SETTING.
	}
	
	// ATMOSPHERIC TEST
	//IF ALTITUDE > 25000 { LOCK THROTTLE TO 0.35. }
	
	// GENERATE PITCH COMMAND (FIXME: FREEZE STEERING CONSTANTS, NOT PITCH ANGLE...)
	//IF CUTOFF_TIME > 7 {
	//}
	
	// GENERATE CUTOFF SIGNAL
	IF CUTOFF_TIME < 5 { // CUTOFF ARMED
		LOCAL HMAG IS HV:MAG.
		LOCAL DV IS (TARGET_H - HMAG)/((R0_0 + TARGET_R)*0.5).
		IF DV < 2 { // SHUTDOWN ENGINES WHEN CUTOFF REACHED
			LOCK THROTTLE TO 0.0.
		}
		
		// FIXME: PERFORMANCE CAN BE IMPROVED BY A MORE PRECISE CUTOFF
	}
	
	
	// SET ANGLE LIMITS TO PREVENT GUIDANCE FROM COMMANDING REALLY SUBOPTIMAL HIGH ANGLES
	SET TGT_PITCH_CMD TO MIN(PITCH_MAX, MAX(-60, TGT_PITCH_CMD )).
	
	// RATE LIMIT PITCH COMMAND
	IF PITCH_CMD < TGT_PITCH_CMD {
		SET PITCH_CMD TO MIN(TGT_PITCH_CMD, PITCH_CMD + MM4_MAX_PITCH_RATE*DT).
	}
	IF PITCH_CMD > TGT_PITCH_CMD {
		SET PITCH_CMD TO MAX(TGT_PITCH_CMD, PITCH_CMD - MM4_MAX_PITCH_RATE*DT).
	}
	
	// GENERATE TARGET YAW COMMAND (FIXME)
	LOCAL YAW_CMD IS 90.
	
	// STEER THE VEHICLE. 
	SET CURRENT_STEERING TO HEADING(YAW_CMD, PITCH_CMD).
	
	//--------------------------------------------------------------------------
	// SHOW UI VARIABLES
	UI_VARIABLE("F", "", F, 			3,6, SIGNED, 11,11).
	UI_VARIABLE("P", "", TGT_PITCH_CMD,	1,6, SIGNED, 11,12).
	UI_VARIABLE("C", "", PITCH_CMD, 	1,6, SIGNED, 11,13).
	UI_VARIABLE("Y", "", YAW_CMD, 		1,6, SIGNED, 11,14).
	
	// RUN MINOR LOOP AT HIGH RATE
	TASK_SCHEDULE(1, MM4_MINOR_LOOP_TASK@).
}


////////////////////////////////////////////////////////////////////////////////
// THROTTLE LOOP: MANAGES THE ENGINE THROTTLE TO MAINTAIN STAGE LIMITS
////////////////////////////////////////////////////////////////////////////////
FUNCTION MM4_THROTTLE_TASK { PARAMETER DT.	
	//IF MINOR_MODE = 1 { // ENGINE CHECK
	//	SET CURRENT_THROTTLE TO MIN(1.0, MODE_TIMER()/3.0).
	//	
	//} ELSE IF (MINOR_MODE > 1) AND (MINOR_MODE < 8) { // FLIGHT THROTTLE
	//	// LIMIT MAX THROTTLE BASED ON ACCELERATION
	//	IF SHIP:SENSORS:ACC:MAG > 16.0 {
	//		SET MAX_THROTTLE TO MAX(0.35, MAX_THROTTLE - MM4_THROTTLE_DRATE*DT).
	//	}
	//	
	//	// DEFINE DECREASE IN THROTTLE DUE TO HIGH Q
	//	LOCAL Q_THROTTLE IS MIN(MAX((SHIP:Q - 0.15)/0.05, 0), 0.50).
	//
	//	// SET THROTTLE LEVEL
	//	SET CURRENT_THROTTLE TO MIN(1.0 - Q_THROTTLE, MAX_THROTTLE).
	//	
	//} ELSE { // OTHER MODES
	//	SET CURRENT_THROTTLE TO 0.0.
	//}
	//SET CURRENT_THROTTLE TO 1.0.
	
	//IF HAS_ACCELERATION_SENSOR {
	//	IF MM4_READ_ACCELERATION() > STAGE1["A0CONST"] {
	//		SET CURRENT_THROTTLE TO MAX(0.10, CURRENT_THROTTLE - 0.05*DT).
	//	}
	//}
	
	// DEFINE DECREASE IN THROTTLE DUE TO HIGH Q
	LOCAL Q_THROTTLE IS MIN(MAX((SHIP:Q - 0.15)/0.05, 0), 0.50).
	SET CURRENT_THROTTLE TO MIN(1.0 - Q_THROTTLE, MAX_THROTTLE).
	
	TASK_SCHEDULE(3, MM4_THROTTLE_TASK@).
}.


////////////////////////////////////////////////////////////////////////////////
// FEED TELEMETRY TO DATA RECORDER/DOWNLINK
////////////////////////////////////////////////////////////////////////////////
FUNCTION MM4_TELEMETRY_TASK { PARAMETER DT.
	//DOWNLINK("PITCH   ",ROUND(PITCH_COMMAND,3) ).
	//DOWNLINK("Q",		ROUND(SHIP:Q*100,3) ).
	//DOWNLINK("THRUST",	ROUND(MAXTHRUST,1) ).
	//DOWNLINK("MASS",	ROUND(MASS,2) ).
	//DOWNLINK("AP",		ROUND(SHIP:APOAPSIS,0) ).
	//DOWNLINK("PE",		ROUND(SHIP:PERIAPSIS,0) ).
	//DOWNLINK("TRIM",	ROUND(SERVO_GET("Main engine"),2) ).
	//DOWNLINK("ABORT",	ABORT_TYPE ).
	
	TASK_SCHEDULE(7, MM4_TELEMETRY_TASK@).
}.




////////////////////////////////////////////////////////////////////////////////
MODE_NAMES:ADD(40, "CONSTANT THRUST ").
MODE_NAMES:ADD(41, "CONSTANT ACCEL  ").

MODE_ENTER		(4, MM4_ENTER@).
MODE_LEAVE		(4, MM4_LEAVE@).
MODE_TRANSFER	(4, MM4_TRANSFER@).
MODE_TASK		(4, MM4_MAJOR_LOOP_TASK@).
MODE_TASK		(4, MM4_MINOR_LOOP_TASK@).
//MODE_TASK		(4, MM4_THROTTLE_TASK@).
//MODE_TASK		(4, MM4_TELEMETRY_TASK@).