//Bossart Expanded Launch To Orbit

//Variables
SET flightState TO "PREFLIGHT".
SET throttlePid TO PIDLOOP().
SET throttlePidOn TO FALSE.
SET targetApoapsis TO 90000.

//Functions
FUNCTION setThrottle {
	PARAMETER newThrottle.

	IF newThrottle > 1.0 {
		SET newThrottle TO 1.0.
	} ELSE IF newThrottle < 0.0 {
		SET newThrottle TO 0.0.
	}
	SET currentThrottle TO newThrottle.
}

FUNCTION calculateDVToCircularize {
	SET velocityApoapsis TO VELOCITYAT(SHIP,TIME + ETA:APOAPSIS):ORBIT:MAG.
	SET desiredVelocityApoapsis TO SQRT(CONSTANT:G * KERBIN:MASS / (APOAPSIS + KERBIN:RADIUS)).
	RETURN desiredVelocityApoapsis - velocityApoapsis.
}

FUNCTION calculateBurnDuration {
	PARAMETER dV.

	LIST ENGINES IN engineList.
	FOR engine IN engineList {
		IF engine:ACTIVE {
			SET eV TO engine:ISP * CONSTANT:G0.
		}
	}
	RETURN ((MASS * eV) / MAXTHRUST) * (1 - CONSTANT:E ^ (-dV / eV)).
}

FUNCTION getPitchComponentOfDirection {
	PARAMETER inputDirection.

	RETURN 90 - VANG(inputDirection:VECTOR, UP:VECTOR).
}

//Start here
CLEARSCREEN.

//Preflight checks
SAS OFF. //Turn off SAS
RCS OFF. //Turn off RCS
setThrottle(1.0). //Main engines full throttle
LOCK THROTTLE TO currentThrottle.
SET steeringDirection TO UP.
LOCK STEERING TO steeringDirection. //Make sure we are pointed straight up
FROM {LOCAL X IS 10.} UNTIL X = 0 STEP {SET X TO X-1.} DO {
	PRINT "Liftoff in T-" + X + "...".
	WAIT 1.
}
PRINT "Main engine ignition.".
STAGE. //Activate first stage engine. Liftoff
RCS ON.
SET flightState TO "FIRST_STAGE_FLIGHT".

//Pitch over maneuver
WAIT UNTIL VERTICALSPEED >= 90.
PRINT "Executing pitch over.".
SET steeringDirection TO HEADING(90, 83).

//Hold prograde
WAIT UNTIL VECTORANGLE(SRFPROGRADE:VECTOR, HEADING(90,83):VECTOR) <= 0.5.
PRINT "Holding prograde.".
UNTIL ETA:APOAPSIS >= 50 {
	SET steeringDirection TO HEADING(90, getPitchComponentOfDirection(SRFPROGRADE)).
}

