ROSBuzz_MISTLab/buzz_scripts/main.bzz

include "update.bzz"
# don't use a stigmergy id=11 with this header, for barrier 
# it requires an 'action' function to be defined here.
include "act/states.bzz"
include "utils/table.bzz"
include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz"
include "taskallocate/bidding.bzz"
include "vstigenv.bzz"
#include "timesync.bzz"
include "utils/takeoff_heights.bzz"

#State launched after takeoff

AUTO_LAUNCH_STATE = "INDIWP"
TARGET     = 9.0
EPSILON    = 30.0
ROOT_ID = 3
graph_id = 3
graph_loop = 0
LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE.

#####
# Vehicule type:
# 0 -> outdoor flying vehicle
# 1 -> indoor flying vehicle
# 2 -> outdoor wheeled vehicle
# 3 -> indoor wheeled vehicle
V_TYPE = 0


# Executed once at init time.
function init() {
  	init_stig()
	init_swarm()
	init_bidding()
	
	TARGET_ALTITUDE = takeoff_heights[id]
	
	# start the swarm command listener
	nei_cmd_listen()

    # Starting state: TURNEDOFF to wait for user input.
	BVMSTATE = "TURNEDOFF"
}

# Executed at each time step.
function step() {

	# listen to Remote Controller
	rc_cmd_listen()

	# update the vstig (status/net/batt/...)
  	uav_updatestig()

	#
	# State machine
	#
	if(BVMSTATE=="TURNEDOFF")
		statef=turnedoff
	else if(BVMSTATE=="CUSFUN")
		statef=cusfun
	else if(BVMSTATE=="STOP")	# ends on turnedoff
		statef=stop
	else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
		if(LAND_AFTER_BARRIER_EXPIRE == 1)
			statef=launch
		else
			statef=launch_switch
	else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
		statef=goinghome
	else if(BVMSTATE=="INDIWP")
		statef=indiWP
	else if(BVMSTATE=="IDLE")
		statef=idle
	else if(BVMSTATE=="AGGREGATE")
		statef=aggregate
	else if(BVMSTATE=="FORMATION")
		statef=formation
	else if(BVMSTATE=="PURSUIT")
		statef=pursuit
	else if(BVMSTATE=="TASK_ALLOCATE")	# or bidding ?
		statef=resetGraph
	else if(BVMSTATE=="BIDDING")	# check the absolute path of the waypointlist csv file in bidding.bzz
		statef=bidding
	else if(BVMSTATE=="GRAPH_FREE")
		statef=DoFree
	else if(BVMSTATE=="GRAPH_ASKING")
		statef=DoAsking
	else if(BVMSTATE=="GRAPH_JOINING")
		statef=DoJoining
	else if(BVMSTATE=="GRAPH_JOINED")
		statef=DoJoined
	else if(BVMSTATE=="GRAPH_LOCK")	# move all together (TODO: not tested), but auto-loop in graph list
		statef=DoLock
	else if(BVMSTATE=="PATHPLAN")	# ends on navigate, defined in rrtstar
		statef=rrtstar
	else if(BVMSTATE=="NAVIGATE")	# ends on idle, defined in rrtstar
		statef=navigate
	else if(BVMSTATE == "FOLLOW")	#TODO: not tested in new structure
		statef=follow
	else if(BVMSTATE == "PICTURE")	#TODO: not tested in new structure
		statef=take_picture
		

	statef()

	log("Current state: ", BVMSTATE)

}
 
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
 
# Executed once at the end of experiment.
function destroy() {
	close_bidding()
}