clearscreen.
set mu to 3.986e14.
set g to 9.81.
set pi to 3.1415926535897932384626433832795028841971693993751058209749445923.
set transThrust to 4.41.  //.530 for 3 clowns 0.26 for resupply
set foreThrust to 8.67. //1.06 for 3 clowns 0.52 for resupply
set bodyRadius to 6371000.
lights on.



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 vectorheading {
  parameter vector.
  set northvec to ship:north:vector.
  set westvec to vcrs(northvec,ship:up:vector).
  set vcrossup to vcrs(vector,ship:up:vector).
  if vang(vcrossup,northvec) > 90 {
    set hdg to -vang(vcrossup,westvec).
  } else {
    set hdg to vang(vcrossup,westvec).
  }
  return hdg.
}

function targetDistance {
    set td to target:position:mag.//(TARGET:ROOTPART:POSITION - SHIP:ROOTPART:POSITION):MAG
    return td.
}

function lateralDistance {
  parameter sv.
  parameter tp.
  set theta to (pi/180)*(sv-tp).
  set ld to theta*target:position:mag.
  return ld.
}
function verticalDistance {
  set theta to (pi/180)*(vang(target:position,(-1*ship:up:vector))-90).
  set ld to -theta*target:position:mag.
  return ld.
}

lock steering to prograde+r(0,0,180).
clearscreen.
print "Closing. Translating Downward".
set traveldist to target:position:mag+200. //we want to maneuver 200m away
set dv to traveldist/4000. //1 m/s downward moves the chaser ~4km Foreward
set a to transThrust/ship:mass.
set t to dv/a.
set t0 to time:seconds.
set ship:control:top to 1.
set dX1 to target:altitude-ship:altitude.

until time:seconds>(t0+t){ //burning downward
  set periodDiff to (target:orbit:period-ship:orbit:period).
  set ship:control:fore to 10*periodDiff.
  print "Maneuver dV is "+(round(dv*100)/100)+" m/s".
  print "Closing. Translating Downward".
  print "Orbital period difference is: "+(round(periodDiff*1000))+" ms".
  wait 0.5.
  clearscreen.
}


//Warping to ~250m.
set ship:control:neutralize to true.
set dX2 to target:altitude-ship:altitude.
set dX to dX2-dX1.
lock steering to retrograde+r(0,0,180).
set steeringmanager:maxstoppingtime to 5.
until vang(-ship:velocity:orbit,ship:facing:vector) < 1 {
  wait 0.1.
}
wait 15.
unlock steering.
sas on.
print "Warping".
set t0 to time:seconds.
set kuniverse:timewarp:warp to 1.
wait until (target:altitude-ship:altitude)>(dx*1.51).
set kuniverse:timewarp:warp to 2.
wait until (target:altitude-ship:altitude < (dx*1.5)).
set kuniverse:timewarp:warp to 1.
wait until (target:altitude-ship:altitude < (dx*1.25)).
set kuniverse:timewarp:warp to 0.
sas off.
lock steering to prograde + r(0,0,180).
wait until (target:altitude-ship:altitude) < dX.

print "Beginning Second Stationkeeping Burn".
set ship:control:top to 1.
wait until (target:altitude-ship:altitude)<10.


sas off.
rcs on.
lock steering to retrograde + r(0,0,180).

set transVelocity to .5.
set kpv to -5. set kps to 1.5.
set kd to 5.
set dLat to 50.
set dVer to 50.
set dTar to 50.
set Vinput to 0. set Tinput to 0. set Linput to 0.
ag6 off.

