#ifndef C_API_CONVERTOR_H
#define C_API_CONVERTOR_H

// C
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "nav_msgs/Odometry.h"
#include "std_msgs/Header.h"
#include "nav_msgs/OccupancyGrid.h"
#include "geometry_msgs/Point.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Pose2D.h"
#include "nav_2d_msgs/Twist2D.h"
#include "nav_2d_msgs/Twist2DStamped.h"
#include "nav_2d_msgs/Path2D.h"
#include "map_msgs/OccupancyGridUpdate.h"
#include "geometry_msgs/PolygonStamped.h"
#include "protocol_msgs/Order.h"

// C++
#include <robot_sensor_msgs/LaserScan.h>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_nav_msgs/Odometry.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Pose2D.h>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <robot_geometry_msgs/PolygonStamped.h>
#include <robot_protocol_msgs/Order.h>

#include <move_base_core/navigation.h>
#include <move_base_core/common.h>

/**
 * @brief Convert C LaserScan to C++ LaserScan
 * @param laser_scan C LaserScan
 * @return C++ LaserScan
 */
robot_sensor_msgs::LaserScan convert2CppLaserScan(const LaserScan& laser_scan);

/**
 * @brief Convert C OccupancyGrid to C++ OccupancyGrid
 * @param occupancy_grid C OccupancyGrid
 * @return C++ OccupancyGrid
 */
robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occupancy_grid);

/**
 * @brief Convert C++ OccupancyGrid to C OccupancyGrid
 * @param cpp C++ OccupancyGrid
 * @param out C OccupancyGrid
 */
void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out);

/**
 * @brief Convert C++ Path2D to C Path2D
 */
void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out);

/**
 * @brief Convert C++ OccupancyGridUpdate to C OccupancyGridUpdate
 */
void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out);

/**
 * @brief Convert C++ PolygonStamped to C PolygonStamped
 */
void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out);

/**
 * @brief Convert C++ LaserScan to C LaserScan
 * @param cpp C++ LaserScan
 * @param out C LaserScan
 */
void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out);

/**
 * @brief Convert C++ PointCloud to C PointCloud
 * @param cpp C++ PointCloud
 * @param out C PointCloud
 */
void convert2CPointCloud(const robot_sensor_msgs::PointCloud& cpp, PointCloud& out);

/**
 * @brief Convert C++ PointCloud2 to C PointCloud2
 * @param cpp C++ PointCloud2
 * @param out C PointCloud2
 */
void convert2CPointCloud2(const robot_sensor_msgs::PointCloud2& cpp, PointCloud2& out);

/**
 * @brief Convert C PointCloud to C++ PointCloud
 * @param c C PointCloud
 * @return C++ PointCloud
 */
robot_sensor_msgs::PointCloud convert2CppPointCloud(const PointCloud& c);

/**
 * @brief Convert C PointCloud2 to C++ PointCloud2
 * @param c C PointCloud2
 * @return C++ PointCloud2
 */
robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c);

/**
 * @brief Convert C Odometry to C++ Odometry
 * @param odometry C Odometry
 * @return C++ Odometry
 */
robot_nav_msgs::Odometry convert2CppOdometry(const Odometry& odometry);

/**
 * @brief Convert C PoseStamped to C++ PoseStamped
 * @param pose_stamped C PoseStamped
 * @return C++ PoseStamped
 */
robot_geometry_msgs::PoseStamped convert2CppPoseStamped(const PoseStamped& pose_stamped);

/**
 * @brief Convert C++ PoseStamped to C PoseStamped
 * @param cpp_pose_stamped C++ PoseStamped
 * @return C PoseStamped
 */
PoseStamped convert2CPoseStamped(const robot_geometry_msgs::PoseStamped& cpp_pose_stamped);


/**
 * @brief Convert C Pose2D to C++ Pose2D
 * @param pose_2d C Pose2D
 * @return C++ Pose2D
 */
robot_geometry_msgs::Pose2D convert2CppPose2D(const Pose2D& pose_2d);

/**
 * @brief Convert C++ Pose2D to C Pose2D
 * @param cpp_pose_2d C++ Pose2D
 * @return C Pose2D
 */
 Pose2D convert2CPose2D(const robot_geometry_msgs::Pose2D& cpp_pose_2d);

/**
 * @brief Convert C Twist2D to C++ Twist2D
 * @param twist_2d C Twist2D
 * @return C++ Twist2D
 */
robot_nav_2d_msgs::Twist2D convert2CppTwist2D(const Twist2D& twist_2d);


/**
 * @brief Convert C++ Twist2D to C Twist2D
 * @param cpp_twist_2d C++ Twist2D
 * @return C Twist2D
 */
Twist2D convert2CTwist2D(const robot_nav_2d_msgs::Twist2D& cpp_twist_2d);

/**
 * @brief Convert C Twist2DStamped to C++ Twist2DStamped
 * @param twist_2d_stamped C Twist2DStamped
 * @return C++ Twist2DStamped
 */
robot_nav_2d_msgs::Twist2DStamped convert2CppTwist2DStamped(const Twist2DStamped& twist_2d_stamped);


/**
 * @brief Convert C++ Twist2DStamped to C Twist2DStamped
 * @param cpp_twist_2d_stamped C++ Twist2DStamped
 * @return C Twist2DStamped
 */
Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist_2d_stamped);

/**
 * @brief Convert C Order to C++ Order
 * @param order C Order
 * @return C++ Order
 */
robot_protocol_msgs::Order convert2CppOrder(const Order& order);

/**
 * @brief Convert C++ Order to C Order
 * @param cpp_order C++ Order
 * @return C Order (caller must call order_free when done to release memory)
 */
Order convert2COrder(const robot_protocol_msgs::Order& cpp_order);

/**
 * @brief Check if a quaternion is valid
 * @param q Quaternion
 * @return True if valid, false otherwise
 */
bool isQuaternionValid(const Quaternion& q);

/**
 * @brief Free dynamic memory held by an Order returned from convert2COrder
 * @param order Order to free (pointers and counts are zeroed)
 */
#ifdef __cplusplus
extern "C"
#endif
void order_free(Order* order);

#endif // C_API_CONVERTOR_H