#pragma once

#include "behaviortree_ros2/bt_service_node.hpp"
#include "mg_msgs/srv/i2c.hpp"

namespace mg {
    class I2cNode : public BT::RosServiceNode<mg_msgs::srv::I2c> {
      public:
        I2cNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
          BT::RosServiceNode<mg_msgs::srv::I2c>(name, conf, params) { }

        static BT::PortsList providedPorts() {
            return providedBasicPorts({ BT::InputPort<int>("Address", 42, {}),
                                        BT::InputPort<int>("Data", 0, {}),
                                        BT::OutputPort<int>("Result") });
        }

        bool setRequest(typename Request::SharedPtr& req) override {
            req->addr = getInput<int>("Address").value_or(42);
            req->data = getInput<int>("Result").value_or(0);
            return true;
        }

        BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& resp) override {
            setOutput<int>("Result", resp->resp.front());
            return BT::NodeStatus::SUCCESS;
        }
    };
}