//New version of the ascent script! Now in lower-case and metric units!
//Program 110
//1st program of flight, number 10 - so I have space for 10 pre-programs, and 90 post-programs, all for phase 1 of flight - initial launch. First program of phase two would be 2##. Of phase 3, 3##, etc.

clearscreen.

set terminal:width to 37.
set terminal:height to 37.

local tgt_hdot is 0. // For use in other, non-circular orbit versions.
local tgt_h is 114000. // 85 nmi - initial "climb to this during ascent" altitude
local tgt_ap is 114000. // triggers engine cut-off.
local tgt_apo is 160000.
local tgt_xdot is sqrt( body:mu / (body:radius + altitude) ).
local pitch_limit is 30.
local cmd_pitch is 90.
local cmd_throttle is 1.0.
//local g_limit is 3.5*9.8. // For smooth ride.
local g_limit is 3.5*9.8. // For smooth ride.
local throttle_floor is 0.21.
local range is 0.
local tower_height is 180. // Saturn V was ~100 meters tall. We can climb for a bit until rolling, no prob.
local wait_time is 0.1.
local transition_h is 36576. // 120,000 ft. Altitude to cease pitch program and use interactive guidance.
local min_turn_speed is 60. // Somewhat arbitrarily chosen minimum speed to begin pitch program.
local nullify_time is 15.
local pitch_increment is 0.3. // divide by wait_time to get degrees/second on aposeek program.
local throttle_increment is 0.01. // divide by wait_time to get throttle/second.
local R_mag is 0.2. // controls how hard we turn to align orbits.
local t is 0.
local tzero is 0.
local timer_on is false.

local ap_window is 926.

local throttle_min is 0.59. // range from 0 to 1! This is minimum throttle engines are capable of! 0.76 is J-2S. .07 RL-10 CECE.
local azimuth is 90.
local dogleg is true.
local t_dogleg is 240. // time to begin dogleg turn
local dogleg_length is 10. //how long for the main part of the dogleg turn.

// To be defined later
local est_time_for_hdot_seek is 30. // est. time to CO to begin hdot seek. Will need to tweak for higher tgt_hdot.
local status is "STANDBY.".
local status_detail is " ".
local hddot is 0.
local aposeek is 0.
local apodot is 0.
local ap_to_go_plus is 0.
local ap_to_go_minus is 0.
local g_eff is 0.
local g_inertial is 0.
local xddot is 0.
local est_tto_co is 1. // keep it larger than est_tto_tgt_hdot to keep from skipping null hdot routine.
local pitch_program_running is false.

local trelease is 6.
local tsrbsep is 148.
local tfairingjettison is 160.
local tmeco is 354.

declare function print_us_telemetry
	{

	clearscreen.

	print "=====================================" at (0,0).

	if hastarget = false {
	print "SLS 1B ASCENT. EUS+TLI. NO TGT." at (0,1).
	}

	if hastarget = true {
	print "SLS 1B ASCENT. EUS+TLI. HAS TGT." at (0,1).
	}

	print "=====================================" at (0,2).
	print "STATE: " + status at (0,3).
	print status_detail at (0,4).

	print "T+ " + round(t - trelease) at (0,6).
	print "CMD PITCH: " + round(cmd_pitch) at (0,7).
	print "ALTITUDE: " + (round(altitude*0.0539957)/100) + " NMI." at (0,8).
	print "ALTITUDE: " + round(altitude*3.28084) + " FT." at (0,9).
	print "VELOCITY: " + round(velocity:orbit:mag*3.28084) + " FPS." at (0,10).
	print "VELOCITY TGO: " + round((tgt_xdot - velocity:orbit:mag)*3.28084) + " FPS." at (0,11).
	print "HDOT: " + round(verticalspeed*3.28084) + " FPS." at (0,12).
	print "HDDOT: " + round(hddot*3.28084) + " FPS." at (0,13).

	print "RANGE: " + round(range*0.00539957) + " NMI." at (0,14).

	print "T: " + round(cmd_throttle*100) at (0,22).
	print "G: " + (round(100*g_inertial/9.82)/100) at (0,23).

	if t < tsrbsep
		{

		print "SRB SEP T -" + round((tsrbsep - t)) at (0, 25).

		}
	else if t < tfairingjettison
		{

		print "FAIRING JETT IN T -" + round((tfairingjettison - t)) at (0, 25).

		}
	else if t < tmeco
		{

		print "MECO IN T -" + round((tmeco - t)) at (0, 25).

		}

	if t + 10 > tsrbsep and t < tsrbsep
		{

		print "STANDBY FOR SRB SEP..." at (0, 26).

		}

	if t + 10 > tfairingjettison and t < tfairingjettison
		{

		print "STANDBY FOR FAIRING JETTISON..." at (0, 26).

		}

	if t + 10 > tmeco and t < tmeco
		{

		print "STANDBY FOR STAGING..." at (0, 26).

		}

	}

