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.
 
# Lennard-Jones parameters
TARGET     = 12.0
EPSILON    = 10.0

#################################################
### UTILITY FUNCTIONS ###########################
#################################################

# Write a table as if it was a matrix
function write_knowledge(k, row, col, val) {
    var key = string.concat(string.tostring(row),"-",string.tostring(col))
    k[key] = val
    log("Writing knowledge:", val, " to ", row, " ", col)
}

# Read a table as if it was a matrix
function read_knowledge(k, row, col) {
    var key = string.concat(string.tostring(row),"-",string.tostring(col))
    if (k[key] == nil) {
        log("Warning: reading 'nil' value from the knowledge table at", row, " ", col, ", returning -1")
        return -1
    } else {
        return k[key]
    }
}

# Int to String 
function itos(i) {
    
    log("Use 'string.tostring(OJB)' instead")
    
    if (i==0) { return "0" }
    if (i==1) { return "1" }
    if (i==2) { return "2" }
    if (i==3) { return "3" }
    if (i==4) { return "4" }
    if (i==5) { return "5" }
    if (i==6) { return "6" }
    if (i==7) { return "7" }
    if (i==8) { return "8" }
    if (i==9) { return "9" }
    
    log("Function 'itos' out of bounds, returning the answer (42)")
    return "42"
}

# String to Int
function stoi(s) {
   if (s=='0') { return 0 }
   if (s=='1') { return 1 }
   if (s=='2') { return 2 }
   if (s=='3') { return 3 }
   if (s=='4') { return 4 }
   if (s=='5') { return 5 }
   if (s=='6') { return 6 }
   if (s=='7') { return 7 }
   if (s=='8') { return 8 }
   if (s=='9') { return 9 }
   
   log("Function 'stoi' out of bounds, returning the answer (42)")
   return 42
   
}

# Rads to degrees
function rtod(r) {
   return (r*(180.0/math.pi))
}

# Degrees to rads
function dtor(d) {
   return (math.pi*(d/180.0))
}

# Force angles in the (-180,180) interval
function degrees_interval(a) {
    var temp = a
    while ((temp>360.0) or (temp<0.0)) {
        if (temp > 360.0) {
            temp = temp - 360.0
        } else if (temp < 0.0){
            temp = temp + 360.0
        }
    }
    if (temp > 180.0) {
        temp = temp - 360.0
    }
    return temp
}

# Force angles in the (-pi,pi) interval
function radians_interval(a) {
    var temp = a
    while ((temp>2.0*math.pi) or (temp<0.0)) {
        if (temp > 2.0*math.pi) {
            temp = temp - 2.0*math.pi
        } else if (temp < 0.0){
            temp = temp + 2.0*math.pi
        }
    }
    if (temp > math.pi) {
        temp = temp - 2.0*math.pi
    }
    return temp
}

#################################################
### MOVEMENT/COMMUNICATION PRIMITIVES ###########
#################################################

# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
}
# 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)
}

# Calculates and actuates the flocking interaction
function hexagon() {
  # Calculate accumulator
  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
  if(neighbors.count() > 0)
    math.vec2.scale(accum, 1.0 / neighbors.count())
  # Move according to vector
  moveto(accum.x, accum.y)
}

function inform_your_neighborhood() {    
    # Reset to 0 the visibility of all neighbors
    foreach(knowledge, function(key, value) {
          column = string.sub(key, string.length(key)-1,string.length(key))
          if (column=='3') { 
              knowledge[key] = 0 
          }     
    })    
    neighbors.foreach( function(rid, data) {               
        # For each neighbor, send a message with its azimuth, as seen by the broadcasting robot
        message_id = string.tostring(rid)
        neighbors.broadcast(message_id, rtod(data.azimuth))        
        # Record the neighbor azimuth in my own knowledge table
        write_knowledge(knowledge, rid, 0, rtod(data.azimuth))        
        # Record the neighbor distance in my own knowledge table
        write_knowledge(knowledge, rid, 2, data.distance)        
        # Set neighbor as visible
        write_knowledge(knowledge, rid, 3, 1)     
    })    
    # Send a message with the desired direction, as seen by the broadcasting robot
    neighbors.broadcast("direction", local_dir)
    
}

function listen_to_your_neighborhood() {    
    # For all "senders" in my neighborhood, record my azimuth, as seen by them
    message_id = string.tostring(id)
    neighbors.listen(message_id, function(vid, value, rid) {
        write_knowledge(knowledge, rid, 1, value)
    })    
}

#################################################
### ACTUAL CONTROLLERS ##########################
#################################################

function zero() {   
  # Do not move
  moveto(0.0,0.0) 
  # Tell the neighbors of the center where to go
  inform_your_neighborhood()       
}

function onetwo() {
  if (id == 1) {
      angle = 45
  } else {
      angle = -135
  }
  # Broadcast information
  inform_your_neighborhood()   
  # If the center 0 is in sight
  if (read_knowledge(knowledge, 0, 3) == 1) {
      arm_offset = degrees_interval(read_knowledge(knowledge, 0, 1) - angle)
      if (arm_offset<3 and arm_offset>(-3)) {
          hexagon()  # Underlying Lennard-Jones potential behavior
      } else {
      
          local_rotation = degrees_interval( read_knowledge(knowledge, 0, 1) + (180.0 - read_knowledge(knowledge, 0, 0)) )
          local_arm = degrees_interval(angle - local_rotation)
          
          if (read_knowledge(knowledge, 0, 2) > 250.0) { 
              x_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0)))
              y_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))       
          } else if (read_knowledge(knowledge, 0, 2) < 30.0) {
              x_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0)))
              y_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))              
          } else {
              spiraling = 2.0+(id/10.0) # Fun stuff but be careful with this, it affects how a robots turns around a central node, use random number generation, eventually
              if (arm_offset > 0) { # Clockwise

                  x_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
                  y_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
              } else { # Counterclockwise
                  x_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
                  y_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
              }
          }
          speed = 100
          moveto(speed * x_mov,speed * y_mov)
      }
  } else {
      hexagon()            
  }            
}

#################################################
### BUZZ FUNCTIONS ##############################
#################################################

function action(){
    if (id == 0)
        statef=zero
    else
        statef=onetwo

    UAVSTATE="TENTACLES"
}

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

  # Local knowledge table
  knowledge = {}
  # Update local knowledge with information from the neighbors
  listen_to_your_neighborhood()
  # Variables initialization
  iteration = 0 

}

# Executed every time step
function step() {
    uav_rccmd()
    uav_neicmd()

	statef()
	log("Current state: ", CURSTATE)
	log("Swarm size: ",ROBOTS)

  # Count the number of steps
  iteration = iteration + 1   
}

# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Execute at exit
function destroy() {
}