clearscreen.
set mu to 3.986e14.
set g to 9.81.
set pi to 3.1415926535897932384626433832795028841971693993751058209749445923.
set transThrust to .26.  //.530 for 3 clowns
set foreThrust to .52. //1.06 for 3 clowns
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.
}

sas off.
rcs on.
lock steering to prograde + 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.1*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.