declare function engage_steering
	{

	local is_on_same_side_of_planet is true.

	if hastarget = false and pitch_program_running = false
		{
		local p is vectorexclude(body:position,velocity:orbit).
		local phat is p:normalized.
		local u is (-body:position).
		local uhat is u:normalized.
		local h is (uhat*sin(cmd_pitch) + phat*cos(cmd_pitch)).

		lock steering to lookdirup(h,u).
		}

//	else if hastarget = true
//		{
//		local p is vectorexclude(body:position,target:velocity:orbit).
//		local phat is p:normalized.
//		local u is (-body:position).
//		local uhat is u:normalized.
//		local h is (uhat*sin(cmd_pitch) + phat*cos(cmd_pitch)).
//
//		lock steering to lookdirup(h,u).
//		}

	if hastarget = true and pitch_program_running = false
		{
		local p is vectorexclude(body:position,target:velocity:orbit).
		local phat is p:normalized.
		local u is (-body:position).
		local uhat is u:normalized.

		local L_tgt is vcrs(target:velocity:orbit,(body:position - target:position)).
		local L is vcrs(velocity:orbit,body:position).
		local L_rel is (L_tgt:normalized - L:normalized).
		local L_relhat is L_rel:normalized.

		local turn_angle is (L_relhat * velocity:orbit:normalized). // ranges from -1 to 1, so just use it.
		// square rooted for mapping. Want small numbers to try to hone in on zero, but not asymptotically!
		local R is vcrs(body:position,velocity:orbit).
		local Rhat is R:normalized.

		local R_mult is 1.

		local h is (phat + R_mult*R_mag*turn_angle*Rhat).

		local hhat is h:normalized.

		local go is (uhat*sin(cmd_pitch) + hhat*cos(cmd_pitch)).
		lock steering to lookdirup(go,u).
		}

	else
		{
		lock steering to heading(azimuth,cmd_pitch).
		}

	}

declare function normal_close
	{
	set range to (range + groundspeed*wait_time).
	set xddot to groundspeed.
	set hddot to verticalspeed.
	wait wait_time.
	set hddot to (verticalspeed - hddot)/wait_time.
	set xddot to (groundspeed - xddot)/wait_time.
	set g_eff to ((velocity:orbit:sqrmagnitude / body:position:mag) - body:mu / body:position:sqrmagnitude).
	set g_inertial to sqrt(xddot^2 + (hddot-g_eff)^2).

	if g_inertial > g_limit
		{
		set cmd_throttle to cmd_throttle - throttle_increment.
		}

	if availablethrust/mass < g_limit and cmd_throttle < 1.0
		{
		set cmd_throttle to 1.0.
		}

	if cmd_throttle < throttle_floor
		{
		set cmd_throttle to throttle_floor.
		set status_detail to "G WARNING.".
		}

	print_us_telemetry.

	if timer_on = true
		{
		set t to (time:seconds - tzero).
		}

	lock throttle to cmd_throttle.
	}

declare function sI_ascent_program
	{
	set cmd_pitch to (0.017 * (t - 220) * (t - 220) + 100) / 10.

	if cmd_pitch > pitch_limit
		{
		set cmd_pitch to pitch_limit.
		}

	engage_steering.

	set status to "SI ADV PITCH PROGRAM.".
	set status_detail to "NOMINAL.".

	normal_close.
	}

//
//
// BEGIN SCRIPT
//
//

until availablethrust > 0
	{
	set status to "STANDBY.".
	set status_detail to "IGNITE TO START.".

	normal_close.
	}

	set timer_on to true.
	set tzero to time:seconds.

until t > trelease
	{
	set status to "RS25 SPOOL-UP.".
	set status_detail to "MAIN ENGINE START.".

	normal_close.
	}

STAGE.

until alt:radar > tower_height
	{

	if availablethrust > 0 and timer_on = false
		{
		set timer_on to true.
		set tzero to time:seconds.
		}

	lock steering to lookdirup(ship:facing:forevector,ship:up:forevector). // should just lock the craft inertially.

	set status to "LIFTOFF.".
	set status_detail to "AWAITING TOWER CLEARANCE.".

	normal_close.
	}

set pitch_program_running to true.

until altitude > transition_h
	{
	set cmd_pitch to (90 - 60*altitude/transition_h).

	engage_steering.

	set status to "PITCH PROGRAM.".
	set status_detail to "NOMINAL.".

	normal_close.
	}

set pitch_program_running to false.

until t > tsrbsep
	{
	sI_ascent_program.
	}

STAGE.

until t > tfairingjettison
	{
	sI_ascent_program.
	}

STAGE.

until t > tmeco
	{
	sI_ascent_program.
	}

lock throttle to 0.0.

WAIT 0.5.

STAGE.

until t > tmeco + 2
	{
	sI_ascent_program.
	}

STAGE.

until t > tmeco + 5
	{
	sI_ascent_program.
	}

until t > 720
	{
	set cmd_pitch to (0.0007 * (t - 350) * (t - 350) + 160) / 10.

	if cmd_pitch > pitch_limit
		{
		set cmd_pitch to pitch_limit.
		}

	engage_steering.

	set status to "SII ADV PITCH PROGRAM.".
	set status_detail to "NOMINAL.".

	normal_close.
	}

local lower_limit is -5.

until periapsis > tgt_apo
	{
	set status to "NULL HDOT.".
	set status_detail to "NOMINAL.".

	set est_tto_co to (tgt_xdot - velocity:orbit:mag)/g_inertial.

	local use_acc is (throttle_min + ((1 - throttle_min)*cmd_throttle))*(availablethrust/mass).

	if g_inertial > g_limit
		{
		set use_acc to g_limit.
		}

	local term is 0.

	if use_acc > 0
		{
		set term to ((((tgt_hdot-verticalspeed)/nullify_time)-g_eff)/use_acc).
		}

	if term < 0.5 AND term > -0.5
		{
		set cmd_pitch to arcsin(term).
		}
	else if term > 0.5
		{
		set cmd_pitch to pitch_limit.
		}
	else if term < -0.5
		{
		set cmd_pitch to -pitch_limit.
		}

	if cmd_pitch < lower_limit
		{
		set cmd_pitch to lower_limit.
		}

	engage_steering.

	normal_close.

	}

lock throttle to 0.0.


set status to "ENGINE CUTOFF AT TIME " + t.
set status_detail to "PROGRAM COMPLETE. ASCENT NOMINAL.".

print "STATE: " + status at (0,3).
print status_detail at (0,4).

wait 7.