#include "lualcm_lcm.h"
#include "lcm/lcm.h"
#include "lua_ref_helper.h"
#include "lua_ver_helper.h"

#ifndef WIN32
#include "sys/select.h"
#else
#include "Winsock2.h"
#endif

/** @file */

/*
 * The LCM userdata has a table which holds subscription related data for
 * each subscription created. Each table entry consists of a userdata
 * (to hold a lcm_subscription_t pointer) and handler function. The entries
 * are indexed by a unique integer, generated by luaX_ref.
 *
 * luaX_ref is similar to luaL_ref, except it stores the reference list in
 * a separate table. (luaL_ref stores a reference list in the table being
 * referenced into. This causes problems when trying to iterate over the
 * table.)
 *
 * When a new subscription is created, a new table is created to hold the
 * subscription details. A new subscription userdata is created, which contains
 * a lcm_subscription_t pointer, and pushed onto that table. The hanlder
 * function is also pushed onto the table. The table is then pushed onto the
 * LCM userdata's subscriptions table, which is stored either in the
 * userdata's environment (Lua 5.1) or the uservalue (Lua 5.2). The table is
 * added to the subscription table using luaX_ref.
 *
 * When the subscription is unsubscribed, the user only needs to supply the
 * index (the reference number given by luaX_ref). The table containing
 * the subscription userdata and handler is removed from the subscription
 * table.
 *
 * The LCM userdata __gc does the same thing, but for all remaining entries.
 */

/* lcm userdata */
typedef struct impl_lcm_userdata {
  lcm_t *lcm;
  lua_State *handler_lua_State;
} impl_lcm_userdata_t;

/* methods */
static int impl_lcm_new(lua_State *); /* constructor */
static int impl_lcm_subscribe(lua_State *);
static int impl_lcm_unsubscribe(lua_State *);
static int impl_lcm_publish(lua_State *);
static int impl_lcm_handle(lua_State *);
static int impl_lcm_handle_timeout(lua_State *);
static int impl_lcm_timedhandle(lua_State *); /* depricated! */

/* metamethods */
static int impl_lcm_tostring(lua_State *);
static int impl_lcm_gc(lua_State *);

/* supporting functions */
static void impl_lcm_c_handler(const lcm_recv_buf_t *, const char *, void *);
static impl_lcm_userdata_t *impl_lcm_newuserdata(lua_State *);
static impl_lcm_userdata_t *impl_lcm_checkuserdata(lua_State *, int);
static void impl_lcm_createsubscriptiontable(lua_State *, int);
static void impl_lcm_getsubscriptiontable(lua_State *, int);
static void *impl_lcm_addtosubscriptiontable(lua_State *, int, int **,
                                             lcm_subscription_t ***);
static int impl_lcm_getfromsubscriptiontable(lua_State *, int, int,
                                             lcm_subscription_t **);
static int impl_lcm_removefromsubscriptiontable(lua_State *, int, int,
                                                lcm_subscription_t **);

/* subscription userdata */
typedef struct impl_sub_userdata {
  lcm_subscription_t *subscription;
  impl_lcm_userdata_t *owning_lcm_userdata;
  int ref_num;
} impl_sub_userdata_t;

static int impl_abs_index(lua_State *L, int i) {
  return i > 0 ? i : i <= LUA_REGISTRYINDEX ? i : lua_gettop(L) + 1 + i;
}

/**
 * Makes LCM userdatas' metatable. The metatable is named "lcm.lcm".
 *
 * @post A metatable exists named "lcm.lcm" and contains all of the LCM
 *     userdata's member functions.
 *
 * @param L The Lua state.
 */
void ll_lcm_makemetatable(lua_State *L) {
  /* create empty meta table */
  if (!luaL_newmetatable(L, "lcm.lcm")) {
    lua_pushstring(L, "cannot create metatable");
    lua_error(L);
  }

  const struct luaL_Reg metas[] = {
      {"__tostring", impl_lcm_tostring}, {"__gc", impl_lcm_gc}, {NULL, NULL},
  };

  /* register to meta */
  luaX_registertable(L, metas);

  const struct luaL_Reg methods[] = {
      {"subscribe", impl_lcm_subscribe},
      {"unsubscribe", impl_lcm_unsubscribe},
      {"publish", impl_lcm_publish},
      {"handle", impl_lcm_handle},
      {"handle_timeout", impl_lcm_handle_timeout},
      {"timedhandle", impl_lcm_timedhandle},
      {NULL, NULL},
  };

  /* register methods to new table, set __index */
  lua_pushstring(L, "__index");
  lua_newtable(L);
  luaX_registertable(L, methods);
  lua_rawset(L, -3);

  /* TODO hide metatable */
  /*lua_pushstring(L, "__metatable");
  lua_pushnil(L);
  lua_rawset(L, -3);*/

  /* pop the metatable */
  lua_pop(L, 1);
}

