clearscreen.

SET SHIP:CONTROL:PILOTMAINTHROTTLE TO 0.0.
SET terminal:width TO 41.
SET terminal:height TO 23.
SET GETVOICE(0):wave to "triangle".
SET GETVOICE(0):attack to 0.

WAIT 0.5.
print "=========================================".
print "  Launch to orbit program".
print "    First arg is a target TWR".
print "    Second arg is target altitude in m".
print "=========================================".
print "".
DECLARE PARAMETER wantedTWR is 2.3, wantedALT is 95000, wantedHEAD is 90, maneuver_prep is 50.

GETVOICE(0):PLAY( NOTE(300, .1) ).wait(0.1).GETVOICE(0):PLAY( NOTE(400, .2) ).wait(0.1).

declare function LocalG {
	// Return local gravity
	return ship:body:mu / ship:body:position:mag ^ 2.
}

declare function getTwrThrust {
	// Estimate thrust for a given TWR (1.5 by default)
	DECLARE PARAMETER TWR is 1.5.
	set m to ship:mass.
	set w to m * LocalG().
	set p to ship:sensors:pres / 100.
	set t to ship:maxthrustat(p).
	set throt to (TWR * w) / t.
	if throt <= 1.0 {
		return throt.
	} else {
		return 1.0.
	}
}


declare function dt {
	SET dtim TO TIME - start.
	return dtim:minute+"m"+dtim:second+"s".
}

declare function spd {
	if ship:distance < 40000 {
		return ship:velocity:surface:mag.
	} else {
		return ship:velocity:orbit:mag.
	}
}

SET ctl TO ship:control.


SET ctl:MAINTHROTTLE TO 0.0.
LOCK throttle TO 0.0.
print "Ready to orbit at "+wantedALT/1000+"km with TWR " + wantedTWR.
HUDTEXT("Orbit "+wantedALT/1000+"km sequence ready(TWR "+wantedTWR+")", 10, 2, 20, red, false).
HUDTEXT("Waiting 1st stage to run", 10, 2, 20, red, false).
WAIT UNTIL not stage:ready.
WAIT UNTIL ship:maxthrust() <> 0.
HUDTEXT("Orbit sequence started. Lift off in 13s", 2, 2, 20, green, false).
GETVOICE(0):PLAY( LIST(
			SLIDENOTE(300, 320, 0.45, .5), SLIDENOTE(320, 300, 0.42, 0.5),
			SLIDENOTE(300, 320, 0.45, .5), SLIDENOTE(320, 300, 0.42, 0.5),
			SLIDENOTE(300, 320, 0.45, .5), SLIDENOTE(320, 300, 0.42, 0.5))).


HUDTEXT("Throttle will be " + round(getTwrThrust(wantedTWR)*100) + "% for TWR " + wantedTWR, 5, 2, 20, green, false).
print "Throttle at takeoff :" + round(getTwrThrust(wantedTWR)*100) + "% for TWR " + wantedTWR.
WAIT 2.0.
HUDTEXT("Moving controls in all directions...", 3, 2, 15, blue, false).
SET ctl:yaw TO -1.0.
SET ctl:pitch TO -1.0.
SET ctl:roll TO -1.0.
WAIT 1.0.
HUDTEXT("So cool !", 3, 2, 12, blue, false).
SET ctl:yaw TO 1.0.
SET ctl:pitch TO 1.0.
SET ctl:roll TO 1.0.
WAIT 1.0.
SET ctl:yaw TO 0.0.
SET ctl:pitch TO 0.0.
SET ctl:roll TO 0.0.
WAIT 1.0.

DECLARE i TO 10.
UNTIL i < 1 {
	HUDTEXT(i, 1, 2, 20, green, false).
	print i+" " AT (0,7).
	GETVOICE(0):PLAY( NOTE(300, .2) ).
	wait(1.0).
	SET i TO i - 1.
}


lock THROTTLE to getTwrThrust(wantedTWR).
HUDTEXT("Lift off !", 2, 2, 20, green, false).
GETVOICE(0):PLAY( NOTE(400, .1) ).wait(0.1).GETVOICE(0):PLAY( NOTE(300, .2) ).
DECLARE start TO TIME.
print "-----------------------------------------".
print dt() + " Lift off !".

