function horizontalLine {
	parameter line.
	local i is 0.
	until i = terminal:width {
		print "-" at (i,line).
		set i to i + 1.
	}
}
function vecs_clear {
	if vecs:length > 0 {
		for vd in vecs {
			set vd:SHOW TO false.
		}
		vecs:clear.
	}
}
// set [variable] to vecs_add([position],[vector],[color],[string]).
// returns: list index.
// example:
//  Create a vecdraw:
//  set velocityVec to vecs_add(ship:position,velocity:orbit,blue,round(velocity:orbit:mag) + " m/s").
//  Update it's starting position:
//  set vecs[velocityVec]:start to ship:position.
function vecs_add {
	parameter p,v,c,descr.
	vecs:add(VECDRAWARGS(p, v, c, descr, 1, true)).
	return vecs:length - 1.
}

global vecs is list().
if vecs:length > 0 vecs_clear().

////////////////////// end of library functions ////////////////////////////


clearscreen. ag1 off.
print " ".
print " ".
print " ".
horizontalLine(3).
print " ".
print " ".

//vecs initialize
local northV is vxcl(up:vector,north:vector):normalized.
local eastV is -vcrs(northV,up:vector):normalized.
//local markN is vecs_add(v(0,0,0),northV*3,rgb(0,0.8,1),"N").
//local markE is vecs_add(v(0,0,0),eastV*3,rgb(0,0.5,1),"E").
//local markCP is vecs_add(v(0,0,0),v(0,0,0),rgb(1,1,0),"CP").
local markGeo is vecs_add(v(0,0,0),v(0,0,0),rgb(1,1,1),"GeoPos").
//local markCPn is vecs_add(v(0,0,0),v(0,0,0),rgb(1,0.5,0),"CPn").
//local markCPe is vecs_add(v(0,0,0),v(0,0,0),rgb(1,0.5,0),"CPe").
local markPole is vecs_add(v(0,0,0),v(0,0,0),rgb(1,0.0,0),"POLE").
local markLaser is vecs_add(v(0,0,0),v(0,0,0),rgb(1,0,0),"").
local markNormal is vecs_add(v(0,0,0),v(0,0,0),rgb(1,0,1),"").
local markP1 is vecs_add(v(0,0,0),v(0,0,0),rgb(1,1,1),"").
local markP2 is vecs_add(v(0,0,0),v(0,0,0),rgb(1,1,1),"").

function getPlaneNormal {
	parameter p1,p2,p3.
	local vec1 is p2-p1.
	local vec2 is p3-p1.
	return vcrs(vec1,vec2):normalized.
}

function getGeoPosition {
	parameter checkpos. //position relative to ship at which to check terrain height
	local northV is vxcl(up:vector,north:vector):normalized.
	local eastV is -vcrs(north:vector,up:vector):normalized.

	local checkposN is northV * vdot(checkpos, northV). // above split into north and east vectors
	local checkposE is eastV * vdot(checkpos, eastV).
	local angN is vang(up:vector, checkposN-body:position).
	local angE is vang(vxcl(body:angularvel,up:vector), vxcl(body:angularvel,(checkposE-body:position))).

	if vdot(checkpos, northV) < 0 set angN to angN * -1. //negative lat change if heading south
	if vdot(checkpos, eastV) < 0 set angE to angE * -1. //negative lng change if heading west

	//checkposition's lat and lng
	local cpLat is latitude + angN.
	local cpLng is longitude + angE.
	//overshoot protection:
	//if cpLat > 90 { set cpLat to 90 - (cpLat-90). set cpLng to 1 * cpLng. }
	//else if cpLat < -90 { set cpLat to -90 - (cpLat+90). set cpLng to 1 * cpLng.}
	if cpLng > 180 set cpLng to -180 + (cpLng - 180).
	else if cpLng < -180 set cpLng to 180 + (cpLng + 180).

	//return geoposition
	return latlng(cpLat,cpLng).
}

until ag1 {

	local horV is vxcl(up:vector,velocity:surface).
	local checkpos is ship:position + (horV * 10). //position at which to check terrain height

	local cpGeoPos is getGeoPosition(checkpos). //geoposition structure
	local cpHeight is cpGeoPos:terrainheight. //terrain height above/below sealevel

	//terrain normal stuff
	local posUp is (cpGeoPos:position - body:position):normalized.
	local forwardV is vxcl(posUp,velocity:surface):normalized.
	local rightV is vcrs(forwardV,posUp):normalized.

	local point1 is getGeoPosition(checkpos + forwardV * 45 + rightV * 30).
	local point2 is getGeoPosition(checkpos + forwardV * 45 - rightV * 30).
	local terrainNormal is getPlaneNormal(cpGeoPos:position,point1:position,point2:position).


	//vecs debug
	//set vecs[markN]:start to facing:starvector:normalized * 10.
	//set vecs[markE]:start to facing:starvector:normalized * 10.

	//set vecs[markCP]:vec to checkpos.
	//set vecs[markCPn]:vec to checkposN.
	//set vecs[markCPe]:vec to checkposE.

	set vecs[markPole]:start to body:position - (body:angularvel:normalized * (body:radius + 50000)).
	set vecs[markPole]:vec to body:angularvel:normalized * (body:radius*2 + 100000).

	set vecs[markGeo]:vec to posUp * 5. //(checkpos - cpGeoPos:position):mag.
	set vecs[markGeo]:start to cpGeoPos:position.
	set vecs[markGeo]:label to round(cpHeight-altitude) + "m".

	set vecs[markP1]:vec to posUp * 5.
	set vecs[markP2]:vec to posUp * 5.
	set vecs[markP1]:start to point1:position.
	set vecs[markP2]:start to point2:position.

	set vecs[markLaser]:vec to cpGeoPos:position.
	set vecs[markNormal]:vec to terrainNormal*300.
	set vecs[markNormal]:start to cpGeoPos:position.
	set vecs[markNormal]:label to round(vang(posUp,terrainNormal),1) + " deg".

	//Terminal window readouts
	print "Lat: " + latitude + "    " at (0,0).
	//print "Ang: " + angN + "     " at (round(terminal:width/2),0).

	print "Lng: " + longitude + "    " at (0,1).
	//print "Ang: " + angE + "     " at (round(terminal:width/2),1).
	//---
	print "Alt Rad: " + round(alt:radar/1000,3) + "km    " at (0,4).

	print "Alt CP:  " + round((altitude-cpHeight)/1000,3) + "km    " at (0,5).
	print "Ter Hgt:  " + round(cpHeight/1000,3) + "km   " at (round(terminal:width/2),5).



	wait 0.
}
vecs_clear().