/**
 * Registers all LCM functions to the LCM module. At the moment, only one
 * function is registered: the LCM constructor.
 *
 * @post All LCM functions have been added to the LCM module.
 *     The modules table is on the top of the stack.
 *
 * @param L The Lua state.
 */
void ll_lcm_register_new(lua_State *L) {
  const struct luaL_Reg new_function[] = {
      {"new", impl_lcm_new}, {NULL, NULL},
  };

  luaX_registerglobal(L, "lcm.lcm", new_function);
}

/**
 * Creates and initializes an LCM userdata.
 *
 * Optionally takes one argument, a string, containing the LCM provider. If no
 * provider is supplied, the LCM userdata is created using the environment
 * variable LCM_DEFAULT_URL if it is defined, or the default
 * "udpm://239.255.76.67:7667".
 *
 * @see lcm_create
 *
 * @pre The Lua arguments on the stack:
 *     A string for the provider.
 *
 * @post The Lua return values on the stack:
 *     The new LCM userdata.
 *
 * @param L The Lua state.
 * @return The number of return values on the Lua stack.
 *
 * @throws Lua error if LCM userdata cannot be created.
 */
static int impl_lcm_new(lua_State *L) {
  /* we expect 1 argument */
  lua_settop(L, 1);

  /* check for a string containing the provider */
  const char *provider = NULL;
  if (!lua_isnil(L, 1)) {
    /* if there was no argument, the stack will have nil */
    provider = luaL_checkstring(L, 1);
  }

  /* open the lcm connection */
  lcm_t *lc = lcm_create(provider);

  /* check for failure */
  if (!lc) {
    lua_pushstring(L, "error lcm create");
    lua_error(L);
  }

  /* create lcm userdata */
  impl_lcm_userdata_t *lcmu = impl_lcm_newuserdata(L);
  lcmu->lcm = lc;

  /* create a subscription table */
  impl_lcm_createsubscriptiontable(L, -1);

  /* return lcm userdata, which is on top of the stack */

  return 1;
}

/**
 * Publishes a message.
 *
 * @see lcm_publish
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata (self), a string containing the channel, and a string
 *     containing message data (from an encode).
 *
 * @post The Lua return values on the stack:
 *     Nothing.
 *
 * @param L The Lua state.
 * @return The number of return values on the Lua stack.
 *
 * @throws Lua error if the message cannot be published.
 */
static int impl_lcm_publish(lua_State *L) {
  /* we expect 3 arguments */
  lua_settop(L, 3);

  /* get the lcm userdata */
  impl_lcm_userdata_t *lcmu = impl_lcm_checkuserdata(L, 1);

  /* get the channel */
  const char *channel = luaL_checkstring(L, 2);

  /* get the buffer */
  size_t data_size;
  const char *data = luaL_checklstring(L, 3, &data_size);

  /* publish the message */
  if (lcm_publish(lcmu->lcm, channel, data, data_size) != 0) {
    lua_pushstring(L, "error lcm publish");
    lua_error(L);
  }

  return 0;
}

/**
 * Subscribes to a channel. Requires the user to specify a Lua handler (which
 * is called by generic_lcm_handler). The whole process is somewhat involved.
 *
 * In Lua 5.1, the handlers are stored in the LCM userdata's environment.
 *
 * Subscriptions are managed by the LCM userdata, and the user is not given
 * access to the subscription userdata. This subscribe method returns
 * subscription reference number, which is used to unsubscribe later. If the
 * user does not manually unsubscribe, the subscription will automatically
 * terminate during garbage collection of the LCM userdata.
 *
 * @see lcm_subscribe
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata (self), a string containing the channel, and a function
 *     for the handler.
 *
 * @post The Lua return values on the stack:
 *     A subscription reference number.
 *
 * @param L The Lua state.
 * @return The number of return values on the Lua stack.
 */