until ag6 { //stationkeeping loop around 8km from station
  set svh to vectorheading(-ship:velocity:orbit). //chaser velocity heading
  set tph to vectorheading(target:position). //target position heading
  set latdist1 to lateralDistance(svh,tph).
  set verdist1 to verticalDistance().
  set t1 to time:seconds.
  set tardistActual to targetDistance().
  set tardist1 to targetDistance()-100.
  wait 0.1.

  clearscreen.
  print "Approaching Station".
  print "Press 6 to continue with docking".
  print " ".
  print "Station Distance is  "+round(tardistActual)+" m".
  print "Warning, Station Distance number may be incorrect".
  print " ".
  print "Lateral Distance is  "+(round(1000*latdist1)/1000)+" m".
  print "Vertical Distance is "+(round(1000*verdist1)/1000)+" m".
  print " ".
  Print "Vertical Control Input: "+(round(100*Vinput)/100).
  print "Lateral Control Input:  "+(round(100*Linput)/100).
  print "Foreward Control Input: "+(round(100*Tinput)/100).
  print " ".
  print "Vertical Velocity: "+(round(1000000*dVer)/1000)+" mm/s".
  print "Lateral Velocity:  "+(round(1000000*dLat)/1000)+" mm/s".
  print "Foreward Velocity: "+(-round(1000000*dtar)/1000)+" mm/s".


  set svh to vectorheading(-ship:velocity:orbit). //chaser velocity heading
  set tph to vectorheading(target:position). //target position heading
  set latdist2 to lateralDistance(svh,tph).
  set verdist2 to verticalDistance().
  set tardist2 to targetDistance()-100.

  set t2 to time:seconds.
  set dLat to (latdist2-latdist1)/(t2-t1).
  set dVer to (verdist2-verdist1)/(t2-t1).
  set dtar to (tardist2-tardist1)/(t2-t1).


  if abs(latdist2) > 125 {
    set lateralVsetpoint to -4*transVelocity*latdist2/abs(latdist2).
    set Linput to (lateralVsetpoint-dLat)*kpv.
  } else if abs(latdist2) > 50 {
    set lateralVsetpoint to -2*transVelocity*latdist2/abs(latdist2).
    set Linput to (lateralVsetpoint-dLat)*kpv.
  } else if abs(latdist2) > 10 {
    set lateralVsetpoint to -transVelocity*latdist2/abs(latdist2).
    set Linput to (lateralVsetpoint-dLat)*kpv.
  } else if abs(latdist2) > 5 {
    set lateralVsetpoint to -0.5*transVelocity*latdist2/abs(latdist2).
    set Linput to (lateralVsetpoint-dLat)*kpv.
  } else {
    set Linput to (kps*latdist2)+(kd*dLat).
  }

  if abs(verdist2) > 125 {
    set verticalVsetpoint to -4*transVelocity*verdist2/abs(verdist2).
    set Vinput to (verticalVsetpoint-dVer)*kpv.
  } else if abs(verdist2) > 50 {
    set verticalVsetpoint to -2*transVelocity*verdist2/abs(verdist2).
    set Vinput to (verticalVsetpoint-dVer)*kpv.
  } else if abs(verdist2)>10 {
    set verticalVsetpoint to -transVelocity*verdist2/abs(verdist2).
    set Vinput to (verticalVsetpoint-dVer)*kpv.
  } else if abs(verdist2) > 5 {
    set verticalVsetpoint to -0.5*transVelocity*verdist2/abs(verdist2).
    set Vinput to (verticalVsetpoint-dVer)*kpv.
  } else {
    set Vinput to (kps*verdist2)+(kd*dVer).
  }

  if abs(tardist2) > 125 {
    set targetVsetpoint to -4*transVelocity.
    set Tinput to (targetVsetpoint-dtar)*kpv.
  } else if abs(tardist2) > 50 {
    set targetVsetpoint to -2*transVelocity.
    set Tinput to (targetVsetpoint-dTar)*kpv.
  } else if abs(tardist2)>10 {
    set targetVsetpoint to -transVelocity.
    set Tinput to (targetVsetpoint-dTar)*kpv.
  } else if abs(tardist2) > 5 {
    set targetVsetpoint to -0.5*transVelocity.
    set Tinput to (targetVsetpoint-dTar)*kpv.
  } else {
    set Tinput to (kps*tardist2)+(kd*dTar).
  }

  set ship:control:fore to Tinput.
  set ship:control:starboard to Linput.
  set ship:control:top to Vinput.

}

ag6 off.

set ILat to 0.
set IVer to 0.
set KIs  to .025.

