void buzzvm_vstig_onconflictlost(buzzvm_t vm,
                                 buzzvstig_t vs,
                                 buzzobj_t k,
                                 buzzvstig_elem_t lv) {
    /* Was a conflict manager defined? */
    if(vs->onconflictlost) {
        /* Push closure */
        buzzvm_push(vm, vs->onconflictlost);
        /* Push key */
        buzzvm_push(vm, k);
        /* Make table for local value */
        buzzvm_pusht(vm);
        buzzobj_t loc = buzzvm_stack_at(vm, 1);
        buzzvm_pushs(vm, buzzvm_string_register(vm, "robot"));
        buzzvm_pushi(vm, lv->robot);
        buzzvm_tput(vm);
        buzzvm_push(vm, loc);
        buzzvm_pushs(vm, buzzvm_string_register(vm, "data"));
        buzzvm_push(vm, lv->data);
        buzzvm_tput(vm);
        buzzvm_push(vm, loc);
        buzzvm_pushs(vm, buzzvm_string_register(vm, "timestamp"));
        buzzvm_pushi(vm, lv->timestamp);
        buzzvm_tput(vm);
        /* Call closure with 2 arguments */
        buzzvm_push(vm, loc);
        buzzvm_closure_call(vm, 2);
    }
}

int buzzvm_set_bcode(buzzvm_t vm,
                     const uint8_t* bcode,
                     uint32_t bcode_size) {
   /* Fetch the string count */
   uint16_t count;
   memcpy(&count, bcode, sizeof(uint16_t));
   /* Go through the strings and store them */
   uint32_t i = sizeof(uint16_t);
   long int c = 0;
   for(; (c < count) && (i < bcode_size); ++c) {
      /* Store string */
      buzzvm_string_register(vm, (char*)(bcode + i), 1);
      /* Advance to first character of next string */
      while(*(bcode + i) != 0) ++i;
      ++i;
   }
   /* Initialize VM state */
   vm->state = BUZZVM_STATE_READY;
   vm->error = BUZZVM_ERROR_NONE;
   /* Initialize bytecode data */
   vm->bcode_size = bcode_size;
   vm->bcode = bcode;
   /* Set program counter */
   vm->pc = i;
   vm->oldpc = vm->pc;
   /*
    * Register function definitions
    * Stop when you find a 'nop'
    */
   while(vm->bcode[vm->pc] != BUZZVM_INSTR_NOP)
      if(buzzvm_step(vm) != BUZZVM_STATE_READY) return vm->state;
   buzzvm_step(vm);
   /* Initialize empty neighbors */
   buzzneighbors_reset(vm);
   /* Register robot id */
   buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1));
   buzzvm_pushi(vm, vm->robot);
   buzzvm_gstore(vm);
   /* Register basic functions */
   buzzobj_register(vm);
   /* Register stigmergy methods */
   buzzvstig_register(vm);
   /* Register swarm methods */
   buzzswarm_register(vm);
   /* Register math methods */
   buzzmath_register(vm);
   /* Register io methods */
   buzzio_register(vm);
   /* Register string methods */
   buzzstring_register(vm);
   /* All done */
   return BUZZVM_STATE_READY;
}

int buzzvstig_register(struct buzzvm_s* vm) {
   /* Push 'stigmergy' table */
   buzzvm_pushs(vm, buzzvm_string_register(vm, "stigmergy", 1));
   buzzvm_pusht(vm);
   /* Add 'create' function */
   buzzvm_dup(vm);
   buzzvm_pushs(vm, buzzvm_string_register(vm, "create", 1));
   buzzvm_pushcc(vm, buzzvm_function_register(vm, buzzvstig_create));
   buzzvm_tput(vm);
   /* Register the 'stigmergy' table */
   buzzvm_gstore(vm);
   return vm->state;
}

