ROSBuzz_MISTLab/buzz_scripts/include/act/states.bzz

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

TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps

path_it = 0
pic_time = 0
g_it = 0

# Core state function when on the ground
function turnedoff() {
    BVMSTATE = "TURNEDOFF"
}

# Core state function when hovering
function idle() {
    BVMSTATE = "IDLE"
}

# Core state function to launch the robot: takeoff and wait for others, or stop (land)
function launch() {
	BVMSTATE = "LAUNCH"
	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
		homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
		if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
			barrier_ready(22)
		} else {
			log("Altitude: ", pose.position.altitude)
			neighbors.broadcast("cmd", 22)
			uav_takeoff(TARGET_ALTITUDE)
		}
	} else {
		barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
		barrier_ready(22)
	}
}

# Launch function version without the timeout and stop state.
function launch_switch() {
	BVMSTATE = "LAUNCH_SWITCH"
	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
		homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
		if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 22)
			barrier_ready(22)
		} else {
			log("Altitude: ", pose.position.altitude)
			neighbors.broadcast("cmd", 22)
			uav_takeoff(TARGET_ALTITUDE)
		}
	} else {
		barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 22)
		barrier_ready(22)
	}
}

gohomeT=100
# State function to go back to ROSBuzz recorded home GPS position (at takeoff)
function goinghome() {
  BVMSTATE = "GOHOME"
	if(gohomeT > 0) {	# TODO: Make a real check if home is reached
		gohome()
		gohomeT = gohomeT - 1
	} else
  	BVMSTATE = AUTO_LAUNCH_STATE
}

# Core state function to stop and land.
function stop() {
  BVMSTATE = "STOP"
	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
		neighbors.broadcast("cmd", 21)
		uav_land()
		if(pose.position.altitude <= 0.2) {
			BVMSTATE = "TURNEDOFF"
			#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
			#barrier_ready(21)
		}
	} else {
			BVMSTATE = "TURNEDOFF"
			#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
			#barrier_ready(21)
	}
}

# State function for individual waypoint control
firsttimeinwp = 1
function indiWP() {
	if(firsttimeinwp) {
		nei_goal_listen()
		storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
		firsttimeinwp = 0
	}
	BVMSTATE = "INDIWP"
	if(rc_goto.id != -1) {
		log(rc_goto.id)
		if(rc_goto.id == id) {
			storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
		} else 
			bd_goal()
	}

	goto_gps(nil)

}

# State function to take a picture from the camera server (developed by HS)
function take_picture() {
  BVMSTATE="PICTURE"
  setgimbal(0.0, 0.0, -90.0, 20.0)
	if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
	  takepicture()
  } else if(pic_time>=PICTURE_WAIT) { # wait for the picture
    BVMSTATE="IDLE"
    pic_time=0
  }
  pic_time=pic_time+1
}

# State function to follow a moving attractor (GPS sent from a user phone)
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"
	}
}

# State function to 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)
}

# State fucntion to circle all together around centroid
function pursuit() {
  BVMSTATE="PURSUIT"
	rd = 20.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)
	var sqr = (r-rd)*(r-rd)+pc*pc*r*r
    gamma = vd / math.sqrt(sqr)
	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    = 30.0
function lj_magnitude(dist, target, epsilon) {
	lj = -(epsilon / dist) * ((target / dist)^4 - (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)
}

# Sate function that calculates and actuates the flocking interaction
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())
	accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
  goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
}

# Custom state function for the developer to play with
function cusfun(){
	BVMSTATE="CUSFUN"
	log("cusfun: yay!!!")
	LOCAL_TARGET = math.vec2.new(5 ,0 )
	local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
    if(math.vec2.length(local_set_point) > GOTODIST_TOL){
		local_set_point = LimitSpeed(local_set_point, 1.0)
		goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
    }
    else{
    	log("TARGET REACHED")
    }
}