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

lock steering to prograde+r(0,0,180).
sas off.
wait 10.
unlock steering.
sas on.
set t0 to time:seconds.
set kuniverse:timewarp:warp to 1.
wait until time:seconds>(t0+10).
set t0 to time:seconds.
set kuniverse:timewarp:warp to 2.
wait until time:seconds > (t0+100).
set kuniverse:timewarp:warp to 3.
clearscreen.
print "Warping to stationkeeping burn".


//warping to similar altitude
wait until target:altitude-ship:altitude < 2000.
set kuniverse:timewarp:warp to 2.
wait until target:altitude-ship:altitude < 1000.
set kuniverse:timewarp:warp to 1.
until target:altitude-ship:altitude < 500 {
  wait 0.1.
  if ship:verticalspeed < 0 {
    break.
  }
}
set kuniverse:timewarp:warp to 0.

sas off.
lock steering to prograde + r(0,0,180).



set Vinput to 50. set Linput to 50.
set transVelocity to .5.
set kpv to -5. set kps to 0.5.
set kd to 5.
set dLat to 50.
set dVer to 50.
set breakloop to 0. set break1 to 0. set break2 to 0. set break3 to 0.

until breakloop > 3 { //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 sin(svh-tph)*target:position:mag.
  set verdist1 to ship:altitude-target:altitude.
  set t1 to time:seconds.
  set periodDiff to target:orbit:period-ship:orbit:period+.05.

  wait 0.1.
  clearscreen.
  print "Station Distance is  "+round(target:position:mag)+" m".
  print "Lateral Distance is  "+(round(1000*latdist1)/1000)+" m".
  print "Vertical Distance is "+(round(1000*verdist1)/1000)+" m".
  print "Period Difference is "+(round(1000*periodDiff)/1000)+" s".
  print " ".
  Print "Vertical Control Input: "+(round(100*Vinput)/100).
  print "Lateral Control Input:  "+(round(100*Linput)/100).
  print "Foreward Control Input: "+(round(100*periodDiff)/100).
  print " ".
  print "Lateral Velocity:  "+(round(1000000*dLat)/1000)+" mm/s".
  print "Vertical Velocity: "+(round(1000000*dVer)/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 sin(svh-tph)*target:position:mag.
  set verdist2 to ship:altitude-target:altitude.

  set t2 to time:seconds.
  set dLat to (latdist2-latdist1)/(t2-t1).
  set dVer to (verdist2-verdist1)/(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).
  }

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

  if abs(periodDiff) < 0.1 and abs(Linput) < 0.1 and abs(Vinput) < 0.1 {
    set break1 to 1.01.
  } else {
    set break1 to 0.
  }
  if abs(latdist2) < 0.5 and abs(verdist2) < 0.5 {
    set break2 to 1.01.
  } else {
    set break2 to 0.
  }
  if abs(dlat) < 0.002 and abs(dver) < 0.002 {
    set break3 to 1.01.
  } else {
    set break3 to 0.
  }

  set breakloop to break1+break2+break3.

}
set ship:control:neutralize to true.
ag5 off.


wait 10.
clearscreen.
print "Closing. Translating Downward".
set traveldist to target:position:mag-2000. //we want to maneuver 2km 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.
wait 5.
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.35)).
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.

set Vinput to 50. set Linput to 50.
set transVelocity to .5.
set kpv to -5. set kps to 0.5.
set kd to 5.
set dLat to 50.
set dVer to 50.
set breakloop to 0. set break1 to 0. set break2 to 0. set break3 to 0.