//Hold time to apoapsis 50 seconds
PRINT "Holding time to apopapsis at 50 seconds.".
SET throttlePidOn TO TRUE.
SET throttlePid:SETPOINT TO 50.
SET throttlePid:MAXOUTPUT TO 1.0.
SET throttlePid:MINOUTPUT TO 0.1.

	WHEN throttlePidOn THEN {
		SET throttleUpdate TO throttlePid:UPDATE(TIME:SECONDS, ETA:APOAPSIS).
		setThrottle(throttleUpdate).
		RETURN TRUE.
	}

	WHEN flightState = "FIRST_STAGE_FLIGHT" THEN {
		SET steeringDirection TO HEADING(90, getPitchComponentOfDirection(SRFPROGRADE)).
		//Discard half stage after 20km altitude
		IF ALTITUDE >= 20000 {
			PRINT "Separating half stage.".
			STAGE.
			SET flightState TO "FIRST_STAGE_FLIGHT_SECOND_PHASE".
			RETURN FALSE.
		}
		RETURN TRUE.
	}

	//First stage separation
	WHEN flightState = "FIRST_STAGE_FLIGHT_SECOND_PHASE" THEN {
		SET steeringDirection TO HEADING(90, getPitchComponentOfDirection(SRFPROGRADE)).
		IF MAXTHRUST = 0.0 {
			PRINT "Separating first stage.".
			SET throttlePidOn TO FALSE.
			setThrottle(0.0).
			STAGE. //Decouple first stage
			WAIT 3.
			STAGE. //Activate second stage engine.
			SET flightState TO "SECOND_STAGE_FLIGHT_THRUST_RAMP".
			RETURN FALSE.
		}
		RETURN TRUE.
	}

	//Ramp up to full thrust
	WHEN flightState = "SECOND_STAGE_FLIGHT_THRUST_RAMP" THEN {
		IF currentThrottle = 1.0 {
			SET flightState TO "SECOND_STAGE_FLIGHT".
			SET prevEtaApoapsis TO ETA:APOAPSIS.
			SET initialTime TO TIME:SECONDS.
			SET deltaEtaApoapsis TO 0.
			RETURN FALSE.
		} ELSE {
			IF currentThrottle = 0.0 {
				PRINT "Second stage engine ignition.".
			}
			setThrottle(currentThrottle + 0.1).
			WAIT 0.1.
			RETURN TRUE.
		}
	}

	WHEN flightState = "SECOND_STAGE_FLIGHT" THEN {
		IF TIME:SECONDS > initialTime + 1 {
			SET deltaEtaApoapsis TO ETA:APOAPSIS - prevEtaApoapsis.
			SET prevEtaApoapsis TO ETA:APOAPSIS.
		}
		//If time to apoapsis starts increasing, pitch down a bit.
		IF deltaEtaApoapsis > 0 AND ETA:APOAPSIS >= 25 AND NOT throttlePidOn {
			SET newPitch TO 90 - VANG(UP:VECTOR, FACING:VECTOR) - 5.
			IF newPitch < (90 - VANG(UP:VECTOR, PROGRADE:VECTOR)) {
				SET newPitch TO 90 - VANG(UP:VECTOR, PROGRADE:VECTOR).
			}
			SET steeringDirection TO HEADING(90, newPitch).
			IF ABS(VANG(FACING:VECTOR, PROGRADE:VECTOR)) < 1 {
				IF NOT throttlePidOn {
					PRINT "Throttle PID on.".
					SET throttlePidOn TO TRUE.
					SET throttlePid:SETPOINT TO 25.
				}
			}
		} ELSE IF throttlePidOn {
			SET steeringDirection TO PROGRADE.
		}

		//Coast once reached target apoapsis
		IF APOAPSIS >= targetApoapsis {
			PRINT "Second stage engine shutdown.".
			SET throttlePidOn TO FALSE.
			setThrottle(0.0).
			SET steeringDirection TO PROGRADE.
			SET flightState TO "SECOND_STAGE_COAST_TO_APOAPSIS".
			RETURN FALSE.
		}

		//Exit if ran out of fuel early
		IF MAXTHRUST <= 0 {
			SET EXIT TO TRUE.
		}
		//Check if we ran out of fuel before hitting target apopapsis

		RETURN TRUE.
	}

	WHEN flightState = "SECOND_STAGE_COAST_TO_APOAPSIS" THEN {
		SET steeringDirection TO PROGRADE.
		IF ALTITUDE > 85000 {
			SET dVToCircularize TO calculateDVToCircularize().
			SET burnDuration TO calculateBurnTime(dVToCircularize).
			SET flightState TO "SECOND_STAGE_PRECIRCULARIZATION".
			SET initialTime TO TIME:SECONDS.
			RETURN FALSE.
		}
		//Exit if ran out of fuel early
		IF MAXTHRUST <= 0 {
			SET EXIT TO TRUE.
		}
		RETURN TRUE.
	}

	WHEN flightState = "SECOND_STAGE_PRECIRCULARIZATION" THEN {
		SET steeringDirection TO PROGRADE.
		SET timeUntilBurn TO ETA:APOAPSIS - burnDuration / 2.
		IF timeUntilBurn <= 0 {
			PRINT "Excuting circularization burn.".
			setThrottle(1.0).
			SET burnEndTime TO TIME + burnDuration.
			SET flightState TO "CIRCULARIZATION".
			RETURN FALSE.
		} ELSE {
			IF TIME:SECONDS >= initialTime + 1 {
				SET initialTime TO TIME:SECONDS.
				PRINT "Time until burn: " + initialTime + " seconds.".
			}
		}
		RETURN TRUE.
	}

	WHEN flightState = "CIRCULARIZATION" THEN {
		SET steeringDirection TO PROGRADE.
		IF (TIME >= burnEndTime OR PERIAPSIS >= targetApoapsis) {
			setThrottle(0.0).
			WAIT 1.
			SET SHIP:CONTROL:PILOTMAINTHROTTLE TO 0.
			UNLOCK STEERING.
			SET EXIT TO TRUE.
			RETURN FALSE.
		}
		RETURN TRUE.
	}

SET EXIT TO FALSE.
WAIT UNTIL EXIT.