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

set lateraloffset   to 0.
set verticaloffset to 0.
rcs on.
sas off.
gear off.
lock steering to prograde + r(0,0,180).

set latint to 0.
set verint to 0.
set maneuverSpeed to .7.

function lateraldist {
	set uct to vcrs(ship:up:vector,target:position). //up cross target
	set ev to vcrs(ship:body:position,ship:north:vector).

	if vang(uct,ship:north:vector) > 90 {
		set thdg to vang(uct,ev).
	} else {
		set thdg to 360-vang(uct,ev). //heading of the target vector
	}.
  if thdg > 360 {
    set thdg to thdg-360.
  }.
  set ucv to vcrs(ship:up:vector,ship:velocity:orbit).
  if vang(ucv,ship:north:vector) > 90 {
		set vhdg to vang(ucv,ev).
	} else {
		set vhdg to 360-vang(ucv,ev). //heading of the velocity vector
	}

  if vhdg-thdg > 180{
    set hdgdiff to vhdg-thdg-360.
  }. else {
    set hdgdiff to vhdg-thdg.
  }.
  set latdist to sin(hdgdiff)*target:position:mag.
	return latdist.
}.
function verticaldist {
  set velangle to vang(ship:velocity:orbit,ship:up:vector).
  set targangle to vang(target:position,ship:up:vector).
  set pitchdiff to velangle-(targangle-verticaloffset).
  set vdist to sin(pitchdiff)*target:position:mag.
	return vdist.
}.
function depthdist {
	set targpos to (target:position+ship:body:position).
	set ang to vang(targpos,ship:body:position)*pi/180.
	set ddist to ang*ship:body:position:mag.
	return ddist.
}
function lateralinputFunction {
	if abs(lateraldistance2) > 10 {
		set vlat to (lateralDistance2-lateralDistance1)/(dt).
		if lateralDistance2 > 0 {
			set ms to -maneuverSpeed.
		}. else {
			set ms to  maneuverSpeed.
		}
		set vlatError to ms-vlat.
		set latinput  to kpv*vlatError.
	}. else {
		set slatError to lateralDistance2.
		set vlat to (lateralDistance2-lateralDistance1)/(dt).
		set latinput to -((kps*lateralDistance2)+(kds*vlat)+(kis*latint)).
	}.
	return latinput.
}
function verticalinputFunction {
	if abs(verticalDistance2+extraH) > 10 {
		set vver to (verticalDistance2-verticalDistance1)/(dt).
		if verticaldistance2+extraH > 0 {
			set ms to -maneuverSpeed.
		}. else {
			set ms to  maneuverSpeed.
		}
		set vverError to ms-vver.
		set vertinput  to kpv*vverError.
    ///////////////////////////////////////////
	}. else {
		set sverError to (verticaldistance2+extraH).
		set vver to (verticalDistance2-verticalDistance1)/(dt).
		set vertinput to -((kps*sverError)+(kds*vver)+(verint*kis)).
	}.
	return vertinput.
}.
function depthinputFunction {
	if abs(depthDistance2-depthoffset) > 10 {
		set vdep to (depthDistance2-depthDistance1)/(dt).
		if depthdistance2-depthoffset > 0 {
			set ms to -maneuverSpeed.
		}. else {
			set ms to  maneuverSpeed.
		}
		set vdepError to ms-vdep.
		set depinput  to -kpv*vdepError.
	///////////////////////////////////////////
	}. else {
		set sdepError to depthdistance2-depthoffset.
		set vdep to ((depthDistance2)-(depthDistance1))/(dt).
		set depinput to ((kps*sdepError)+(kds*vdep)).
	}.
	return depinput.
}

set kpv to 1.
set kps to .3.
set kds to 1.7.
set kis to 0.01.

set latinput to 0.
set verticalinput to 0.
set depthinput to 0.
set extraH to 0.
set vver to 0.

set depthoffset to target:position:mag.

//Approaching for docking

until ag1 {
	if 2 > 1 {
		set lateralDistance1  to lateraldist().
		set verticalDistance1 to verticaldist().
		set depthDistance1    to target:position:mag.
		set t1                to time:seconds.
		print "Lateral Difference  "+round(lateralDistance1)+" m".
		print "Vertical Difference "+round(verticalDistance1)+" m".
		print "Depth Difference    "+round(depthDistance1)+" m".
		print " ".
		print "Lateral Input  "+round(100*latinput).
		print "Vertical Input "+round(100*verticalinput).
		print "Depth Input "+round(100*depthinput).
		print " ".
		print "Depth Offset "+round(depthoffset)+" m".
		wait 0.2.
		clearscreen.
		set lateralDistance2  to lateraldist().
		set verticalDistance2 to verticaldist().
		set depthDistance2    to target:position:mag.
		set t2                to time:seconds.
		set dt                to t2-t1.
	}. //this is just so i can hide it easily
	////////////////////////////////////////////
	set lateralinput to lateralinputFunction().
	set verticalinput to verticalinputfunction().
	set depthinput to depthinputfunction().
	///////////////////////////////////////////
	set ship:control:starboard to lateralinput.
	set ship:control:top       to verticalinput.
	set ship:control:fore      to depthinput.
}.
ag1 off.

clearscreen.
print "Go for docking".

set depthoffset to 0.
set t0 to time:seconds.
set latint to 0.
set verint to 0.

until hastarget = false {
	if hastarget = true {
		set lateralDistance1  to lateraldist().
		set verticalDistance1 to verticaldist().
		set depthDistance1    to target:position:mag.
		set t1                to time:seconds.
		print "Go for Docking".
		print " ".
		print "Lateral Difference  "+lateralDistance1+" m".
		print "Vertical Difference "+verticalDistance1+" m".
		print "Depth Difference    "+round(depthDistance1)+" m".
		print " ".
		print "Lateral Input  "+round(100*latinput).
		print "Vertical Input "+round(100*verticalinput).
		print "Depth Input "+round(100*depthinput).
		print " ".
		print "Depth Offset "+round(depthoffset)+" m".
		wait 0.2.
		clearscreen.
		if hastarget = true {
			set lateralDistance2  to lateraldist().
			set verticalDistance2 to verticaldist().
			set depthDistance2    to target:position:mag.
			set t2                to time:seconds.
			set dt                to t2-t1.
		}.
	}. //this is just so i can hide it easily
	////////////////////////////////////////////
	//giving the integral gain something to multiply
	set latint to (((lateraldistance2+lateraldistance1)/2)*dt)+latint.
	set verint to (((verticaldistance2+verticaldistance1)/2)*dt)+verint.

	set lateralinput to lateralinputFunction().
	set verticalinput to verticalinputfunction().
	set depthinput to depthinputfunction().
	///////////////////////////////////////////

	set ship:control:starboard to lateralinput.
	set ship:control:top       to verticalinput.
	set ship:control:fore      to depthinput.
	if depthdistance2 < 20 {
		set maneuverspeed to 0.3.
	}.
}.
clearscreen.
set ship:control:neutralize to true.
print " ".
print "Docked!".
print " ".