static int impl_lcm_subscribe(lua_State *L) {
  /* we expect 3 arguments */
  lua_settop(L, 3);

  /* get the lcm userdata */
  impl_lcm_userdata_t *lcmu = impl_lcm_checkuserdata(L, 1);

  /* get the channel */
  const char *channel = luaL_checkstring(L, 2);

  /* check the handler */
  luaL_checktype(L, 3, LUA_TFUNCTION);

  /* add a subscription table entry */
  int *ref_num_ptr;
  lcm_subscription_t **subscription;
  void *userdata =
      impl_lcm_addtosubscriptiontable(L, 1, &ref_num_ptr, &subscription);

  /* pop subscription table */
  lua_pop(L, 1);

  /* do the actual subscribe */
  *subscription =
      lcm_subscribe(lcmu->lcm, channel, impl_lcm_c_handler, userdata);

  /* push reference number */
  lua_pushinteger(L, *ref_num_ptr);

  return 1;
}

/**
 * Handles an incoming message. Calls lcm_handle. Just like lcm_handle, handler
 * functions are invoked one at a time, in the order they were subscribed,
 * during the execution of this function.
 *
 * Notice that the handle method prepares the Lua stack for the handler
 * functions. When the handler functions execute, the Lua stack contains only
 * the LCM userdata.
 *
 * Recursive calls to handle are not allowed, therefore handlers must not
 * call handle.
 *
 * @see lcm_handle
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata (self).
 *
 * @post The Lua return values on the stack:
 *     Nothing.
 *
 * @param L The Lua state.
 * @return The number of return values on the Lua stack.
 *
 * @throws Lua error if the message cannot be handled.
 */
static int impl_lcm_handle(lua_State *L) {
  /* we expect 1 argument */
  lua_settop(L, 1);

  /* get the lcm userdata */
  impl_lcm_userdata_t *lcmu = impl_lcm_checkuserdata(L, 1);

  /* update the lua state */
  lcmu->handler_lua_State = L;

  /* call lcm handle */
  if (lcm_handle(lcmu->lcm) != 0) {
    lua_pushstring(L, "error lcm handle");
    lua_error(L);
  }

  return 0;
}

/**
 * Handles an incoming message. Only blocks for the given amount of time. Calls
 * lcm_handle_timeout. Just like lcm_handle, handler functions are invoked one
 * at a time, in the order they were subscribed, during the execution of this
 * function.
 *
 * Notice that the handle method prepares the Lua stack for the handler
 * functions. When the handler functions execute, the Lua stack contains only
 * the LCM userdata.
 *
 * Recursive calls to handle are not allowed, therefore handlers must not
 * call handle.
 *
 * @see lcm_handle_timeout
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata (self), and a number of milliseconds to block.
 *
 * @post The Lua return values on the stack:
 *     A boolean: true if a message was handled, false otherwise.
 *
 * @param L The Lua state.
 * @return The number of return values on the Lua stack.
 *
 * @throws Lua error if the message cannot be handled.
 */
static int impl_lcm_handle_timeout(lua_State *L) {
  /* we expect 2 arguments */
  lua_settop(L, 2);

  /* get the lcm userdata */
  impl_lcm_userdata_t *lcmu = impl_lcm_checkuserdata(L, 1);

  /* check for a timeout in milliseconds */
  lua_Integer timeout_millisec = luaL_checkinteger(L, 2);

  /* update the lua state */
  lcmu->handler_lua_State = L;

  /* call lcm handle_timeout */
  int status = lcm_handle_timeout(lcmu->lcm, timeout_millisec);

  if (status == 1) {
    /* ok, return true */
    lua_pushboolean(L, 1);

  } else if (status == 0) {
    /* timeout, return false */
    lua_pushboolean(L, 0);

  } else {
    /* an error occured */
    lua_pushstring(L, "error lcm handle");
    lua_error(L);
  }

  return 1;
}

