#include "rclcpp/rclcpp.hpp"
#include "mg_msgs/srv/i2c.hpp"

extern "C" {

#include <fcntl.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include <i2c/smbus.h>
}
class MgI2c : public rclcpp::Node {
    using I2cSrv = mg_msgs::srv::I2c;

  public:
    MgI2c(const std::string& name) : rclcpp::Node(name), i2c_fd_(open("/dev/i2c-1", O_RDWR)) { // NOLINT
        auto cb
          = [this](I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) { send_req(req, resp); };
        i2c_srv_ = create_service<I2cSrv>("/i2c", cb);
    }

    void send_req(I2cSrv::Request::ConstSharedPtr req, I2cSrv::Response::SharedPtr resp) const {
        ioctl(i2c_fd_, I2C_SLAVE, req->addr); // NOLINT
        i2c_smbus_write_byte(i2c_fd_, req->data);
        int ch = 0;

        rclcpp::Rate rate(100);

        while (ch == 0 || (ch > 255 || ch < 0)) {
            ch = i2c_smbus_read_byte(i2c_fd_);
            rate.sleep();
        }
        resp->resp.push_back(ch);
        RCLCPP_INFO(get_logger(), "Recieved %d", resp->resp.front());
    }

  private:
    rclcpp::Service<mg_msgs::srv::I2c>::SharedPtr i2c_srv_;
    int                                           i2c_fd_;
};

int main(int argc, const char* const* argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MgI2c>("i2cu"));
    rclcpp::shutdown();

    return 0;
}