UNTIL spd() > 1 {
	WAIT 0.1.
}

HUDTEXT(dt()+" Rotation 0", 2, 2, 20, green, false).
GETVOICE(0):PLAY( NOTE(400, .1) ).wait(0.1).GETVOICE(0):PLAY( NOTE(400, .1) ).
lock STEERING to HEADING(wantedHEAD,90).
print dt() + " Heading "+ wantedHEAD.

UNTIL spd() > 50 {
	WAIT 0.1.
}

HUDTEXT(dt()+" Rotation 1", 2, 2, 20, green, false).
GETVOICE(0):PLAY( NOTE(400, .1) ).wait(0.1).GETVOICE(0):PLAY( NOTE(300, .2) ).

SET tgt_spd TO 600.
print dt() + " Next rotation at "+tgt_spd+" m/s".

lock STEERING to HEADING(wantedHEAD,70).

UNTIL spd() > tgt_spd {
	WAIT 0.1.
}

HUDTEXT(dt()+" Rotation 2", 2, 2, 20, green, false).
GETVOICE(0):PLAY( NOTE(400, .1) ).wait(0.1).GETVOICE(0):PLAY( NOTE(300, .2) ).
print dt() + " Last rotation 1000 m/s".

//declare function endrot {
//	return 60 - ((spd() - tgt_spd) * (60 - 10) / (1000 - tgt_spd)).
//}
//lock STEERING to HEADING(wantedHEAD,endrot()).
//WAIT UNTIL spd() > 1000 or SHIP:ORBIT:APOAPSIS > wantedALT.
//if endrot() >= 10 {
//	LOCK STEERING to HEADING(wantedHEAD, 10).
//} else {
//	LOCK STEERING to ship:prograde.
//}

UNTIL spd() > 1000 {
	LOCK STEERING to ship:prograde.
	WAIT 0.1.
}
LOCK STEERING to ship:prograde.

HUDTEXT("Waiting for AP to reach 80km", 5, 2, 20, green, false).
GETVOICE(0):PLAY( NOTE(400, .1) ).wait(0.1).GETVOICE(0):PLAY( NOTE(300, .2) ).
print dt() + " Waiting for AP to reach "+wantedALT/1000+"km".

WAIT UNTIL SHIP:ORBIT:APOAPSIS > wantedALT.

HUDTEXT("Apoapsis 100km reached", 5, 2, 20, green, false).
GETVOICE(0):PLAY( LIST(NOTE(300, .1),NOTE(400, .2),NOTE(400, 0.2), NOTE(220,0.2)) ).

UNLOCK throttle.
SET ctl:MAINTHROTTLE TO 0.0.
LOCK THROTTLE to 0.0.
LOCK steering TO ship:prograde.
print dt() + " Engine cut-off".
print dt() + " Waiting for AP. ETA : " + ROUND(ETA:APOAPSIS) + "s".

// TODO : calculate timeout given TWR
SET ins_alrm TO addAlarm("Maneuver",time:seconds+(ETA:APOAPSIS - maneuver_prep + 0.6), "[kOS] "+(wantedALT/1000)+"km orbit insertion", "Targeting "+(wantedALT/1000)+"km").

WAIT UNTIL MOD(round(eta:apoapsis), 5) = 0.
UNTIL ETA:APOAPSIS < maneuver_prep {
	if wantedALT - SHIP:ORBIT:APOAPSIS > 100 {
		LOCK throttle TO 1.0.
		WAIT UNTIL SHIP:ORBIT:APOAPSIS > wantedALT.
		LOCK throttle TO 0.0.
	}
	if MOD(round(ETA:APOAPSIS), 5) = 0 {
		HUDTEXT("Time to apoapsis " + round(eta:apoapsis) + "s", 5, 2, 20, green, false).
		HUDTEXT("Time to insertion maneuver " + round(eta:apoapsis) - maneuver_prep + "s", 5, 2, 20, green, false).
	}
	WAIT .3.
}
DELETEALARM(ins_alrm:id).