void CBuzzControllerFootBot::UpdateSensors() {
   /*
    * Update generic sensors
    */
   CBuzzController::UpdateSensors();
   /*
    * Update proximity sensor table
    */
   if(m_pcProximity != NULL) {
      /* Create empty proximity table */
      buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "proximity", 1));
      buzzvm_pusht(m_tBuzzVM);
      buzzobj_t tProxTable = buzzvm_stack_at(m_tBuzzVM, 1);
      buzzvm_gstore(m_tBuzzVM);
      /* Get proximity readings */
      const CCI_FootBotProximitySensor::TReadings& tProxReads = m_pcProximity->GetReadings();
      /* Fill into the proximity table */
      buzzobj_t tProxRead;
      for(size_t i = 0; i < tProxReads.size(); ++i) {
         /* Create table for i-th read */
         buzzvm_pusht(m_tBuzzVM);
         tProxRead = buzzvm_stack_at(m_tBuzzVM, 1);
         buzzvm_pop(m_tBuzzVM);
         /* Fill in the read */
         TablePut(tProxRead, "value", tProxReads[i].Value);
         TablePut(tProxRead, "angle", tProxReads[i].Angle);
         /* Store read table in the proximity table */
         TablePut(tProxTable, i, tProxRead);
      }
   }
   /*
    * Camera
    */
   if(m_pcCamera) {
      buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "blobs", 1));
      buzzvm_pusht(m_tBuzzVM);
      buzzobj_t tBlobs = buzzvm_stack_at(m_tBuzzVM, 1);
      buzzvm_gstore(m_tBuzzVM);
      const CCI_ColoredBlobOmnidirectionalCameraSensor::SReadings& sBlobs = m_pcCamera->GetReadings();
      for(size_t i = 0; i < sBlobs.BlobList.size(); ++i) {
         buzzvm_pusht(m_tBuzzVM);
         buzzobj_t tEntry = buzzvm_stack_at(m_tBuzzVM, 1);
         buzzvm_pop(m_tBuzzVM);
         TablePut(tBlobs, i, tEntry);
         TablePut(tEntry, "distance", sBlobs.BlobList[i]->Distance);
         TablePut(tEntry, "angle",    sBlobs.BlobList[i]->Angle);
         TablePut(tEntry, "color",    sBlobs.BlobList[i]->Color);
      }
   }
}

buzzvm_state CBuzzController::RegisterFunctions() {
   /* Pointer to this controller */
   buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "controller"));
   buzzvm_pushuserdata(m_tBuzzVM, this);
   buzzvm_gstore(m_tBuzzVM);
   /* BuzzLOG */
   buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "log"));
   buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzLOG));
   buzzvm_gstore(m_tBuzzVM);
   /* BuzzDebug */
   buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "debug"));
   buzzvm_pushcc(m_tBuzzVM, buzzvm_function_register(m_tBuzzVM, BuzzDebug));
   buzzvm_gstore(m_tBuzzVM);
   return m_tBuzzVM->state;
}

int buzzvm_vstig_setonconflictlost(struct buzzvm_s* vm) {
    buzzvm_lnum_assert(vm, 1);
    /* Get vstig id */
    buzzvm_lload(vm, 0);
    buzzvm_pushs(vm, buzzvm_string_register(vm, "id"));
    buzzvm_tget(vm);
    uint16_t id = buzzvm_stack_at(vm, 1)->i.value;
    /* Look for virtual stigmergy */
    buzzvstig_t* vs = buzzdict_get(vm->vstigs, &id, buzzvstig_t);
    if(vs) {
        /* Virtual stigmergy found */
        /* Get closure */
        buzzvm_lload(vm, 1);
        buzzvm_type_assert(vm, 1, BUZZTYPE_CLOSURE);
        /* Clone the closure */
        if((*vs)->onconflictlost) free((*vs)->onconflictlost);
        (*vs)->onconflictlost = buzzobj_clone(buzzvm_stack_at(vm, 1));
    }
    else {
        /* No virtual stigmergy found, just push false */
        /* If this happens, its a bug */
        buzzvm_pushnil(vm);
        fprintf(stderr, "[BUG] [ROBOT %u] Can't find virtual stigmergy %u\n", vm->robot, id);
    }
    /* Return the value found */
    return buzzvm_ret0(vm);
}

