#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "mg_msgs/srv/i2c.hpp"
#include "mg_msgs/action/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;
    using I2cAction     = mg_msgs::action::I2c;
    using GoalHandleI2c = rclcpp_action::ServerGoalHandle<I2cAction>;

  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); };
        using namespace std::placeholders;
        i2c_action_ = rclcpp_action::create_server<I2cAction>(this,
                                                              "/i2c_action",
                                                              bind(&MgI2c::handle_goal, this, _1, _2),
                                                              bind(&MgI2c::handle_cancel, this, _1),
                                                              bind(&MgI2c::handle_accepted, this, _1));
    }

    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());
    }

    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
                                            std::shared_ptr<const I2cAction::Goal> goal) {
        RCLCPP_INFO(this->get_logger(), "Received goal request with addr %d and data %d", goal->addr, goal->data);
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleI2c>) {
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void handle_accepted(const std::shared_ptr<GoalHandleI2c> goal_handle) {
        using namespace std::placeholders;
        ioctl(i2c_fd_, I2C_SLAVE, goal_handle->get_goal()->addr); // NOLINT
        i2c_smbus_write_byte(i2c_fd_, goal_handle->get_goal()->data);
        int ch = 0;

        rclcpp::Rate rate(100);

        while (ch == 0 || (ch > 255 || ch < 0)) {
            ch = i2c_smbus_read_byte(i2c_fd_);
            rate.sleep();
        }
        auto res = std::make_shared<I2cAction::Result>();
        RCLCPP_INFO(get_logger(), "Recieved %d", ch);
        res->resp.push_back(ch);
        goal_handle->succeed(res);
        // this needs to return quickly to avoid blocking the executor, so spin up a new thread
    }

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

    rclcpp_action::Server<mg_msgs::action::I2c>::SharedPtr i2c_action_;

    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;
}