# listens to commands from the remote control (web, commandline, rcclient node, etc)
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) {
		flight.rc_cmd=0
		AUTO_LAUNCH_STATE = "TURNEDOFF"
		#barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
		#barrier_ready(21)
		BVMSTATE = "STOP"
		neighbors.broadcast("cmd", 21)
	} else if(flight.rc_cmd==20) {
		flight.rc_cmd=0
		AUTO_LAUNCH_STATE = "IDLE"
		barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
		barrier_ready(20)
		neighbors.broadcast("cmd", 20)
#	} else if(flight.rc_cmd==16) {
#		flight.rc_cmd=0
#		BVMSTATE = "PATHPLAN"
	} else if(flight.rc_cmd==400) {
		flight.rc_cmd=0
		arm()
		neighbors.broadcast("cmd", 400)
	} else if (flight.rc_cmd==401){
		flight.rc_cmd=0
		disarm()
		neighbors.broadcast("cmd", 401)
	} else if (flight.rc_cmd==666){
		flight.rc_cmd=0
		stattab_send()
	} else if (flight.rc_cmd==777){
		flight.rc_cmd=0
		reinit_time_sync()
		neighbors.broadcast("cmd", 777)
	}else if (flight.rc_cmd==900){
		flight.rc_cmd=0
		barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
		barrier_ready(900)
		neighbors.broadcast("cmd", 900)
	} else if (flight.rc_cmd==901){
		flight.rc_cmd=0
		destroyGraph()
		barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
		barrier_ready(901)
		neighbors.broadcast("cmd", 901)
	} else if (flight.rc_cmd==902){
		flight.rc_cmd=0
		destroyGraph()
		barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
		barrier_ready(902)
		neighbors.broadcast("cmd", 902)
	} else if (flight.rc_cmd==903){
		flight.rc_cmd=0
		destroyGraph()
		resetWP()
		barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
		barrier_ready(903)
		neighbors.broadcast("cmd", 903)
	} else if (flight.rc_cmd==904){
		flight.rc_cmd=0
		destroyGraph()
		barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
		barrier_ready(904)
		neighbors.broadcast("cmd", 904)
	}
}

# listens to neighbors broadcasting commands
function nei_cmd_listen() {
    neighbors.listen("cmd",
			function(vid, value, rid) {
				print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
				#if(BVMSTATE!="BARRIERWAIT") {
					if(value==22 and BVMSTATE=="TURNEDOFF") {
						BVMSTATE = "LAUNCH"
					}else if(value==20) {
						AUTO_LAUNCH_STATE = "IDLE"
						BVMSTATE = "GOHOME"
					} else if(value==21 and BVMSTATE!="TURNEDOFF") {
						BVMSTATE = "STOP"
					} else if(value==400 and BVMSTATE=="TURNEDOFF") {
						arm()
					} else if(value==401 and BVMSTATE=="TURNEDOFF"){
						disarm()
					} else if(value==777 and BVMSTATE=="TURNEDOFF"){
						reinit_time_sync()
						#neighbors.broadcast("cmd", 777)
					}else if(value==900){	# Shapes
						barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
						#barrier_ready(900)
						#neighbors.broadcast("cmd", 900)
					} else if(value==901 and BVMSTATE!="BARRIERWAIT"){	# Pursuit
						destroyGraph()
						barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
						#barrier_ready(901)
						#neighbors.broadcast("cmd", 901)
					} else if(value==902 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="WAYPOINT"){	# Waypoint
						destroyGraph()
						barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
						#barrier_ready(902)
						#neighbors.broadcast("cmd", 902)
					} else if(value==903 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="POTENTIAL"){	# Formation
						destroyGraph()
						resetWP()
						barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
						#barrier_ready(903)
						#neighbors.broadcast("cmd", 903)
					} else if(value==904 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="IDLE"){	# idle
						destroyGraph()
						barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
						#barrier_ready(904)
						#neighbors.broadcast("cmd", 904)
					} 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
					#  })
					}
				#}
    })
}

firsttimeinwp = 1
function check_rc_wp() {
	if(firsttimeinwp) {
		v_wp = stigmergy.create(WP_STIG)
		storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
		firsttimeinwp = 0
	}
	if(rc_goto.id != -1) {
		if(rc_goto.id == id) {
			wpreached = 0
			storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
			return
		} else {
			var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0)
			v_wp.put(rc_goto.id,ls)
			reset_rc()
		}
	}
}

function resetWP() {
	firsttimeinwp = 1
	if(v_wp!=nil)
		v_wp.foreach(function(key, value, robot){v_wp[key]=nil})
}