/*
 * ov_plane: Monocular Visual-Inertial Odometry with Planar Regularities
 * Copyright (C) 2022-2023 Chuchu Chen
 * Copyright (C) 2022-2023 Patrick Geneva
 * Copyright (C) 2022-2023 Guoquan Huang
 *
 * OpenVINS: An Open Platform for Visual-Inertial Research
 * Copyright (C) 2018-2023 Patrick Geneva
 * Copyright (C) 2018-2023 Guoquan Huang
 * Copyright (C) 2018-2023 OpenVINS Contributors
 * Copyright (C) 2018-2019 Kevin Eckenhoff
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#ifndef OV_PLANE_ROSVISUALIZER_HELPER_H
#define OV_PLANE_ROSVISUALIZER_HELPER_H

#include <Eigen/Eigen>

#if ROS_AVAILABLE == 1
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <tf/transform_broadcaster.h>
#endif

namespace ov_type {
class PoseJPL;
}

namespace ov_plane {

class State;
class VioManager;
class Simulator;

/**
 * @brief Helper class that handles some common versions into and out of ROS formats
 */
class ROSVisualizerHelper {

public:
#if ROS_AVAILABLE == 1
  /**
   * @brief Will visualize the system if we have new things
   * @param feats Vector of features we will convert into ros format
   * @return ROS pointcloud
   */
  static sensor_msgs::PointCloud2 get_ros_pointcloud(const std::vector<Eigen::Vector3d> &feats) {
    std::vector<Eigen::VectorXd> feats_tmp;
    for (auto const &feat : feats)
      feats_tmp.emplace_back(feat);
    return get_ros_pointcloud(feats_tmp);
  }

  /**
   * @brief Will visualize the system if we have new things
   * @param feats Vector of features we will convert into ros format
   * @return ROS pointcloud
   */
  static sensor_msgs::PointCloud2 get_ros_pointcloud(const std::vector<Eigen::VectorXd> &feats);
  /**
   * @brief Will visualize the system if we have new things
   * @param feats Vector of features we will convert into ros format
   * @param colors Map from size_t to RGB color
   * @return ROS pointcloud
   */
  static sensor_msgs::PointCloud2 get_ros_pointcloud(const std::map<size_t, std::vector<Eigen::Vector3d>> &feats,
                                                     const std::map<size_t, Eigen::Vector3d> &colors);

  /**
   * @brief Given a ov_type::PoseJPL this will convert into the ros format.
   *
   * NOTE: frame ids need to be handled externally!
   *
   * @param pose Pose with JPL quaternion (e.g. q_GtoI, p_IinG)
   * @param flip_trans If we should flip / inverse the translation
   * @return TF of our pose in global (e.g. q_ItoG, p_IinG)
   */
  static tf::StampedTransform get_stamped_transform_from_pose(const std::shared_ptr<ov_type::PoseJPL> &pose, bool flip_trans);
#endif

  /**
   * @brief Save current estimate state and groundtruth including calibration
   * @param state Pointer to the state
   * @param sim Pointer to the simulator (or null)
   * @param of_state_est Output file for state estimate
   * @param of_state_std Output file for covariance
   * @param of_state_gt Output file for groundtruth (if we have it from sim)
   */
  static void sim_save_total_state_to_file(std::shared_ptr<State> state, std::shared_ptr<Simulator> sim, std::ofstream &of_state_est,
                                           std::ofstream &of_state_std, std::ofstream &of_state_gt);

private:
  // Cannot create this class
  ROSVisualizerHelper() = default;
};

} // namespace ov_plane

#endif // OV_PLANE_ROSVISUALIZER_HELPER_H
