#ifndef SWEET_RVIZ_PANEL_HPP
#define SWEET_RVIZ_PANEL_HPP

#include <memory>
#include <QTimer>
#include <QLabel>
#include <QPushButton>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QWidget>
#include <QVector>
#include <QElapsedTimer>
#include <rviz_common/panel.hpp>
#include "sweet_rviz_panel/qcustomplot.h"
#include <rclcpp/rclcpp.hpp>
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include <nav_msgs/msg/path.hpp>
#include <std_msgs/msg/float64.hpp>
#include <rviz_common/display_context.hpp>
#include <rviz_common/ros_integration/ros_node_abstraction_iface.hpp>

class QCustomPlot;
class QCPGraph;
namespace sweet_rviz_panel
{

class SweetRvizPanel : public rviz_common::Panel
{
  Q_OBJECT

public:
  explicit SweetRvizPanel(QWidget *parent = nullptr);

protected:
  void onInitialize() override;

private Q_SLOTS:
  void onStart();
  void onStop();

private:
  void poseCallback(const geometry_msgs::msg::Pose::SharedPtr msg);
  void velCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
  void pathCallback(const nav_msgs::msg::Path::SharedPtr msg);
  void calculateData();
  void updateDataPlot();
  void initPlots();

  QLabel *x_label_;
  QLabel *y_label_;
  QLabel *yaw_label_;
  QLabel *status_label_;

  QLabel *vel_cmd_label_;
  QLabel *yaw_cmd_label_;
  QPushButton *start_button_;
  QPushButton *stop_button_;
  QTimer *timer_;
  QCustomPlot *lateral_plot_;
  QCustomPlot *heading_plot_;
  QCustomPlot *yaw_cmd_plot_;
  rclcpp::Node::SharedPtr node_;
  std::shared_ptr<
  rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
  rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr pose_sub_;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_cmd_sub_;
  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr odom_path_sub_;
  double current_x_ = 0.0;
  double current_y_ = 0.0;
  double current_yaw_ = 0.0;
  bool pose_received_ = false;
  bool path_received_ = false;
  nav_msgs::msg::Path current_path_;
  QVector<double> error_time_;
  QVector<double> lateral_errors_;
  QVector<double> heading_errors_;
  QVector<double> yaw_cmd_;
  QElapsedTimer error_timer_;
  static constexpr int MAX_ERROR_POINTS = 110;
  static constexpr int TIME_LIMIT = 10;
  double search_distance_ = 10.0;
  int start_index_=0;
  int forward_index_ = 100;

  double cur_vel_cmd_ = 0.0;
  double cur_yaw_cmd_ = 0.0;
  bool running_;
};

}  // namespace sweet_rviz_panel

#endif