Inclination match launch

if not (defined launch_margin) declare global launch_margin is 60.
if not (defined orbital_speed_multiplier) declare global orbital_speed_multiplier is 0.6.
function hdg {parameter x. return mod(360+arctan2(x*vcrs(up:vector,north:vector),x*north:vector),360).}
set t0 to time:seconds.
set r0 to ship:position-body:position.
set rh to vxcl(v(0,1,0),r0).
set rv to vxcl(rh,r0).
set ph to vcrs(rh,v(0,1,0)).
lock t to time:seconds.
function pos {parameter x. return positionat(x,t)-x:body:position.}
function vel {parameter x. return velocityat(x,t):obt.}
function nrm {parameter x. return vcrs(vel(x),pos(x)):normalized.}
lock rs to pos(ship). lock vs to vel(ship). lock ns to nrm(ship).
lock rt to pos(target). lock vt to vel(target). lock nt to nrm(target).
set a to vdot(rh,nt).
set b to vdot(ph,nt).
set arg to -vdot(rv,nt)/sqrt(a^2+b^2).
if(abs(arg) > 1) log_error("no solution").
set theta1 to arccos(arg).
set theta2 to 360-theta1.
set q1 to mod(360+theta1+arctan2(b,a),360).
set q2 to mod(360+theta2+arctan2(b,a),360).
set first to min(q1,q2).
set tlaunch to t0 + (first/360)*body:rotationperiod - launch_margin.
print "Warping to launch window...".
if time:seconds < tlaunch-300 {warpto(tlaunch-300). wait 0.}
wait until warp=0 and kuniverse:timewarp:issettled and velocity:surface:mag < 0.1 and time:seconds > tlaunch-300.
if time:seconds < tlaunch-10 {set warp to 1. wait 1. warpto(tlaunch-10). wait 0.}
wait until warp=0 and kuniverse:timewarp:issettled and time:seconds > tlaunch-10.
print "Waiting " + round(tlaunch-time:seconds) + " seconds for launch window...".
wait until warp=0 and kuniverse:timewarp:issettled and time:seconds > tlaunch - 3.
set h to hdg(vcrs(rs, nt):normalized*sqrt(body:mu/(body:radius+body:atm:height+10e3))*orbital_speed_multiplier - velocity:obt).
print "please launch at heading " + h.