#ifndef VISUALIZATION_H_
#define VISUALIZATION_H_

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <functional>
#include <map>
#include <memory>
#include <Eigen/Core>

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <cv_bridge/cv_bridge.h>

#include "read_configs.h"
#include "map_stitcher.h"
#include "map_builder.h"

class Visualizer {
public:
  enum class TrajectoryType {
    Frame = 0,
    KCC = 1,
    Odom = 2,
  };

  Visualizer(rclcpp::Node::SharedPtr node, VisualizationConfig& config);
  void AddNewPoseToPath(
    Eigen::Vector3d& pose, double time_double,
    nav_msgs::msg::Path& path, std::string& frame_id);
  void UpdateOdomPose(Eigen::Vector3d& pose, double time_double);
  void UpdateKccPose(Eigen::Vector3d& pose, double time_double);
  void UpdateFramePose(Aligned<std::vector, Eigen::Vector3d>& frame_poses,
                       std::vector<double>& timestamps);
  void ConvertMapToOccupancyMsgs(OccupancyData& map,
                                 nav_msgs::msg::OccupancyGrid& msgs);
  void UpdateMap(MapBuilder& map_builder);
  void PublishImage(cv::Mat& image, double time_double);
  void GetTrajectoryTxt(std::vector<std::vector<std::string>>& lines,
                        TrajectoryType trajectory_type);

private:
  rclcpp::Node::SharedPtr node_;
  std::string frame_id_;

  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr kcc_pose_pub_;
  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr frame_pose_pub_;
  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;

  nav_msgs::msg::Path odom_pose_msgs_;
  nav_msgs::msg::Path kcc_pose_msgs_;
  nav_msgs::msg::Path frame_pose_msgs_;
  nav_msgs::msg::OccupancyGrid occupancy_map_msgs_;
};

#endif  // VISUALIZATION_H_