#pragma once

#include <zephyr/sys/slist.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rosidl_runtime_c/message_type_support_struct.h>
#include <zephyr/kernel.h>

class MicrorosExecutor {
private:
    const char *node_name;

    rcl_allocator_t allocator;
    rclc_executor_t executor;
    rcl_node_t node;
    rclc_support_t support;

    sys_slist_t publishers_head;
    sys_slist_t subscribers_head;

    struct publisher_node {
        rcl_publisher_t publisher;
        sys_snode_t node;  // embedded Zephyr list node
    };

    struct subscription_node {
        rcl_subscription_t subscription;
        sys_snode_t node;  // embedded Zephyr list node
    };

public:
    MicrorosExecutor(const char *name);
    ~MicrorosExecutor(void);

    bool connect(void);
    bool execute(void);
    rcl_publisher_t* add_publisher(const rosidl_message_type_support_t *type_support, const char *topic_name);
    rcl_subscription_t* add_subscription(const rosidl_message_type_support_t *type_support, const char *topic_name, void* msg, rclc_callback_t callback);
};