GOTO_MAXVEL = 2.5 # m/steps
GOTO_MAXDIST = 250 # m.
GOTODIST_TOL = 0.4  # m.
GOTOANG_TOL = 0.1 # rad.

include "utils/field_config.bzz"

# Core naviguation function to travel to a GPS target location.
function goto_gps(transf) {
	m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0)
	#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
		log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
	else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
			transf()
	} else {
		m_navigation = LimitSpeed(m_navigation, 1.0)
		gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit[1].lat, GPSlimit[1].lng, 0), .2=vec_from_gps(GPSlimit[2].lat, GPSlimit[2].lng, 0), .3=vec_from_gps(GPSlimit[3].lat, GPSlimit[3].lng, 0), .4=vec_from_gps(GPSlimit[4].lat, GPSlimit[4].lng, 0)}
		#TODO: ENSURE THAT IF WE GO OUT WE CAN GET BACK IN!!!
		geofence(gf)
		if(CA_ON==1)
			m_navigation = LCA(m_navigation)
		if(CA_ON==2)
			m_navigation = RVO(m_navigation)
		goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
	}
}

function LimitSpeed(vel_vec, factor){
  if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor)
  	vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec))
	return vel_vec
}

# Core naviguation function to travel to provided GPS target location.
function goto_gps_in(transf, gps_to_go) {
	m_navigation = vec_from_gps(gps_to_go.latitude, gps_to_go.longitude, 0)
	print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
 	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
		log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
	else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
			;
			# transf()
	} else {
		#log("move to x", m_navigation.x," y ", m_navigation.y)
		m_navigation = LimitSpeed(m_navigation, 1.0)
		#log("after limit speed move to x", m_navigation.x," y ", m_navigation.y)
		goto_abs(m_navigation.x, m_navigation.y, gps_to_go.altitude - pose.position.altitude, 0.0)
	}
}