int buzzvm_swarm_others(buzzvm_t vm) {
   buzzvm_lnum_assert(vm, 1);
   /* Get the id of the current swarm */
   buzzvm_lload(vm, 0);
   buzzvm_pushs(vm, buzzvm_string_register(vm, "id"));
   buzzvm_tget(vm);
   uint16_t id1 = buzzvm_stack_at(vm, 1)->i.value;
   /* Get the swarm entry */
   uint8_t* x = buzzdict_get(vm->swarms, &id1, uint8_t);
   if(!x) {
      vm->state = BUZZVM_STATE_ERROR;
      vm->error = BUZZVM_ERROR_SWARM;
      return BUZZVM_STATE_ERROR;
   }
   /* Get the id of the new swarm to create */
   buzzvm_lload(vm, 1);
   buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
   uint16_t id2 = buzzvm_stack_at(vm, 1)->i.value;
   /* Add a new entry for the swarm */
   uint8_t v = *x ? 0 : 1;
   buzzdict_set(vm->swarms, &id2, &v);
   /* Send update, if necessary */
   if(v)
      buzzoutmsg_queue_append_swarm_joinleave(
         vm->outmsgs, BUZZMSG_SWARM_JOIN, id2);
   /* Create a table to return */
   make_table(vm, id2);
   /* Return */
   return buzzvm_ret1(vm);
}

int buzzvm_swarm_select(buzzvm_t vm) {
   buzzvm_lnum_assert(vm, 1);
   /* Get the id */
   buzzvm_lload(vm, 0);
   buzzvm_pushs(vm, buzzvm_string_register(vm, "id"));
   buzzvm_tget(vm);
   uint16_t id = buzzvm_stack_at(vm, 1)->i.value;
   /* Get the result of the condition check */
   buzzvm_lload(vm, 1);
   buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
   uint8_t in = buzzvm_stack_at(vm, 1)->i.value;
   /* Update the swarm, if known */
   if(buzzdict_exists(vm->swarms, &id)) {
      /* Store membership */
      buzzdict_set(vm->swarms, &id, &in);
      /* Send update */
      buzzoutmsg_queue_append_swarm_joinleave(
         vm->outmsgs,
         in ? BUZZMSG_SWARM_JOIN : BUZZMSG_SWARM_LEAVE,
         id);
      /* Return */
      return buzzvm_ret0(vm);
   }
   else {
      vm->state = BUZZVM_STATE_ERROR;
      vm->error = BUZZVM_ERROR_SWARM;
      return BUZZVM_STATE_ERROR;
   }
}

void CBuzzControllerFootBot::UpdateSensors() {
   /*
    * Update generic sensors
    */
   CBuzzController::UpdateSensors();
   /*
    * Update proximity sensor table
    */
   if(m_pcProximity != NULL) {
      /* Create empty proximity table */
      buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, "proximity", 1));
      buzzvm_pusht(m_tBuzzVM);
      buzzobj_t tProxTable = buzzvm_stack_at(m_tBuzzVM, 1);
      buzzvm_gstore(m_tBuzzVM);
      /* Get proximity readings */
      const CCI_FootBotProximitySensor::TReadings& tProxReads = m_pcProximity->GetReadings();
      /* Fill into the proximity table */
      buzzobj_t tProxRead;
      for(size_t i = 0; i < tProxReads.size(); ++i) {
         /* Create table for i-th read */
         buzzvm_pusht(m_tBuzzVM);
         tProxRead = buzzvm_stack_at(m_tBuzzVM, 1);
         buzzvm_pop(m_tBuzzVM);
         /* Fill in the read */
         TablePut(tProxRead, "value", tProxReads[i].Value);
         TablePut(tProxRead, "angle", tProxReads[i].Angle);
         /* Store read table in the proximity table */
         TablePut(tProxTable, i, tProxRead);
      }
   }
}