/**
 * This function is deprecated! It was written before lcm_handle_timeout
 * existed, so now you should use that instead!
 *
 * Handles an incoming message. Only blocks for the given amount of time. Calls
 * lcm_handle. Just like lcm_handle, handler functions are invoked one at a
 * time, in the order they were subscribed, during the execution of this
 * function.
 *
 * Notice that the handle method prepares the Lua stack for the handler
 * functions. When the handler functions execute, the Lua stack contains only
 * the LCM userdata.
 *
 * Recursive calls to handle are not allowed, therefore handlers must not
 * call handle.
 *
 * @see lcm_handle
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata (self), and a number for the seconds to block.
 *
 * @post The Lua return values on the stack:
 *     A boolean: true if a message was handled, false otherwise.
 *
 * @param L The Lua state.
 * @return The number of return values on the Lua stack.
 *
 * @throws Lua error if the message cannot be handled.
 */
static int impl_lcm_timedhandle(lua_State *L) {
  /* we expect 2 arguments */
  lua_settop(L, 2);

  /* get the lcm userdata */
  impl_lcm_userdata_t *lcmu = impl_lcm_checkuserdata(L, 1);

  /* check for a floating point timeout */
  double timeout_sec = luaL_checknumber(L, 2);

  /* update the lua state */
  lcmu->handler_lua_State = L;

  int fd = lcm_get_fileno(lcmu->lcm);
  fd_set fds;
  FD_ZERO(&fds);
  FD_SET(fd, &fds);

  /* convert the timeout to microseconds */
  struct timeval timeout = {0};
  timeout.tv_usec = timeout_sec * 1000000.0;

  int status = select(fd + 1, &fds, 0, 0, &timeout);

  if (status == 0) {
    /* timeout, return false */
    lua_pushboolean(L, 0);

  } else if (FD_ISSET(fd, &fds)) {
    /* read for handle */
    if (lcm_handle(lcmu->lcm) != 0) {
      lua_pushstring(L, "error lcm handle");
      lua_error(L);
    }

    /* ok, return true */
    lua_pushboolean(L, 1);

  } else {
    /* select must have encountered an error */
    lua_pushstring(L, "error lcm handle (select)");
    lua_error(L);
  }

  return 1;
}

/**
 * Used by the LCM C API to dispatch a Lua handler. This function invokes a
 * Lua handlers, which is stored in the subscribtion userdata, which was
 * created by the subscribe method.
 *
 * In order to do its work, this function need a pointer to the Lua stack
 * used by the handle function, a lcm_subscription_t pointer, and a Lua
 * handler function.
 *
 * The handle method prepares the Lua stack for the handlers. The userdata
 * supplied to this function is always the subscription userdata created during
 * subscribe.
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata.
 *
 * @post The Lua return values on the stack:
 *     A LCM userdata.
 *
 * @post On exit, the Lua stack is the same as it was on entrance.
 *
 * @param recv_buf The receive buffer containing the message.
 * @param channel The channel of the message.
 * @param userdata The userdata given during the call to handle. For this
 *     function the userdata is always the subscription userdata created
 *     during the call to subscribe.
 *
 * @throws The handler function may propagate Lua errors.
 */
static void impl_lcm_c_handler(const lcm_recv_buf_t *recv_buf,
                               const char *channel, void *userdata) {
  /* get the subscription userdata */
  impl_sub_userdata_t *subu = (impl_sub_userdata_t *)userdata;

  /* get the current lua state */
  lua_State *L = subu->owning_lcm_userdata->handler_lua_State;

  /* get the ref_num */
  int ref_num = subu->ref_num;

  /* remeber, lcm userdata is the only value on the stack */
  /* this was setup by handle function */

  /* get subscription table entry, this pushes the handler on the stack */
  lcm_subscription_t *subscription;
  if (!impl_lcm_getfromsubscriptiontable(L, 1, ref_num, &subscription)) {
    /* this should never happen */
    lua_pushstring(L, "lcm handler cannot find lua handler");
    lua_error(L);
  }

  /* push channel */
  lua_pushstring(L, channel);

  /* push buffer as a binary string */
  lua_pushlstring(L, (const char *)recv_buf->data, recv_buf->data_size);

  /* call the handler */
  lua_call(L, 2, 0);
}

/**
 * Unsubscribes from a channel. Removes the subscription from the internal
 * subscription table.
 *
 * @see lcm_unsubscribe
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata (self), and a subscription reference number.
 *
 * @post The Lua return values on the stack:
 *     Nothing.
 *
 * @param L The Lua state.
 * @return The number of return values on the Lua stack.
 *
 * @throws Lua error if the subscription cannot be unsubscribed.
 */
