#include "mg_tree_executor.hpp"

#include "behaviortree_cpp/xml_parsing.h"
#include "tree_nodes/calib.hpp"
#include "tree_nodes/choose_can.hpp"
#include "tree_nodes/choose_tactic.hpp"
#include "tree_nodes/i2c.hpp"
#include "tree_nodes/move_point.hpp"
#include "tree_nodes/move_local.hpp"
#include "tree_nodes/move_global.hpp"
#include "tree_nodes/look_at.hpp"
#include "tree_nodes/rotate.hpp"
#include "tree_nodes/side_pub.hpp"
#include "tree_nodes/set_pos.hpp"
#include "tree_nodes/zero.hpp"

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

namespace mg {
    MgTreeExecutor::MgTreeExecutor(const rclcpp::NodeOptions opts) : BT::TreeExecutionServer(opts) {
        executeRegistration();
        std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
        tf_buffer_   = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

        const std::function<std::pair<glm::vec2, double>()> func = std::bind(&MgTreeExecutor::position, this);
    }

    void MgTreeExecutor::onTreeCreated(BT::Tree& tree) { logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree); }

    void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
        factory.registerNodeType<mg::MovePointNode>("MovePoint", node());
        factory.registerNodeType<mg::MoveGlobalNode>("MoveGlobal", node());
        factory.registerNodeType<mg::RotateNode>("RotateNode", node());
        factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
        factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
        factory.registerNodeType<mg::SidePub>("SideObstaclePub", node());
        factory.registerNodeType<mg::SendPoseNode>("SendPose", node());
        factory.registerNodeType<mg::LookAtNode>("LookAtNode", node());
        factory.registerNodeType<mg::MoveLocalNode>("MoveLocal", node(), [this]() { return this->position(); });
        factory.registerNodeType<mg::CanChooser>("CanChooser");
        factory.registerNodeType<mg::TacticsChooser>("TacticChooser");
        factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
    }

    std::pair<glm::vec2, double> MgTreeExecutor::position() {
        double    x = NAN;
        double    y = NAN;
        double    theta;
        glm::vec2 pos;

        auto tmsg = tf_buffer_->lookupTransform("odom", "base-link", tf2::TimePointZero);

        tf2::Transform t;
        tf2::convert(tmsg.transform, t);
        t.getBasis().getRPY(x, y, theta);
        auto vec3 = tmsg.transform.translation;

        pos = glm::dvec2(vec3.x, vec3.y);
        return { pos, theta };
    }

    std::optional<std::string> MgTreeExecutor::onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) {
        (void)status;
        logger_cout_.reset();

        if (was_cancelled) {
            std::cout << "Behavior tree was cancled" << std::endl;
        }

        return std::nullopt;
    }
}