########################################
#
# FLIGHT-RELATED FUNCTIONS
#
########################################
include "utils/vec2.bzz"
include "act/barrier.bzz"
include "utils/conversions.bzz"

TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 2.5 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.5  # m.
GOTOANG_TOL = 0.1 # rad.
path_it = 0
graphid = 0
pic_time = 0
g_it = 0

function turnedoff() {
    BVMSTATE = "TURNEDOFF"
}

function idle() {
    BVMSTATE = "IDLE"
}

function launch() {
	BVMSTATE = "LAUNCH"
	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
		homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
		if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
			barrier_ready()
		} else {
			log("Altitude: ", pose.position.altitude)
			neighbors.broadcast("cmd", 22)
			uav_takeoff(TARGET_ALTITUDE)
		}
	} else {
		barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
		barrier_ready()
	}
}

function stop() {
    BVMSTATE = "STOP"
	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
		neighbors.broadcast("cmd", 21)
		uav_land()
		if(flight.status != 2 and flight.status != 3) {
			barrier_set(ROBOTS,"TURNEDOFF","STOP")
			barrier_ready()
		}
	} else {
			barrier_set(ROBOTS,"TURNEDOFF","STOP")
			barrier_ready()
	}	
}

function take_picture() {
  BVMSTATE="PICTURE"
  uav_setgimbal(0.0, 0.0, -90.0, 20.0)
	if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
	  uav_takepicture()
  } else if(pic_time>=PICTURE_WAIT) { # wait for the picture
    BVMSTATE="IDLE"
    pic_time=0 
  }
  pic_time=pic_time+1
}

function goto_gps(transf) {
	m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.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.")
	else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
		transf()
	else {
		LimitSpeed(m_navigation, 1.0)
		goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
	}
}

function follow() {
	if(size(targets)>0) {
		BVMSTATE = "FOLLOW"
		attractor=math.vec2.newp(0,0)
		foreach(targets, function(id, tab) {
			force=(0.05)*(tab.range)^4
			attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
		})
		goto_abs(attractor.x, attractor.y, 0.0, 0.0)
	} else {
		log("No target in local table!")
		BVMSTATE = "IDLE"
	}
}

# converge to centroid
function aggregate() {
  BVMSTATE="AGGREGATE"
  centroid = neighbors.reduce(function(rid, data, centroid) {
	  centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth))
	  return centroid
  }, {.x=0, .y=0})
  if(neighbors.count() > 0)
    centroid = math.vec2.scale(centroid, 1.0 / neighbors.count())
	cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.0, id * 2 * math.pi / ROBOTS))
  cmdbin = LimitSpeed(cmdbin, 1.0/2.0)
  goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) 
}

# circle all together around centroid
function pursuit() {
  BVMSTATE="PURSUIT"
	rd = 15.0
	pc = 3.2
	vd = 15.0
  r_vec = neighbors.reduce(function(rid, data, r_vec) {
	  r_vec = math.vec2.add(r_vec, math.vec2.newp(data.distance, data.azimuth))
	  return r_vec
  }, {.x=0, .y=0})
  if(neighbors.count() > 0)
    r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
	r = math.vec2.length(r_vec)
	gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r)
	dr = -gamma * (r-rd)
	dT = gamma * pc
  vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
  vfg = LimitSpeed(vfg, 2.0)
  goto_abs(vfg.x, vfg.y, 0.0, 0.0) 
}

# Lennard-Jones interaction magnitude
TARGET     = 8.0
EPSILON    = 3.0
function lj_magnitude(dist, target, epsilon) {
	lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2)
  return lj
}
 
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
 
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
  return math.vec2.add(data, accum)
}
 
# Calculates and actuates the flocking interaction
old_lj = math.vec2.newp(0.0, 0.0)	# 1-step filter, to smoothen dir. changes
function formation() {
  BVMSTATE="FORMATION"
  # Calculate accumulator
  accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
  if(neighbors.count() > 0)
    accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count())
	old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0)
  goto_abs(old_lj.x, old_lj.y, 0.0, 0.0)
}

function rc_cmd_listen() {
    if(flight.rc_cmd==22) {
		log("cmd 22")
		flight.rc_cmd=0
		BVMSTATE = "LAUNCH"		
		neighbors.broadcast("cmd", 22)
	} else if(flight.rc_cmd==21) {
		log("cmd 21")
		flight.rc_cmd=0
		BVMSTATE = "STOP"
		neighbors.broadcast("cmd", 21)
	} else if(flight.rc_cmd==16) {
		flight.rc_cmd=0
		BVMSTATE = "PATHPLAN"
	} else if(flight.rc_cmd==400) {
		flight.rc_cmd=0
		uav_arm()
		neighbors.broadcast("cmd", 400)
	} else if (flight.rc_cmd==401){
		flight.rc_cmd=0
		uav_disarm()
		neighbors.broadcast("cmd", 401)
	} else if (flight.rc_cmd==666){
		flight.rc_cmd=0
		stattab_send()
	} else if (flight.rc_cmd==900){
		flight.rc_cmd=0
		BVMSTATE = "TASK_ALLOCATE"
		neighbors.broadcast("cmd", 900)
	} else if (flight.rc_cmd==901){
		flight.rc_cmd=0
		destroyGraph()
		BVMSTATE = "PURSUIT"
		neighbors.broadcast("cmd", 901)
	} else if (flight.rc_cmd==902){
		flight.rc_cmd=0
		destroyGraph()
		BVMSTATE = "AGGREGATE"
		neighbors.broadcast("cmd", 902)
	} else if (flight.rc_cmd==903){
		flight.rc_cmd=0
		destroyGraph()
		BVMSTATE = "FORMATION"
		neighbors.broadcast("cmd", 903)
	}
}

function nei_cmd_listen() {
    neighbors.listen("cmd",
			function(vid, value, rid) {
				print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
				if(value==22 and BVMSTATE!="BARRIERWAIT") {
		      		BVMSTATE = "LAUNCH"
				} else if(value==21 and BVMSTATE!="BARRIERWAIT") {
		      		BVMSTATE = "STOP"
				} else if(value==400 and BVMSTATE=="TURNEDOFF") {
					uav_arm()
				} else if(value==401 and BVMSTATE=="TURNEDOFF"){
					uav_disarm()
				} else if(value==900){	# Shapes
					BVMSTATE = "TASK_ALLOCATE"
				} else if(value==901){	# Pursuit
					destroyGraph()
					BVMSTATE = "PURSUIT"
				} else if(value==902){	# Agreggate
					destroyGraph()
					BVMSTATE = "AGGREGATE"
				} else if(value==903){	# Formation
					destroyGraph()
					BVMSTATE = "FORMATION"
				} else if(value==16 and BVMSTATE=="IDLE"){
				#  neighbors.listen("gt",function(vid, value, rid) {
				#    print("Got (", vid, ",", value, ") from robot #", rid)
				#    # if(gt.id == id) statef=goto
				#  }) 
				}
    })
}