#include "rqt_demo_plugin/rqt_demo_plugin.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "mg_msgs/msg/t_edit.hpp"
#include "mg_msgs/msg/t_edit_cycle.hpp"
#include "rqt_demo_plugin/lineItem.hpp"

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

#include "pluginlib/class_list_macros.hpp"
#include <mutex>
#include <qnamespace.h>
#include <qqml.h>
#include <qsurfaceformat.h>

PLUGINLIB_EXPORT_CLASS(mg::DemoPluginMg, rqt_gui_cpp::Plugin)

mg::DemoPluginMg::DemoPluginMg() :
  children_dirty(false), points_dirty(false), robotPos_(0, 0), robotTheta_(0), cchildren_(0) {
    QSurfaceFormat fmt;
    fmt.setSwapInterval(0);
    fmt.setRenderableType(QSurfaceFormat::OpenGLES);
    QSurfaceFormat::setDefaultFormat(fmt);
    static bool typesRegistered = false;
    if (!typesRegistered) {
        qmlRegisterType<LineItem>("LineItem", 1, 0, "LineItem");
        qmlRegisterAnonymousType<DemoPluginMg>("DemoPlugin", 1);
        typesRegistered = true;
    }
}

void mg::DemoPluginMg::cb_GraphEdits(mg_msgs::msg::TEditCycle::ConstSharedPtr msg) {
    std::lock_guard lock(tree_mut_);
    RCLCPP_INFO(node_->get_logger(), "Got edit: %u %u", msg->cnew_points, msg->cmodifications);
    for (int i = 0; i < msg->cnew_points; i++) {
        points_.emplace_back(msg->new_points[i].x, msg->new_points[i].y);
        children_.emplace_back();
    }
    for (int i = 0; i < msg->cmodifications; i++) {
        const mg_msgs::msg::TEdit modification = msg->modifications[i];
        if (modification.type == mg_msgs::msg::TEditCycle::DELETE) {
            children_[modification.idx].erase(modification.idy);
            cchildren_--;
        } else {
            children_[modification.idx].insert(modification.idy);
            cchildren_++;
        }
    }

    points_dirty   = msg->cnew_points > 0;
    children_dirty = msg->cmodifications > 0;
    RCLCPP_INFO(node_->get_logger(), "Got edit: %lu %u", points_.size(), cchildren_);
    emit treeChanged();
}

void mg::DemoPluginMg::cb_tf_timer() {

    try {
        double x = NAN;
        double y = NAN;

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

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

        robotPos_ = { vec3.x, vec3.y };
        robotPosChanged(robotPos_);
        robotThetaChanged(robotTheta_);
    } catch (const tf2::TransformException& ex) { }
}

void mg::DemoPluginMg::postPoint(qreal x, qreal y) {
    geometry_msgs::msg::PointStamped point;
    point.header.frame_id = "odom";
    point.point.x         = x;
    point.point.y         = y;
    pub_point_->publish(point);
}