run "library/lib_math".
run "library/lib_vessel_utils".

local athmoAccelTarget is 600.

CORE:PART:GETMODULE("kOSProcessor"):DOEVENT("Open Terminal").

clearScreen.
BRAKES on.
print "Waiting for engine ignition".
WaitForEngineStart().
if not (defined rotateSpeed){
	set rotateSpeed to 140.
}

print "Preparing to launch".
from { local c is 5.} until c = 0 step {set c to c - 1.} do {
	print c.
	wait 1.
}
set runwayPitch to (90 - vectorangle(UP:FOREVECTOR, FACING:FOREVECTOR)).
clearScreen.
print "Launching!".
// stage.
lock throttle to 1.
lock steering to heading(90, runwayPitch).
brakes off.
sas off.

print "Phase: Takeoff".
print "Waiting for rotate speed: " + rotateSpeed.
wait until groundspeed > rotateSpeed.

print "Phase: Rotate".
lock steering to heading(90, runwayPitch + 4).
wait until altitude > 80.
print "Gear up".
gear off.
wait until altitude > 150.

set accelPitch to runwayPitch + 4.
local accelPitchLoop is PIDLoop(1.0, 0.06, 0.3, runwayPitch, 15).
set accelPitchLoop:setpoint to 4.

print "Phase: Athmospheric Acceleration".
lock steering to heading(90, accelPitch, 0).
until groundspeed > athmoAccelTarget {
	set accelPitch to accelPitchLoop:update(time:seconds, verticalSpeed).
	wait 0.001.
}

print "Phase: Athmospheric Climb".
lock tgtPitch to Map(groundSpeed, athmoAccelTarget, 1600, 5, 15).
lock steering to heading(90, tgtPitch, 0).
until altitude > 20000 {
	print tgtPitch.
}

wait until altitude > 20000.

print "Phase: Mode Switch".
ag1 on. //Switch engine mode
lock steering to heading(90, 25, 0).
wait until apoapsis > 80000.

print "Phase: Sub-Orbital Coast".
lock throttle to 0.
set warpmode to "physics".
set warp to 1.

wait until altitude > 70000.
print "Deploying Solar".
panels on. //deploy solar
print "Opening Docking Port".
ag3 on. //open docking port
lock steering to prograde.
set warp to 0.
set warpmode to "rails".
set warp to 1.


lock distToApo to abs(apoapsis - altitude).
wait until distToApo < 1000.
set warp to 0.
lock steering to heading(90, 0, 0).
wait until distToApo < 800.


print "Phase: Circularize".
lock steering to heading(90, 0, 0).
rcs on.
lock throttle to 1.
wait until periapsis > 75000.
unlock throttle.

print "In orbit. Releasing controls".