buzzvm_state CBuzzController::Register(const std::string& str_key,
                                       Real f_value) {
   buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, str_key.c_str(), 1));
   buzzvm_pushf(m_tBuzzVM, f_value);
   buzzvm_gstore(m_tBuzzVM);
   return m_tBuzzVM->state;
}

int buzzvstig_create(buzzvm_t vm) {
   buzzvm_lnum_assert(vm, 1);
   /* Get vstig id */
   buzzvm_lload(vm, 1);
   buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
   uint16_t id = buzzvm_stack_at(vm, 1)->i.value;
   buzzvm_pop(vm);
   /* Look for virtual stigmergy */
   const buzzvstig_t* vs = buzzdict_get(vm->vstigs, &id, buzzvstig_t);
   if(vs) {
      /* Found, destroy it */
      buzzdict_remove(vm->vstigs, &id);
   }
   /* Create a new virtual stigmergy */
   buzzvstig_t nvs = buzzvstig_new();
   buzzdict_set(vm->vstigs, &id, &nvs);
   /* Create a table */
   buzzvm_pusht(vm);
   /* Add data and methods */
   buzzvm_dup(vm);
   buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1));
   buzzvm_pushi(vm, id);
   buzzvm_tput(vm);
   function_register(foreach);
   function_register(size);
   function_register(put);
   function_register(get);
   function_register(onconflict);
   function_register(onconflictlost);
   /* Return the table */
   return buzzvm_ret1(vm);
}

buzzvm_state buzzvm_function_call(buzzvm_t vm,
                                  const char* fname,
                                  uint32_t argc) {
   /* Reset the VM state if it's DONE */
   if(vm->state == BUZZVM_STATE_DONE)
      vm->state = BUZZVM_STATE_READY;
   /* Don't continue if the VM has an error */
   if(vm->state != BUZZVM_STATE_READY)
      return vm->state;
   /* Push the function name (return with error if not found) */
   buzzvm_pushs(vm, buzzvm_string_register(vm, fname, 0));
   /* Get associated symbol */
   buzzvm_gload(vm);
   /* Make sure it's a closure */
   buzzvm_type_assert(vm, 1, BUZZTYPE_CLOSURE);
   /* Move closure before arguments */
   if(argc > 0) {
      buzzdarray_insert(vm->stack,
                        buzzdarray_size(vm->stack) - argc - 1,
                        buzzvm_stack_at(vm, 1));
      buzzvm_pop(vm);
   }
   /* Call the closure */
   return buzzvm_closure_call(vm, argc);
}

// proper way to send loooong 1D array
int BuzzSetMap(buzzvm_t vm){
      /* Make sure one parameter has been passed */
      buzzvm_lnum_assert(vm, 1);
      /* Get the parameter */
      buzzvm_lload(vm, 1);
      buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);    // matrix
      /* Get the table */
      buzzobj_t t = buzzvm_stack_at(vm, 1);
      /* Copy the values into a vector */
      std::vector<float> mat;
      for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) {
         /* Duplicate the table */
         buzzvm_dup(vm);
         /* Push the index */
         buzzvm_pushi(vm, i);
         /* Get the value */
         buzzvm_tget(vm);
         /* Store it in the vector (assume all values are float, no mistake...) */
         mat.push_back((float)buzzvm_stack_at(vm, 1)->f.value);
         /* Get rid of the value, now useless */
         buzzvm_pop(vm);
      }
      /* Get a pointer to the controller */
      buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
      buzzvm_gload(vm);
      /* Copy data into the controller */
      reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->SetMapParams(mat, Sqrt(buzzdict_size(t->t.value)));
      /* Done with the function */
      return buzzvm_ret0(vm);
}

static int BuzzCameraDisable(buzzvm_t vm) {
   /* Get pointer to the controller */
   buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
   buzzvm_gload(vm);
   /* Call function */
   reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->CameraDisable();
   return buzzvm_ret0(vm);
}

