ROSBuzz_MISTLab/buzz_scripts/include/vstigenv.bzz

########################################
#
# FLEET V.STIGMERGY-RELATED FUNCTIONS
#
########################################
#
# Constants
#
STATUS_VSTIG = 20
GROUND_VSTIG = 21
HIGHEST_ROBOTID = 14
WAIT4STEP = 10

#
# Init var
#
var v_status = {}
var v_ground = {}
b_updating = 0
vstig_counter = WAIT4STEP


function init_swarm() {
	s = swarm.create(1)
	s.join()
}

function init_stig() {
	v_status = stigmergy.create(STATUS_VSTIG)
	v_ground = stigmergy.create(GROUND_VSTIG)
}

function uav_updatestig() {
	# TODO: Push values on update only.
	if(vstig_counter<=0) {
		b_updating=1
		#var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status}
		ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status
	    log("Pushing ", ls, "on vstig with id:", id)
	    v_status.put(id, ls)
		vstig_counter=WAIT4STEP
	} else {
		b_updating=0
		vstig_counter=vstig_counter-1
	}
}

function unpackstatus(recv_value){
    gps=(recv_value-recv_value%1000000)/1000000
    recv_value=recv_value-gps*1000000
    batt=(recv_value-recv_value%1000)/1000
    recv_value=recv_value-batt*1000
    xbee=(recv_value-recv_value%10)/10
    recv_value=recv_value-xbee*10
    fc=recv_value
	log("- GPS ", gps)
	log("- Battery ", batt)
	log("- Xbee ", xbee)
	log("- Status ", fc)
}
	
function checkusers() {
	# Read a value from the structure
	if(size(users)>0)
		log("Got a user!")

#	log(users)      
#	users_print(users.dataG)
#	if(size(users.dataG)>0)
#		vt.put("p", users.dataG)
			
	# Get the number of keys in the structure
#	log("The vstig has ", vt.size(), " elements")
#	users_save(vt.get("p"))
#	table_print(users.dataL)
}

function users_save(t) {
	if(size(t)>0) {
		foreach(t, function(id, tab) {
				#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
				add_user_rb(id,tab.la,tab.lo)
			})
	}
}

# printing the contents of a table: a custom function
function usertab_print(t) {
  if(size(t)>0) {
		foreach(t, function(u, tab) {
				log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
			})
	}
}

function stattab_print() {
	if(v_status.size()>0) {
		if(b_updating==0) {
			u=0
			while(u<HIGHEST_ROBOTID){
				tab = v_status.get(u)
				if(tab!=nil)
					unpackstatus(tab)
				u=u+1
			}
		}
	}
}

function stattab_send() {
	if(v_status.size()>0) {
		if(b_updating==0) {
			u=0
			while(u<HIGHEST_ROBOTID){
				tab = v_status.get(u)
				if(tab!=nil){
					recv_value=tab
					gps=(recv_value-recv_value%1000000)/1000000
					recv_value=recv_value-gps*1000000
					batt=(recv_value-recv_value%1000)/1000
					recv_value=recv_value-batt*1000
					xbee=(recv_value-recv_value%10)/10
					recv_value=recv_value-xbee*10
					fc=recv_value
					add_neighborStatus(u,gps,batt,xbee,fc)
				}
				u=u+1
			}
		}
	}
}