#include <array>
#include <chrono>
#include <cstddef>
#include <functional>
#include <memory>
#include <rclcpp/service.hpp>
#include <string>
#include <libserial/SerialStream.h>

#include "mg_msgs/srv/set_width.hpp"
#include "std_srvs/srv/empty.hpp"

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/transform_broadcaster.h"

#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/LinearMath/Transform.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2/convert.hpp"

#include "rclcpp/rclcpp.hpp"

#define TIMEOUT 10u

constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";

using SetWidthSrv = mg_msgs::srv::SetWidth;
using ZeroSrv     = std_srvs::srv::Empty;

class MgOdomPublisher : public rclcpp::Node {
  public:
    MgOdomPublisher() : Node("mg_odom_publisher") {
        this->declare_parameter("odom", "odom");
        this->declare_parameter("target", "base-link");
        this->declare_parameter("serial_path", ENCODER_SERIAL_PATH_DEFAULT);

        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

        timer_ = this->create_wall_timer(std::chrono::milliseconds(TIMEOUT),
                                         std::bind(&MgOdomPublisher::timer_callback, this));

        set_width_service_ = create_service<SetWidthSrv>(
          "/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));

        zero_service_
          = create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
    }

  private:
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    rclcpp::Service<SetWidthSrv>::SharedPtr        set_width_service_;
    rclcpp::Service<ZeroSrv>::SharedPtr            zero_service_;
    rclcpp::TimerBase::SharedPtr                   timer_;
    LibSerial::SerialStream                        enc_serial_port_;

    void set_width(const std::shared_ptr<SetWidthSrv::Request> request) {
        RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width);

        if (!enc_serial_port_.IsOpen()) {
            union {
                std::array<double, sizeof(double)> bytes;
                double                             data;
            } value{};
            value.data = request->width;

            enc_serial_port_ << "w;";
            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
        }
    }

    void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
        RCLCPP_INFO(get_logger(), "Zeroing odometry");

        if (!enc_serial_port_.IsOpen()) {
            enc_serial_port_ << "z;";
        }
    }

    void timer_callback() {
        double _x     = NAN;
        double _y     = NAN;
        double _theta = NAN;

        RCLCPP_DEBUG(this->get_logger(), "Callback called");

        try {
            if (!enc_serial_port_.IsOpen()) {
                enc_serial_port_.Open(this->get_parameter("serial_path").as_string());
            }

            enc_serial_port_ << "g;";

            if (!(enc_serial_port_ >> _x >> _y >> _theta)) {
                RCLCPP_INFO(this->get_logger(), "Reading Serial Port failed. Closing...");
                throw "Unfortunately bug with libserial";
            }

            RCLCPP_DEBUG(this->get_logger(), "Got following from rpi:{ x: %lf, y: %lf, z: %lf}\n", _x, _y, _theta);

            make_transform(_x, _y, _theta);
        } catch (const std::exception& e) { RCLCPP_ERROR(this->get_logger(), "%s\n", e.what()); }
    }

    void make_transform(double x, double y, double theta) {
        geometry_msgs::msg::TransformStamped t;

        t.header.stamp    = this->get_clock()->now();
        t.header.frame_id = this->get_parameter("odom").as_string();
        t.child_frame_id  = this->get_parameter("target").as_string();

        tf2::Quaternion q;
        q.setRPY(0, 0, theta);
        const tf2::Transform tf(tf2::Transform(q, tf2::Vector3(x, y, 0)));
        tf2::convert(tf, t.transform);

        tf_broadcaster_->sendTransform(t);
    }
};

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

    return 0;
}