//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.
//M means "mode", 150 indicates a 150x150km orbit. "NO" means OMS is not required. "LOW G" is that. A smooth ride.

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 157420. // 85 nmi - initial "climb to this during ascent" altitude
local tgt_ap is 157420. // triggers engine cut-off.
local tgt_xdot is sqrt(body:mu / (body:radius+tgt_h)). // For use in other, non-circular orbit versions.
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.76. // 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 110.
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.

//debug variables
local debuga is 0.
local debugb is 0.
local debugc is 0.
local debugd is 0.
local debuge is 0.

declare function print_us_telemetry
	{

	clearscreen.

	print "=====================================" at (0,0).
	print "URSALV.P110.M85-85.NO OMS.LOW-G." at (0,1).
	print "=====================================" at (0,2).
	print "STATE: " + status at (0,3).
	print status_detail at (0,4).

	print "T+ " + round(t) 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).

	if aposeek = 1
		{
		print "APOSEEK ENGAGED." at (0,17).
		print "APOAPSIS: " + (round(apoapsis*0.0539957)/100) + " NMI." at (0,18).
		print "TGT APO:  " + (round(tgt_h*0.0539957)/100) + " NMI." at (0,19).
		print "APODOT:   " + (round(apodot*0.0539957)/100) + " NMI." at (0,20).
		}
	else if aposeek = 2
		{
		print "APOSEEK COMPLETE." at (0,17).
		}
	else
		{
		}

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

	}

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.

		set debuga to L.
		set debugb to L:normalized.
		set debugc to L_relhat.
		set debugd to R.

		local R_mult is 1.

		if dogleg = true
			{
			set R_mult to ((1+(1/90)*arctan((t-t_dogleg+dogleg_length)/dogleg_length))/2).
			}

		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.
	}

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

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.

set aposeek to 1.

until verticalspeed <= 0
	{
	set status to "APOAPSIS SEEK.".
	set status_detail to "NOMINAL.".

	local tempdot is apoapsis.

	normal_close.

	set apodot to ((apoapsis - tempdot)/wait_time).

	set ap_to_go_plus to (tgt_h + ap_window - apoapsis)/nullify_time.
	set ap_to_go_minus to (tgt_h - ap_window - apoapsis)/nullify_time.

	if ap_to_go_minus > apodot
		{
		set cmd_pitch to cmd_pitch + pitch_increment.
		}
	else if ap_to_go_plus < apodot
		{
		set cmd_pitch to cmd_pitch - pitch_increment.
		}

	if cmd_pitch > pitch_limit
		{
		set cmd_pitch to pitch_limit.
		}
	else if cmd_pitch < -pitch_limit
		{
		set cmd_pitch to -pitch_limit.
		}

	engage_steering.

	}

set aposeek to 2.

until est_time_for_hdot_seek > est_tto_co
	{
	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.
		}

	engage_steering.

	normal_close.

	}

until velocity:orbit:mag > tgt_xdot
	{

	if tgt_hdot = 0
		{
		set status to "NULL HDOT.".
		}
	else
		{
		set status to "TGT HDOT: " + (tgt_hdot*3.28084).
		}
	set status_detail to "NOMINAL.".

	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.
		}

	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.