static int impl_lcm_unsubscribe(lua_State *L) {
  /* we expect 2 arguments */
  lua_settop(L, 2);

  /* get the lcm userdata */
  impl_lcm_userdata_t *lcmu = impl_lcm_checkuserdata(L, 1);

  /* get the ref_num */
  int ref_num = luaL_checkint(L, 2);

  /* get subscription table entry, this pushes the handler on the stack */
  lcm_subscription_t *subscription;
  if (!impl_lcm_removefromsubscriptiontable(L, 1, ref_num, &subscription)) {
    /* made up reference number */
    lua_pushstring(L, "subscription number invalid");
    lua_error(L);
  }

  /* unsubscribe */
  if (lcm_unsubscribe(lcmu->lcm, subscription) != 0) {
    lua_pushstring(L, "error lcm unsubscribe");
    lua_error(L);
  }

  return 0;
}

/**
 * Creates a string from an LCM userdata. This is the __tostring metamethod of
 * the LCM userdata.
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata (self).
 *
 * @post The Lua return values on the stack:
 *     A string representing the LCM userdata.
 *
 * @param L The Lua state.
 * @return The number of return values on the Lua stack.
 */
static int impl_lcm_tostring(lua_State *L) {
  /* we expect 1 argument */
  lua_settop(L, 1);

  /* get the lcm userdata */
  impl_lcm_userdata_t *lcmu = impl_lcm_checkuserdata(L, 1);

  /* make the string */
  lua_pushfstring(L, "lcm.lcm [v%d.%d.%d] (@ %p)", LCM_MAJOR_VERSION,
                  LCM_MINOR_VERSION, LCM_MICRO_VERSION, lcmu);

  return 1;
}

/**
 * Cleans up the LCM userdata. This is the __gc metamethod of the LCM userdata.
 * This method is called automatically by the Lua garbage collector.
 *
 * Automatically unsubscribes all channels.
 *
 * @see lcm_destroy
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata (self).
 *
 * @post The Lua return values on the stack:
 *     Nothing.
 *
 * @param L The Lua state.
 * @return The number of return values on the Lua stack.
 */
static int impl_lcm_gc(lua_State *L) {
  /* we expect 1 argument */
  lua_settop(L, 1);

  /* get the lcm userdata */
  impl_lcm_userdata_t *lcmu = impl_lcm_checkuserdata(L, 1);

  /* check if this userdata was ever created */
  if (!lcmu->lcm) {
    return 0;
  }

  /* get subscription table */
  impl_lcm_getsubscriptiontable(L, 1);

  /* subscription table traversal */
  lua_pushnil(L); /* first key */
  while (lua_next(L, 2) != 0) {
    /* get the subscription userdata */
    lua_pushstring(L, "userdata");
    lua_rawget(L, -2);

    impl_sub_userdata_t *subu = (impl_sub_userdata_t *)lua_touserdata(L, -1);

    /* unsubscribe */
    if (lcm_unsubscribe(lcmu->lcm, subu->subscription) != 0) {
      lua_pushstring(L, "error lcm unsubscribe");
      lua_error(L);
    }

    /* pop the userdata and subscription table entry */
    lua_pop(L, 2);
  }

  /* free lcm */
  lcm_destroy(lcmu->lcm);

  /* clear out the subscription table */
  impl_lcm_createsubscriptiontable(L, 1);

  return 0;
}

/**
 * Creates a new LCM userdata. The userdata is defined using the "lcm.lcm"
 * metatable.
 *
 * @pre The Lua arguments on the stack:
 *     Nothing.
 *
 * @pre The metatable "lcm.lcm" is defined.
 *
 * @post The Lua return values on the stack:
 *     The new LCM userdata.
 *
 * @param L The Lua state.
 * @return A pointer to the new LCM userdata.
 */
static impl_lcm_userdata_t *impl_lcm_newuserdata(lua_State *L) {
  /* make new user data */
  impl_lcm_userdata_t *lcmu =
      (impl_lcm_userdata_t *)lua_newuserdata(L, sizeof(impl_lcm_userdata_t));

  /* initialize struct */
  lcmu->lcm = NULL;
  lcmu->handler_lua_State = NULL;

  /* set the metatable */
  luaL_getmetatable(L, "lcm.lcm");
  if (lua_isnil(L, -1)) {
    lua_pushstring(L, "cannot find metatable");
    lua_error(L);
  }
  lua_setmetatable(L, -2);

  return lcmu;
}

