// Inital gravity turn
// #include staging
run once staging.
// #include maneuver
run once maneuver.

function signedHeading {
  local fwd is ship:srfprograde:vector.
  local n is north:vector.
  local e is vCrs(up:vector, north:vector).

  local hdg is arcTan2(vdot(fwd, e), vdot(fwd, n)).
  return mod(hdg + 360, 360).
}

function gravityTurn {
  parameter targetHeading.

  set tarSteer to heading(targetHeading, 90).
  lock steering to tarSteer.
  until ship:velocity:surface:mag >= 100 {
    set tarSteer to heading(targetHeading, 90 - (ship:velocity:surface:mag / 20)).
    wait 0.
  }

  // lock steering to the target heading, but account for drift, and the pitch
  // of the surface normal vector
  lock calculatedHeading to mod(targetHeading + 2 * (targetHeading - signedHeading()) + 360, 360).
  lock steering to heading(
    calculatedHeading,
    max(5, 90 - vAng(up:vector, ship:srfprograde:vector))
  ).
  print "Locked steering to surface prograde pitch and target heading".

}

// function structuredAscent {
//   parameter targetHeading.

//   set tarSteer to heading(targetHeading, 90).
//   lock steering to tarSteer.
//   until ship:velocity:surface:mag >= 100 {
//     set tarSteer to heading(targetHeading, 90 - (ship:velocity:surface:mag / 10)).
//     wait 0.
//   }

//   // lock steering to the target heading, but account for drift, and the pitch
//   // of the surface normal vector
//   lock calculatedHeading to mod(targetHeading + 2 * (targetHeading - signedHeading()) + 360, 360).
//   lock steering to heading(
//     calculatedHeading,
//     max(5, 90 - vAng(up:vector, ship:srfprograde:vector))
//   ).
//   print "Locked steering to surface prograde pitch and target heading".
// }

function basicAscent {
  parameter targetHeading is 90, targetAltitude is 80000.

  if ship:altitude > body:atm:height return.

  // get parts of interest
  set fairings to ship:partstitledpattern("^AE-FF.*").
  set antennas to ship:partstitledpattern("^(CommTech|Communotron|HG-|RA-|Reflectron).*").

  lock throttle to 1.

  print "Counting down:".
  from {local countdown is 5.} until countdown = 0 step {set countdown to countdown - 1.} do {
    print "..." + countdown.
    wait 1.
  }

  // initial stage to takeoff
  stage.

  // stage everytime thrust drops to zero
  when ship:maxThrust = 0 then {
    print "Staging".
    stage.
    return true.
  }.

  gravityTurn(targetHeading).

  when ship:altitude > body:atm:height then {
    for f in fairings {
      // get the module and deploy it
      set m to f:getmodule("ModuleProceduralFairing").
      if m:hasevent("deploy") m:doevent("deploy").
    }

    wait 0.1.

    // this isn't always true, figure out a way around
    panels on.
    radiators on.
    for a in antennas {
      if a:hasmodule("ModuleDeployableAntenna") {
        set m to a:getmodule("ModuleDeployableAntenna").
        if m:hasevent("extend antenna") m:doevent("extend antenna").
      }
    }    
  }

  // try to limit the throttle to get a good ascent
  set ascentPID to pidLoop(1/30, 0, 0, 0.1, 1).
  set ascentPID:setpoint to 60.
  set ascentThrottle to 1.
  lock throttle to ascentThrottle.
  lock pitchAngle to 90 - vAng(up:vector, ship:prograde:vector).
  until ship:apoapsis >= targetAltitude or pitchAngle < 10 {
    set ascentThrottle to ascentPID:update(time:seconds, eta:apoapsis).
  }

  // we're pitched down so much we might as well just burn
  if ship:apoapsis < targetAltitude {
    lock throttle to 1.
    wait until ship:apoapsis >= targetAltitude.
  }

  // coast to apoapsis
  unlock calculatedHeading.
  unlock pitchAngle.
  lock steering to ship:prograde.
  lock throttle to 0.

  // don't do futher calcs until out of the atmosphere or very close to apoapsis
  until ship:altitude >= body:atm:height or eta:apoapsis < 60.

  
  // if we're using a booster to get to orbit drop it before finishing the cirularization
  when ship:periapsis > 30000 then {
    // use RCS as a condition to not stage the booster
    if ship:stagenum <> 0 and ship:stagedeltav(ship:stagenum - 1):current > 0 and not brakes {
      print "Dropping booster".

      // shutdown engines before dropping them
      for e in ship:engines {
        if e:ignition e:shutdown().
      }

      stage.
    }
  }
  executeNode(createCircularizationNodeAp()).

  print "Circularized".
}