LOCK steering TO ship:prograde.
DECLARE prev_ETA to ETA:APOAPSIS.
LOCK throttle TO 1.0.
SET throt TO SHIP:CONTROL:MAINTHROTTLE.
SET throt_min TO 0.7.
SET throt_brk TO 0.2.

print dt() + " Starting insertion".
GETVOICE(0):PLAY( NOTE(300, .1) ).wait(0.1).GETVOICE(0):PLAY( NOTE(400, .2) ).
HUDTEXT("Starting insertion", 5, 2, 20, green, false).

// TODO : Problem occurs when there is stages left to drop
// causing wrong estimation overtaking AP
UNTIL SHIP:ORBIT:PERIAPSIS > wantedALT {
	SET dETA TO ETA:APOAPSIS - prev_ETA.
	if ETA:APOAPSIS > 120 {
		print dt() + " AP behind us ! Full thrust to safe orbit !".
		HUDTEXT("AP behind us ! Full thrust for orbit !", 0.1 ,2,20, red, false).
		SET throt TO 1.0.
		WAIT UNTIL SHIP:ORBIT:PERIAPSIS > 68000.
		HUDTEXT("Wrong orbitreached "+dt(), 25, 2, 20, red, false).
		break.
	} else {
		if dETA < 0 {
			SET throt to throt + 0.0005.
			if throt > 1.0 {
				print dt() + "Node approach, notenough thrust !!".
				SET throt TO 1.0.
				WAIT 1.0.
			}
			if throt < throt_min - throt_brk {
				GETVOICE(0):PLAY( SLIDENOTE(220,250, .3, .3) ).
				set throt to throt_min.
			} else {
				//GETVOICE(0):PLAY( NOTE(300 + ((throt - .5) * 500), .1) ).
			}
		}
		if dETA > 0 {
			SET throt TO throt - 0.0005.
			if throt < 0.0 {
				print "Failing ! Node escape, WTF !!".
				SET throt TO 0.
			} else {
				if throt < throt_min - throt_brk AND ETA:APOAPSIS > 10
				{
					// Breaking !
					GETVOICE(0):PLAY( SLIDENOTE(250, 220, .3, .3) ).
					SET throt TO 0.
					LOCK throttle TO 0.
					print "AP ETA " + round(ETA:APOAPSIS,1) + "s throttle BREAK" AT (0, terminal:height - 1).
					if ETA:APOAPSIS > 20 {
						WAIT((ETA:APOAPSIS / 3.5)-0.1).
					} else {
						if ETA:APOAPSIS > 10 {
							WAIT 1.0.
						}
					}
				} else {
					//GETVOICE(0):PLAY( NOTE(300 + ((throt - .5) * 500), .1) ).
				}
			}
		}
	}
	//SET SHIP:CONTROL:MAINTHROTTLE TO throt.
	//SET SHIP:CONTROL:PILOTMAINTHROTTLE TO throt.
	LOCK throttle TO throt.
	//LOCK throttle TO throt.
	SET prev_ETA to ETA:APOAPSIS.
	print "AP ETA " + round(ETA:APOAPSIS,1) + "s throttle "+ round(throt * 100) + "%   " AT (0, terminal:height - 1).
	//HUDTEXT("AP ETA " + ETA:APOAPSIS + "s throttle "+ throt * 100 + "%", 0.1,2,20, green, false).
	WAIT 0.
}


print dt() + " Orbiting !".
HUDTEXT("Orbiting after "+dt(), 15, 2, 20, green, false).
HUDTEXT("programs exiting...", 15, 2, 20, red, false).
GETVOICE(0):PLAY( LIST(
			SLIDENOTE(300, 320, 0.3, .3), SLIDENOTE(320, 300, 0.3, 0.3),
			SLIDENOTE(300, 320, 0.3, .3), SLIDENOTE(320, 300, 0.3, 0.3),
			SLIDENOTE(300, 320, 0.3, .3), SLIDENOTE(320, 300, 0.3, 0.3))).

unlock STEERING.
unlock throttle.
SET SHIP:CONTROL:MAINTHROTTLE TO 0.0.
SET SHIP:CONTROL:PILOTMAINTHROTTLE TO 0.0.