/**
 * Checks for a LCM userdata at the given index. Checks the userdata type using
 * the "lcm.lcm" metatable.
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata at the given index.
 *
 * @pre The metatable "lcm.lcm" is defined.
 *
 * @post The Lua return values on the stack:
 *     Nothing.
 *
 * @param L The Lua state.
 * @param index The index of the LCM userdata.
 * @return A pointer to the LCM userdata.
 */
static impl_lcm_userdata_t *impl_lcm_checkuserdata(lua_State *L, int index) {
  return (impl_lcm_userdata_t *)luaL_checkudata(L, index, "lcm.lcm");
}

/**
 * Creates a new subscription table. Creates a blank table and then adds it to
 * the userdata's environment/uservalue.
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata at the given index.
 *
 * @post The Lua return values on the stack:
 *     Nothing.
 * @post The LCM userdata has a subscription table.
 *
 * @param L The Lua state.
 * @param index The index of the LCM userdata.
 */
static void impl_lcm_createsubscriptiontable(lua_State *L, int index) {
  index = impl_abs_index(L, index);

  /* check userdata type */
  /* ...unnecessary, assume lcm userdata at index */

  /* create a blank table */
  lua_newtable(L);

  /* add subscription table */
  lua_pushstring(L, "subscriptions");
  lua_newtable(L);
  lua_rawset(L, -3);

  /* add a table to hold references */
  lua_pushstring(L, "subreflist");
  lua_newtable(L);
  lua_rawset(L, -3);

  /* set as environment */
  luaX_setfenv(L, index);
}

/**
 * Pushes LCM userdata's subscription table onto the stack. The LCM userdata's
 * subscription table can then be modified.
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata at the given index.
 *
 * @post The Lua return values on the stack:
 *     The subscription table, and the subscription table reference list.
 *
 * @param L The Lua state.
 * @param index The index of the LCM userdata.
 */
static void impl_lcm_getsubscriptiontable(lua_State *L, int index) {
  /* index = impl_abs_index(L, index); */

  /* check userdata type */
  /* ...unnecessary, assume lcm userdata at index */

  /* get the environment */
  luaX_getfenv(L, index);

  /* get the subscription table */
  lua_pushstring(L, "subscriptions");
  lua_rawget(L, -2);

  /* get subscription reference list */
  lua_pushstring(L, "subreflist");
  lua_rawget(L, -3);

  /* remove the environment */
  lua_remove(L, -3);
}

/**
 * Add a new subscription to a subscription table.
 *
 * Given a LCM userdata and a handler function on the stack, create a new
 * entry in the subscription table. The handler function is popped off the
 * stack. The subscription double pointer is set to point to the underlying
 * subscription pointer stored in the subscription table. The reference pointer
 * is set to point to the reference number (used to index the subscription
 * table) stored in the subscription table. Weird, I know.
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata at the given index, and a handler function.
 *
 * @post The Lua return values on the stack:
 *     Nothing.
 *
 * @param L The Lua state.
 * @param index The index of the subscription table.
 * @param ref_num_ptr Used to return a pointer to the reference number.
 * @param subscription_double_ptr Used to return a pointer to a subscription
 *     pointer. This needs to be set by the caller.
 */
