ROSBuzz_MISTLab/buzz_scripts/testflocksim.bzz

include "vec2.bzz"
include "update.bzz"
include "barrier.bzz"	# don't use a stigmergy id=11 with this header.
include "uavstates.bzz"	# require an 'action' function to be defined here.
include "vstigenv.bzz"

TARGET_ALTITUDE = 10.0
 
# Lennard-Jones parameters
TARGET     = 12.0
EPSILON    = 14.0
 
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
  #return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
}
 
# 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)
}

function user_attract(t) {
	fus = math.vec2.new(0.0, 0.0)
	if(size(t)>0) {
		foreach(t, function(u, tab) {
				#log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
				fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b))
			})
		math.vec2.scale(fus, 1.0 / size(t))
	}
	#print("User attract:", fus.x," ", fus.y, " [", size(t), "]")
	return fus
}
 
# Calculates and actuates the flocking interaction
function action() {
  statef=action
  # Calculate accumulator
  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
  if(neighbors.count() > 0)
    accum = math.vec2.scale(accum, 1.0 / neighbors.count())

  accum = math.vec2.add(accum, user_attract(users.dataL))
  accum = math.vec2.scale(accum, 1.0 / 2.0)

  if(math.vec2.length(accum) > 1.0) {
  	  accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
  }

  # Move according to vector
  print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
  uav_moveto(accum.x, accum.y)
  UAVSTATE = "LENNARDJONES"

#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
#  	timeW =0
#  	statef=land
#  	} else if(timeW>=WAIT_TIMEOUT/2) {
#		  UAVSTATE ="GOEAST"
#  	timeW = timeW+1
#  	uav_moveto(0.0,5.0)
#  	} else {
#		  UAVSTATE ="GONORTH"
#  	timeW = timeW+1
#  	uav_moveto(5.0,0.0)
#  	}
}


########################################
#
# MAIN FUNCTIONS
#
########################################

# Executed once at init time.
function init() {
	uav_initswarm()

	vt = stigmergy.create(5)
	t = {}
	vt.put("p",t)
}

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

	statef()
	log("Current state: ", UAVSTATE)
	log("Swarm size: ",ROBOTS)
	
	# Read a value from the structure
		#		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)
}
 
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
 
# Executed once at the end of experiment.
function destroy() {
}