until breakloop > 3 { //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 sin(svh-tph)*target:position:mag.
  set verdist1 to ship:altitude-target:altitude.
  set t1 to time:seconds.
  set periodDiff to target:orbit:period-ship:orbit:period+.05.

  wait 0.1.
  clearscreen.
  print "Station Distance is  "+round(target:position:mag)+" m".
  print "Lateral Distance is  "+(round(1000*latdist1)/1000)+" m".
  print "Vertical Distance is "+(round(1000*verdist1)/1000)+" m".
  print "Period Difference is "+(round(1000*periodDiff)/1000)+" s".
  print " ".
  Print "Vertical Control Input: "+(round(100*Vinput)/100).
  print "Lateral Control Input:  "+(round(100*Linput)/100).
  print "Foreward Control Input: "+(round(100*periodDiff)/100).
  print " ".
  print "Lateral Velocity:  "+(round(1000000*dLat)/1000)+" mm/s".
  print "Vertical Velocity: "+(round(1000000*dVer)/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 sin(svh-tph)*target:position:mag.
  set verdist2 to ship:altitude-target:altitude.

  set t2 to time:seconds.
  set dLat to (latdist2-latdist1)/(t2-t1).
  set dVer to (verdist2-verdist1)/(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).
  }

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

  if abs(periodDiff) < 0.1 and abs(Linput) < 0.1 and abs(Vinput) < 0.1 {
    set break1 to 1.01.
  } else {
    set break1 to 0.
  }
  if abs(latdist2) < 0.5 and abs(verdist2) < 0.5 {
    set break2 to 1.01.
  } else {
    set break2 to 0.
  }
  if abs(dlat) < 0.002 and abs(dver) < 0.002 {
    set break3 to 1.01.
  } else {
    set break3 to 0.
  }

  set breakloop to break1+break2+break3.

}
set ship:control:neutralize to true.


wait 10.
clearscreen.
print "Closing again. Translating Downward".
set traveldist to target:position:mag-250. //we want to maneuver 2km 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.
wait 5.
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.35)).
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 Third Stationkeeping Burn".
set ship:control:top to 1.
wait until (target:altitude-ship:altitude)<10.

set Vinput to 50. set Linput to 50.
set transVelocity to .5.
set kpv to -5. set kps to 0.5.
set kd to 5.
set dLat to 50.
set dVer to 50.
set breakloop to 0. set break1 to 0. set break2 to 0. set break3 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 sin(svh-tph)*target:position:mag.
  set verdist1 to ship:altitude-target:altitude.
  set t1 to time:seconds.
  set periodDiff to target:orbit:period-ship:orbit:period+.05.

  wait 0.1.
  clearscreen.
  print "Station Distance is  "+round(target:position:mag)+" m".
  print "Lateral Distance is  "+(round(1000*latdist1)/1000)+" m".
  print "Vertical Distance is "+(round(1000*verdist1)/1000)+" m".
  print "Period Difference is "+(round(1000*periodDiff)/1000)+" s".
  print " ".
  Print "Vertical Control Input: "+(round(100*Vinput)/100).
  print "Lateral Control Input:  "+(round(100*Linput)/100).
  print "Foreward Control Input: "+(round(100*periodDiff)/100).
  print " ".
  print "Lateral Velocity:  "+(round(1000000*dLat)/1000)+" mm/s".
  print "Vertical Velocity: "+(round(1000000*dVer)/1000)+" mm/s".
  print " ".
  print "Press action group 6 to stop station keeping".

  set svh to vectorheading(ship:velocity:orbit). //chaser velocity heading
  set tph to vectorheading(target:position). //target position heading
  set latdist2 to sin(svh-tph)*target:position:mag.
  set verdist2 to ship:altitude-target:altitude.

  set t2 to time:seconds.
  set dLat to (latdist2-latdist1)/(t2-t1).
  set dVer to (verdist2-verdist1)/(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).
  }

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

  if abs(periodDiff) < 0.1 and abs(Linput) < 0.1 and abs(Vinput) < 0.1 {
    set break1 to 1.01.
  } else {
    set break1 to 0.
  }
  if abs(latdist2) < 0.5 and abs(verdist2) < 0.5 {
    set break2 to 1.01.
  } else {
    set break2 to 0.
  }
  if abs(dlat) < 0.002 and abs(dver) < 0.002 {
    set break3 to 1.01.
  } else {
    set break3 to 0.
  }

  set breakloop to break1+break2+break3.

}
ag6 off.
clearscreen.
print "Please run docking.ks file".
set ship:control:neutralize to true.