static void *impl_lcm_addtosubscriptiontable(
    lua_State *L, int index, int **ref_num_ptr,
    lcm_subscription_t ***subscription_double_ptr) {
  /* leave nothing new on the stack */
  const int initial_top = lua_gettop(L);

  /* save handler index for later */
  int handler_index = impl_abs_index(L, -1);

  /* get the lcm userdata */
  impl_lcm_userdata_t *lcmu = impl_lcm_checkuserdata(L, index);

  /* get the subscription table */
  impl_lcm_getsubscriptiontable(L, index);
  int subscription_table_index = impl_abs_index(L, -2);
  int subscription_ref_list_index = impl_abs_index(L, -1);

  /* the new subscription table entry */
  lua_newtable(L);

  /* add userdata to table */
  lua_pushstring(L, "userdata");
  impl_sub_userdata_t *subu =
      (impl_sub_userdata_t *)lua_newuserdata(L, sizeof(impl_sub_userdata_t));
  lua_rawset(L, -3);

  /* set the owning lcm userdata */
  subu->owning_lcm_userdata = lcmu;

  /* we need to return a pointer to this subscription pointer */
  *subscription_double_ptr = &subu->subscription;

  /* we need to return a pointer to this int */
  *ref_num_ptr = &subu->ref_num;

  /* add handler function to table */
  lua_pushstring(L, "handler");
  lua_pushvalue(L, handler_index);
  lua_rawset(L, -3);

  /* add to subscription table */
  subu->ref_num =
      luaX_ref(L, subscription_table_index, subscription_ref_list_index);

  /* leave nothing new on the stack */
  lua_settop(L, initial_top);

  /* pop handler table */
  lua_pop(L, 1);

  return (void *)subu;
}

/**
 * Get a subscription from a subscription table.
 *
 * Given a LCM userdata and reference number, push the handler function
 * onto the stack and return a pointer to the underlying subscription.
 *
 * @see impl_addtosubscriptiontable
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata at the given index.
 *
 * @post The Lua return values on the stack:
 *     The handler function.
 *
 * @param L The Lua state.
 * @param index The index of the LCM userdata.
 * @param ref_num The reference number into the subscription table.
 * @param subscription The subscription pointer.
 * @return Non-zero on success.
 */
static int impl_lcm_getfromsubscriptiontable(
    lua_State *L, int index, int ref_num, lcm_subscription_t **subscription) {
  /* get lcm userdata */
  impl_lcm_getsubscriptiontable(L, index);

  /* remove the reference list, don't need it */
  lua_pop(L, 1);

  /* get the subscription table entry */
  lua_rawgeti(L, -1, ref_num);

  /* remove subscription table */
  lua_remove(L, -2);

  /* make sure this is a real entry */
  if (lua_isnil(L, -1)) {
    return 0;
  }

  /* get the userdata */
  lua_pushstring(L, "userdata");
  lua_rawget(L, -2);

  impl_sub_userdata_t *subu = (impl_sub_userdata_t *)lua_touserdata(L, -1);

  /* set the subscription */
  *subscription = subu->subscription;

  /* pop the userdata */
  lua_pop(L, 1);

  /* get the handler */
  lua_pushstring(L, "handler");
  lua_rawget(L, -2);

  /* remove the subscription table entry */
  lua_remove(L, -2);

  return 1;
}

/**
 * Remove a subscription from a subscription table.
 *
 * Given a LCM userdata and reference number, remove a subscription.
 *
 * @see impl_addtosubscriptiontable
 *
 * @pre The Lua arguments on the stack:
 *     A LCM userdata at the given index.
 *
 * @post The Lua return values on the stack:
 *     Nothing.
 *
 * @param L The Lua state.
 * @param index The index of the LCM userdata.
 * @param ref_num The reference number into the subscription table.
 * @param subscription The subscription pointer.
 * @return Non-zero on success.
 *
 * Doesn't push handler function.
 */
static int impl_lcm_removefromsubscriptiontable(
    lua_State *L, int index, int ref_num, lcm_subscription_t **subscription) {
  /* leave nothing new on the stack */
  const int initial_top = lua_gettop(L);

  /* get lcm userdata */
  impl_lcm_getsubscriptiontable(L, index);
  int subscription_table_index = impl_abs_index(L, -2);
  int subscription_ref_list_index = impl_abs_index(L, -1);

  /* get the subscription table entry */
  lua_rawgeti(L, subscription_table_index, ref_num);

  /* make sure this is a real entry */
  if (lua_isnil(L, -1)) {
    return 0;
  }

  /* get the userdata */
  lua_pushstring(L, "userdata");
  lua_rawget(L, -2);

  impl_sub_userdata_t *subu = (impl_sub_userdata_t *)lua_touserdata(L, -1);

  /* set the subscription */
  *subscription = subu->subscription;

  /* remove from subscription table */
  luaX_unref(L, subscription_table_index, subscription_ref_list_index, ref_num);

  /* leave nothing new on the stack */
  lua_settop(L, initial_top);

  return 1;
}
