ROSBuzz_MISTLab/buzz_scripts/include/act/states.bzz

########################################
#
# 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)>GOTO_MAXVEL) { # limit velocity
		m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
		goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
	} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
		transf()
	else
		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)
    math.vec2.scale(centroid, 1.0 / neighbors.count())
  if(math.vec2.length(centroid)>GOTO_MAXVEL)
  	centroid = math.vec2.scale(centroid, GOTO_MAXVEL/math.vec2.length(centroid))
  goto_abs(centroid.x, centroid.y, 0.0, 0.0) 
}

# follow one another
rotang = 0.0
function pursuit() {
  BVMSTATE="PURSUIT"
  insight = 0
  leader = math.vec2.newp(0.0, 0.0)
  var cmdbin = math.vec2.newp(0.0, 0.0)
  neighbors.foreach(function(rid, data) {
  	if(data.distance < 11.0 and data.azimuth < 3.2 and data.azimuth > 2.8) {
		  insight = 1
		  leader = math.vec2.newp(data.distance, data.azimuth)
	  }
  })
  if(insight == 1) {
	log("Leader in sight !")
  	#cmdbin = math.vec2.newp(lj_magnitude(math.vec2.length(leader), 3.0, 0.01), math.vec2.angle(leader))
	cmdbin = math.vec2.newp(2.0, math.vec2.angle(leader))
  } else {
	rotang = rotang + math.pi/60
	cmdbin = math.vec2.newp(2.0, rotang)
  }
  goto_abs(cmdbin.x, cmdbin.y, 0.0, rotang)
}

# Lennard-Jones interaction magnitude
TARGET     = 8.0
EPSILON    = 0.000001
function lj_magnitude(dist, target, epsilon) {
  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
}
 
# 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
function formation() {
  BVMSTATE="FORMATION"
  # Calculate accumulator
  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
  if(neighbors.count() > 0)
    math.vec2.scale(accum, 1.0 / neighbors.count())
  if(math.vec2.length(accum)>GOTO_MAXVEL*10)
  	accum = math.vec2.scale(accum, 10*GOTO_MAXVEL/math.vec2.length(accum))
  goto_abs(accum.x, accum.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
				#  }) 
				}
    })
}