ROSBuzz_MISTLab/buzz_scripts/include/act/barrier.bzz

########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################

#
# Constants
#
BARRIER_TIMEOUT = 600 # in steps
BARRIER_VSTIG = 80
timeW = 1
barrier = nil
hvs = 0;


#
# Sets a barrier
#
function barrier_create() {
  timeW = 1
  if(barrier==nil)
    barrier = stigmergy.create(BARRIER_VSTIG)
}

function barrier_set(threshold, transf, resumef, bc) {
  statef = function() {
    barrier_wait(threshold, transf, resumef, bc);
  }
  BVMSTATE = "BARRIERWAIT"
  barrier_create()
}

#
# Make yourself ready
#
function barrier_ready(bc) {
  #log("BARRIER READY -------")
  barrier.put(id, bc)
  barrier.put("d", 0)
  barrier.put("n", 1)
}

#
# Executes the barrier
#
function barrier_wait(threshold, transf, resumef, bc) {
  if(barrier==nil) #failsafe
    barrier_create()
  
  barrier.put(id, bc)
  if(barrier.get("n")<threshold)
    barrier.put("n", threshold)
  if(threshold>1) {
    neighbors.foreach(function (nei,data){
      barrier.get(nei)
    })
  }

  # Not to miss any RC inputs, for waypoints, potential and deploy states
	check_rc_wp()

  #log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
  if(barrier.size()-2 >= barrier.get("n")) {
    if(barrier_allgood(barrier,bc)) {
      barrier.put("d", 1)
      timeW = 0
      BVMSTATE = transf
    } else 
      barrier.put("d", 0)
  }

  if(timeW >= BARRIER_TIMEOUT) {
    log("------> Barrier Timeout !!!!")
    #barrier = nil
    timeW = 0
    BVMSTATE = resumef
  } else if(timeW % 10 == 0 and bc > 0)
    neighbors.broadcast("cmd", bc)

  timeW = timeW+1
}

function barrier_wait_graph(threshold, transf, resumef, bc) {
  if(barrier==nil) #failsafe
    barrier_create()
  
  barrier.put(id, bc)
  if(barrier.get("n")<threshold)
    barrier.put("n", threshold)
  if(threshold>1) {
    neighbors.foreach(function (nei,data){
      barrier.get(nei)
    })
  }

  # Not to miss any RC inputs, for waypoints, potential and deploy states
	check_rc_wp()

  #log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
  if(barrier.size()-2 >= barrier.get("n")) {
    if(barrier_allgood(barrier,bc)) {
      barrier.put("d", 1)
      timeW = 0
      GRAPHSTATE = transf
    } else 
      barrier.put("d", 0)
  }

  if(timeW >= BARRIER_TIMEOUT) {
    log("------> Barrier Timeout !!!!")
    #barrier = nil
    timeW = 0
    GRAPHSTATE = resumef
  } else if(timeW % 10 == 0 and bc > 0)
    neighbors.broadcast("cmd", bc)

  timeW = timeW+1
}

# Check all members state
function barrier_allgood(barrier, bc) {
  barriergood = 1
  barrier.foreach(function(key, value, robot){
      barrier.get(key)
      #log("VS entry : ", key, " ", value, " ", robot)
      if(key == "d"){
        if(value == 1)
          return 1
      } else if(key!="n" and value != bc)
        barriergood = 0
    })
  return barriergood
}