int BuzzStopProcessing(buzzvm_t vm) {
   /* Get pointer to the controller */
   buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
   buzzvm_gload(vm);
   /* Call function */
   reinterpret_cast<CBuzzControllerEFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->StopProcessing();
   return buzzvm_ret0(vm);
}

buzzvm_state CBuzzController::TablePut(buzzobj_t t_table,
                                       const std::string& str_key,
                                       Real f_value) {
   buzzvm_push(m_tBuzzVM, t_table);
   buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, str_key.c_str(), 1));
   buzzvm_pushf(m_tBuzzVM, f_value);
   buzzvm_tput(m_tBuzzVM);
   return m_tBuzzVM->state;
}

static int BuzzTakeOff(buzzvm_t vm) {
   /* Get pointer to the controller */
   buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
   buzzvm_gload(vm);
   /* Call function */
   int cont = reinterpret_cast<CBuzzControllerSpiri*>(buzzvm_stack_at(vm, 1)->u.value)->TakeOff();
   buzzvm_pushi(vm, cont);
   return buzzvm_ret1(vm);
}

int buzzneighbors_kin(buzzvm_t vm) {
   buzzvm_lnum_assert(vm, 0);
   /* Initialize the swarm id to 'unknown' */
   int32_t swarmid = -1;
   /* If the swarm stack is not empty, look for the swarm id */
   if(!buzzdarray_isempty(vm->swarmstack)) {
      /* Get position in swarm stack */
      uint16_t sstackpos = 1;
      if(buzzdarray_size(vm->lsyms->syms) > 1)
         sstackpos = buzzdarray_get(vm->lsyms->syms, 1, buzzobj_t)->i.value;
      /* Get swarm id */
      if(sstackpos <= buzzdarray_size(vm->swarmstack))
         swarmid = buzzdarray_get(vm->swarmstack,
                                  buzzdarray_size(vm->swarmstack) - sstackpos,
                                  uint16_t);
   }
   /* Get the self table */
   buzzvm_lload(vm, 0);
   buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
   /* Get the data table */
   buzzvm_pushs(vm, buzzvm_string_register(vm, "data", 1));
   buzzvm_tget(vm);
   buzzobj_t data = buzzvm_stack_at(vm, 1);
   /* Create a new table as return value */
   buzzobj_t t;
   vm->state = make_table(vm, &t);
   if(vm->state != BUZZVM_STATE_READY) return vm->state;
   /* If data is available, filter it */
   if(data->o.type == BUZZTYPE_TABLE) {
      /* Create a new data table */
      buzzobj_t kindata = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
      /* Filter the neighbors in data and add them to kindata */
      struct neighbor_filter_s fdata = { .vm = vm, .swarm_id = swarmid, .result = kindata->t.value };
      buzzdict_foreach(data->t.value, neighbor_filter_kin, &fdata);
      /* Add kindata as the "data" field in t */
      buzzvm_push(vm, t);
      buzzvm_pushs(vm, buzzvm_string_register(vm, "data", 1));
      buzzvm_push(vm, kindata);
      buzzvm_tput(vm);
   }
   /* Return the table */
   buzzvm_push(vm, t);
   return buzzvm_ret1(vm);
}

static int BuzzRotate(buzzvm_t vm) {
   /* Push the yaw angle */
   buzzvm_lload(vm, 1);
   /* Create a new angle with that */
   CRadians cYaw(buzzvm_stack_at(vm, 1)->f.value);
   /* Get pointer to the controller */
   buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
   buzzvm_gload(vm);
   /* Call function */
   reinterpret_cast<CBuzzControllerSpiri*>(buzzvm_stack_at(vm, 1)->u.value)->SetYaw(cYaw);
   return buzzvm_ret0(vm);
}

int BuzzRemoveCS(buzzvm_t vm) {
   buzzvm_lnum_assert(vm, 1);
   buzzvm_lload(vm, 1);

   buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
   int id = buzzvm_stack_at(vm, 1)->i.value;

   buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
   buzzvm_gload(vm);

   reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->RemoveCS(id);
   return buzzvm_ret0(vm);
}