#include <stdio.h>

#include "rcl/timer.h"
#include "rcl/subscription.h"
#include "rcl/wait.h"

#include "rmw/qos_profiles.h"
#include "rmw/types.h"

#define ADDR_OR_NULL(elem) (elem != NULL ? elem->impl : NULL);

/*
 * rclada_wait_set_client_check
 *
 * The set type is not properly generated by -fada-dump-spec, so this is needed
 */
const void * rclada_wait_set_client_check (rcl_wait_set_t *set, int i) {
  if (i >= set->size_of_clients) {
    printf("rclada_wait_set_client_check: index out of range %d >= %zd\n",
	   i, set->size_of_clients);
    return NULL;
  }
  else
    return ADDR_OR_NULL(set->clients[i]);
}

/*
 * rclada_wait_set_service_check
 *
 * The set type is not properly generated by -fada-dump-spec, so this is needed
 */
const void * rclada_wait_set_service_check (rcl_wait_set_t *set, int i) {
  if (i >= set->size_of_services) {
    printf("rclada_wait_set_service_check: index out of range %d >= %zd\n",
	   i, set->size_of_services);
    return NULL;
  }
  else
    return ADDR_OR_NULL(set->services[i]);
}

/*
 * rclada_wait_set_subscription_check
 *
 * The set type is not properly generated by -fada-dump-spec, so this is needed
 */
const void * rclada_wait_set_subscription_check (rcl_wait_set_t *set, int i) {
  if (i >= set->size_of_subscriptions) {
    printf("rclada_wait_set_subscription_check: index out of range %d >= %zd\n",
	   i, set->size_of_subscriptions);
    return NULL;
  }
  else
    return ADDR_OR_NULL(set->subscriptions[i]);
}

/*
 * rclada_wait_set_timer_check
 */
const void * rclada_wait_set_timer_check (rcl_wait_set_t *set, int i) {
  if (i >= set->size_of_timers) {
    printf("rclada_wait_set_timer_check: index out of range %d >= %zd\n",
	   i, set->size_of_timers);
    return NULL;
  }
  else
    return ADDR_OR_NULL(set->timers[i]);
}

/*
 * IMPORT THE QOS DEFAULTS, which are not visible otherwise bc they're declared static
 */

rmw_qos_profile_t get_rmw_qos_profile_default(void) {
  return rmw_qos_profile_default;
}

rmw_qos_profile_t get_rmw_qos_profile_sensor_data(void) {
  return rmw_qos_profile_sensor_data;
}
