include "update.bzz"
include "act/states.bzz"
include "vstigenv.bzz"

# State launched after takeoff
AUTO_LAUNCH_STATE = "ACTION"

#####
# 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_swarm()

	TARGET_ALTITUDE = 25.0 # m

	# start the swarm command listener
	nei_cmd_listen()
    
    # Starting state: TURNEDOFF to wait for user input.
	BVMSTATE = "TURNEDOFF"
}

function action() {
    BVMSTATE = "ACTION"
    # do some actions....
}

# Executed at each time step.
function step() {
    # listen to Remote Controller
	rc_cmd_listen()
    
    #
	# State machine
	#
	if(BVMSTATE=="TURNEDOFF")
		statef=turnedoff
	else if(BVMSTATE=="STOP")	# ends on turnedoff
		statef=stop
	else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
		statef=launch
	else if(BVMSTATE=="IDLE")
		statef=idle
	else if(BVMSTATE=="ACTION")
		statef=action

	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() {
}