clearscreen.
set pi to 3.1415926535897932384626433832795028841971693993751058209749445923.
set mu to 3.986*(10^14).
ag1 off.

function launchHeading {
  set azimuth to arcsin(cos(target:orbit:inclination)/cos(ship:latitude)).
  return azimuth.
}
function orbitHeading {
  set azimuth to arcsin(cos(ship:orbit:inclination)/cos(ship:latitude)).
  return azimuth.
}
function pitchAngle {
  set ang to 90-(vang(ship:velocity:surface,ship:up:vector)).
  return ang.
} //for ascent
function relativeInclination {
  set targpos to target:position-ship:body:position.
  set htarg to vcrs(targpos,target:velocity:orbit).
  set hship to -vcrs(ship:body:position,ship:velocity:orbit).
  set relinc to vang(htarg,hship).
  return relinc.
}
function distancefromPlane {
  set targpos to target:position-ship:body:position.
  set htarg to vcrs(targpos,target:velocity:orbit).
  set hship to vcrs(ship:body:position,ship:velocity:orbit).
  set relinc to vang(htarg,hship).
  set descendingNode to vcrs(htarg,hship).
  set ang to (pi/180)*vang(descendingNode,ship:body:position).
  set hypoteneuse to ang*ship:body:position:mag.
  set dist to sin(relinc)*hypoteneuse.
  return dist.
}
function pitchUp { //for space
  set r to ship:body:position:mag.
  set v to ship:velocity:orbit:mag.
  set ag to (mu/(r^2))-((v^2)/r)+alevel.
  set hypoteneuseAcc to ship:maxthrust/ship:mass.
  set theta to arcsin(ag/hypoteneuseAcc).
  return theta.
}

set gturnAngle to 3.
set gturnVelocity to 42.
set doglegAcc to 2.
set apheight to 220000.
set periap to apheight-10000.
set kpPitch to 1.
set kiPitch to 0.1.

lock steering to heading(launchHeading(),90)+r(0,0,180).
print "Liftoff!".
lock throttle to 1.
stage.
set t0 to time:seconds.

wait until ship:verticalspeed > gturnVelocity.

lock steering to heading(launchHeading(),90-gturnAngle)+r(0,0,180).
print "T+"+round(time:seconds-t0)+" Seconds. Beginning Roll Program".

wait until vang(ship:velocity:surface,ship:up:vector)>gturnAngle.
print "T+"+round(time:seconds-t0)+" Seconds. Roll Program Complete".

lock steering to heading(launchHeading(),pitchAngle())+r(0,0,180).

//staging
wait until (time:seconds-t0)>154.
lock throttle to 0.
print "T+"+round(time:seconds-t0)+" Seconds. MECO".
wait until (time:seconds-t0)>158.
print "T+"+round(time:seconds-t0)+" Seconds. First Stage Separation. RCS on".

stage.
rcs on.
lock throttle to 1.
wait until (time:seconds-t0)>161.
print "T+"+round(time:seconds-t0)+" Seconds. Second Stage Ignition".
stage.


set newPitch to pitchAngle().
until ship:apoapsis > apheight {
  lock steering to heading(orbitHeading(),newPitch)+r(0,0,180).
  wait 0.1.
}

set a to 0.
until a > doglegAcc {
  lock steering to heading(orbitHeading(),0)+r(0,0,180).
  set v to (sin(relativeInclination())*ship:velocity:orbit:mag).
  set x to distancefromPlane().
  set a to (1/(2*x))*(v^2).
  wait 0.1.
}


print "T+"+round(time:seconds-t0)+" Seconds. Beginning Dogleg Maneuver".
set upAngle to 0.
set alevel to 0.
until alevel > 1  and ship:verticalspeed < 0 {
  set relativeInc to relativeInclination().
  if relativeInc > 0.05 {
    set v to (sin(relativeInc)*ship:velocity:orbit:mag).
    set x to distancefromPlane().
    set a to ((1/(2*x))*(v^2)).
    set thrustacc to (ship:maxthrust/ship:mass)*cos(upAngle).
    set offsetAngle to arcsin(a/thrustacc).
  }. else {
    set offsetAngle to 0.
  }.
  set vlevel to ship:verticalspeed.
  set hdiff to ship:altitude-periap.
  set alevel to (1/(2*hdiff))*(vlevel^2).



  lock steering to heading(orbitHeading()-offsetAngle,upAngle)+r(0,0,180).
}
print "T+"+round(time:seconds-t0)+" Seconds. Leveling off at 210 km".
until ship:verticalspeed > -0.5 {
  set relativeInc to relativeInclination().
  if relativeInc > 0.05 {
    set v to (sin(relativeInc)*ship:velocity:orbit:mag).
    set x to distancefromPlane().
    set a to ((1/(2*x))*(v^2))+1.
    set thrustacc to (ship:maxthrust/ship:mass)*cos(upAngle).
    set offsetAngle to arcsin(a/thrustacc).
  }. else {
    set offsetAngle to 0.
  }.

  set vlevel to ship:verticalspeed.
  set hdiff to ship:altitude-periap.
  set alevel to (1/(2*hdiff))*(vlevel^2).

  set upAngle to pitchUp().
  lock steering to heading(orbitHeading()-offsetAngle,upAngle)+r(0,0,180).
}

set alevel to 0.
set vsInt to 0.
until ship:periapsis > 0 {
  set relativeInc to relativeInclination().
  if relativeInc > 0.05{
    set v to (sin(relativeInc)*ship:velocity:orbit:mag).
    set x to distancefromPlane().
    set a to ((1/(2*x))*(v^2))+1.
    set thrustacc to (ship:maxthrust/ship:mass)*cos(upAngle).
    set offsetAngle to arcsin(a/thrustacc)..
  }. else {
    set offsetAngle to 0.
  }.

  set vs1 to ship:verticalspeed.
  set t1 to time:seconds.
  wait 0.1.
  set vs2 to ship:verticalspeed.
  set t2 to time:seconds.
  set dt to t2-t1.

  set vsInt to vsInt+(dt*(vs2+vs1)/2).

  set anglediff to (-kpPitch*(ship:verticalspeed))+(-kiPitch*vsInt).
  set upAngle to pitchUp()+anglediff.
  lock steering to heading(orbitHeading()-offsetAngle,upAngle)+r(0,0,180).
}

lock steering to prograde +r(0,0,180).
ag2 on.
wait until ship:apoapsis > 390000.
lock throttle to 0.
print "T+"+round(time:seconds-t0)+" Seconds. SECO".
wait 10.
stage.
print "T+"+round(time:seconds-t0)+" Seconds. Spacecraft Separation".
print "Remember to engage persistent rotation".
set ship:control:fore to 1.
lock steering to prograde +r(0,0,180).

wait until ship:apoapsis>400000.

set ship:control:neutralize to true.
run rendezvous.