/*
for UDP config
mbed_app.json

{
    "target_overrides": {
        "*": {
            "platform.stdio-convert-newlines": true,
            "target.network-default-interface-type": "ETHERNET"
        }
    }
}
*/

#include "mbed.h"
#include "ros2mbed.h"
#include "subscription.hpp"
#if USER_TRANSPORT_TYPE == 0 || USER_TRANSPORT_TYPE == 1
#include "EthernetInterface.h"
#endif 

#include <string>

DigitalOut led(LED1);

class SamplePub : public ros2::Node
{
public:
    SamplePub()
    : Node("sample_node"), count_(0)
    {
        pub_ = this->create_publisher<std_msgs::String>("string_pub");

        sub_ = this->create_subscription<std_msgs::Bool>(
            "led_sub",
            [&](std::shared_ptr<std_msgs::Bool> msg)
            {
                led = msg->data;
            });

        timer_ = this->create_wall_timer(
            500ms,
            [&](){
                auto msg = std::make_shared<std_msgs::String>();
                msg->data = "Hello ros2mbed! " + std::to_string(count_++);
                pub_->publish(msg);
            });
    }

private:
    int count_;
    ros2::Publisher<std_msgs::String>::SharedPtr pub_;
    ros2::Subscription<std_msgs::Bool>::SharedPtr sub_;
    
    ros2::TimerBase::SharedPtr timer_;
};


int main()
{
    led = 1;
#if USER_TRANSPORT_TYPE == 0 || USER_TRANSPORT_TYPE == 1
    EthernetInterface net;
    ros2::init(&net, "192.168.2.101", 2018);
#endif 

#if USER_TRANSPORT_TYPE == 2
    BufferedSerial pc(USBTX, USBRX, 115200);
    ros2::init(&pc);
#endif

    SamplePub sample_node;
    ros2::spin(&sample_node);
}