#pragma once

#include "geometry_msgs/msg/point.hpp"
#include "rqt_gui_cpp/plugin.h"
#include "mg_msgs/msg/t_edit_cycle.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

#include <QtGui>
#include <QtQuick>
#include <QtQuickControls2/QQuickStyle>

#include <QtWidgets>
#include <chrono>
#include <functional>
#include <memory>
#include <rclcpp/publisher.hpp>
#include <rclcpp/qos.hpp>
#include <unordered_set>
#include <vector>

inline void initResources() { Q_INIT_RESOURCE(res); }
inline void cleanupResources() { Q_CLEANUP_RESOURCE(res); }

namespace mg {

    class DemoPluginMg : public rqt_gui_cpp::Plugin {
        Q_OBJECT
        Q_PROPERTY(QObject* demoPlugin READ demoPlugin NOTIFY demoPluginChanged)
        Q_PROPERTY(QPointF robotPos READ robotPos NOTIFY robotPosChanged)
        Q_PROPERTY(qreal robotTheta READ robotTheta NOTIFY robotThetaChanged)

      public:
        std::mutex tree_mut_;
        bool       children_dirty;
        bool       points_dirty;

        DemoPluginMg();

        void initPlugin(qt_gui_cpp::PluginContext&) override;
        void shutdownPlugin() override { cleanupResources(); }

        Q_INVOKABLE void postPoint(qreal x, qreal y);

        QObject* demoPlugin() const { return (QObject*)(this); }
        QPointF  robotPos() const { return robotPos_; }
        float    robotTheta() const { return robotTheta_; }

        const std::vector<std::unordered_set<int>>& children() const { return children_; }
        const std::vector<std::pair<float, float>>& points() const { return points_; }

        int cchildren() const { return cchildren_; };

      signals:

        void demoPluginChanged(QObject* const);
        void robotPosChanged(QPointF const);
        void robotThetaChanged(float const);

        void treeChanged();

      private:
        std::shared_ptr<rclcpp::Subscription<mg_msgs::msg::TEditCycle>> editcycle_sub_;

        std::vector<std::unordered_set<int>> children_;
        std::vector<std::pair<float, float>> points_;

        std::shared_ptr<rclcpp::TimerBase>          tf_timer_;
        std::shared_ptr<tf2_ros::Buffer>            tf_buffer_;
        std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

        rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr pub_point_;

        QPointF robotPos_;
        qreal   robotTheta_;

        int cchildren_;

        void cb_GraphEdits(mg_msgs::msg::TEditCycle::ConstSharedPtr msg);
        void cb_tf_timer();
    };

    inline void DemoPluginMg::initPlugin(qt_gui_cpp::PluginContext& pc) {
        initResources();

        tf_buffer_   = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

        tf_timer_  = node_->create_timer(std::chrono::milliseconds(15), [this]() { this->cb_tf_timer(); });
        pub_point_ = node_->create_publisher<geometry_msgs::msg::PointStamped>("/clicked_point", rclcpp::QoS(1));

        QQuickStyle::setStyle("fusion");

        const QUrl qrl("qrc:/qml/helloworld.qml");
        auto*      qv = new QQuickView();

        auto editcycle_cb = std::bind(&DemoPluginMg::cb_GraphEdits, this, std::placeholders::_1);
        editcycle_sub_    = node_->create_subscription<mg_msgs::msg::TEditCycle>(
          "GraphEdits", rclcpp::QoS(1000).transient_local(), editcycle_cb);

        qv->rootContext()->setContextProperty("DemoPlug", this);
        qv->setSource(qrl);
        qv->setResizeMode(QQuickView::ResizeMode::SizeRootObjectToView);
        QSurfaceFormat format = qv->format();
        format.setSamples(4);
        qv->setFormat(format);
        QWidget* qw_ = QWidget::createWindowContainer(qv);
        pc.addWidget(qw_);
    }

}; // namespace mg