until ag6 { //stationkeeping loop around 8km from station
  set svh to vectorheading(-ship:velocity:orbit). //chaser velocity heading
  set tph to vectorheading(target:position). //target position heading
  set latdist1 to lateralDistance(svh,tph).
  set verdist1 to verticalDistance().
  set t1 to time:seconds.
  set tardistActual to targetDistance().
  set tardist1 to targetDistance().
  wait 0.1.

  clearscreen.
  print "Docking".
  print " ".
  print "Station Distance is  "+round(tardistActual)+" m".
  print "Lateral Distance is  "+(round(1000*latdist1)/1000)+" m".
  print "Vertical Distance is "+(round(1000*verdist1)/1000)+" m".
  print " ".
  Print "Vertical Control Input: "+(round(100*Vinput)/100).
  print "Lateral Control Input:  "+(round(100*Linput)/100).
  print "Foreward Control Input: "+(round(100*Tinput)/100).
  print " ".
  print "Vertical Velocity: "+(round(1000000*dVer)/1000)+" mm/s".
  print "Lateral Velocity:  "+(round(1000000*dLat)/1000)+" mm/s".
  print "Foreward Velocity: "+(-round(1000000*dtar)/1000)+" mm/s".


  set svh to vectorheading(-ship:velocity:orbit). //chaser velocity heading
  set tph to vectorheading(target:position). //target position heading
  set latdist2 to lateralDistance(svh,tph).
  set verdist2 to verticalDistance().
  set tardist2 to targetDistance().

  set t2 to time:seconds.
  set dLat to (latdist2-latdist1)/(t2-t1).
  set dVer to (verdist2-verdist1)/(t2-t1).
  set dtar to (tardist2-tardist1)/(t2-t1).





  if abs(latdist2) > 125 {
    set lateralVsetpoint to -4*transVelocity*latdist2/abs(latdist2).
    set Linput to (lateralVsetpoint-dLat)*kpv.
  } else if abs(latdist2) > 50 {
    set lateralVsetpoint to -2*transVelocity*latdist2/abs(latdist2).
    set Linput to (lateralVsetpoint-dLat)*kpv.
  } else if abs(latdist2) > 10 {
    set lateralVsetpoint to -transVelocity*latdist2/abs(latdist2).
    set Linput to (lateralVsetpoint-dLat)*kpv.
  } else if abs(latdist2) > 5 {
    set lateralVsetpoint to -0.5*transVelocity*latdist2/abs(latdist2).
    set Linput to (lateralVsetpoint-dLat)*kpv.
    set ILat to 0.
  } else {
    set ILat to (latdist2*(t2-t1))+ILat.
    set Linput to (kps*latdist2)+(kd*dLat)+(KIs*ILat).
  }

  if abs(verdist2) > 125 {
    set verticalVsetpoint to -4*transVelocity*verdist2/abs(verdist2).
    set Vinput to (verticalVsetpoint-dVer)*kpv.
  } else if abs(verdist2) > 50 {
    set verticalVsetpoint to -2*transVelocity*verdist2/abs(verdist2).
    set Vinput to (verticalVsetpoint-dVer)*kpv.
  } else if abs(verdist2)>10 {
    set verticalVsetpoint to -transVelocity*verdist2/abs(verdist2).
    set Vinput to (verticalVsetpoint-dVer)*kpv.
  } else if abs(verdist2) > 5 {
    set verticalVsetpoint to -0.5*transVelocity*verdist2/abs(verdist2).
    set Vinput to (verticalVsetpoint-dVer)*kpv.
    set IVer to 0.
  } else {
    set IVer to (Verdist2*(t2-t1))+IVer.
    set Vinput to (kps*verdist2)+(kd*dVer)+(KIs*IVer).
  }

  if abs(tardist2) > 125 {
    set targetVsetpoint to -2*transVelocity.
    set Tinput to (targetVsetpoint-dtar)*kpv.
  } else if abs(tardist2) > 35 {
    set targetVsetpoint to -1*transVelocity.
    set Tinput to (targetVsetpoint-dTar)*kpv.
  } else {
    set targetVsetpoint to -0.5*transVelocity.
    set Tinput to (targetVsetpoint-dTar)*kpv.
  }

  set ship:control:fore to Tinput.
  set ship:control:starboard to Linput.
  set ship:control:top to Vinput.

}

ag6 off.