/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

/*---------------------------------------------------------------
	APPLICATION: mrpt_ros bridge
	FILE: imu.cpp
	AUTHOR: Raghavender Sahdev <raghavendersahdev@gmail.com>
  ---------------------------------------------------------------*/

#include <mrpt/ros1bridge/imu.h>

bool mrpt::ros1bridge::fromROS(
	const sensor_msgs::Imu& msg, mrpt::obs::CObservationIMU& obj)
{
	using namespace mrpt::obs;

	if (msg.orientation_covariance.at(0) >= 0)
	{
		obj.set(IMU_ORI_QUAT_X, msg.orientation.x);
		obj.set(IMU_ORI_QUAT_Y, msg.orientation.y);
		obj.set(IMU_ORI_QUAT_Z, msg.orientation.z);
		obj.set(IMU_ORI_QUAT_W, msg.orientation.w);
	}

	if (msg.linear_acceleration_covariance.at(0) >= 0)
	{
		obj.set(IMU_X_ACC, msg.linear_acceleration.x);
		obj.set(IMU_Y_ACC, msg.linear_acceleration.y);
		obj.set(IMU_Z_ACC, msg.linear_acceleration.z);
	}

	if (msg.angular_velocity_covariance.at(0) >= 0)
	{
		obj.set(IMU_WX, msg.angular_velocity.x);
		obj.set(IMU_WY, msg.angular_velocity.y);
		obj.set(IMU_WZ, msg.angular_velocity.z);
	}

	return true;
}

bool mrpt::ros1bridge::toROS(
	const mrpt::obs::CObservationIMU& obj, const std_msgs::Header& msg_header,
	sensor_msgs::Imu& msg)
{
	using namespace mrpt::obs;

	msg.header = msg_header;

	const auto& m = obj.rawMeasurements;
	if (obj.has(IMU_ORI_QUAT_X))
	{
		msg.orientation.x = m.at(IMU_ORI_QUAT_X);
		msg.orientation.y = m.at(IMU_ORI_QUAT_Y);
		msg.orientation.z = m.at(IMU_ORI_QUAT_Z);
		msg.orientation.w = m.at(IMU_ORI_QUAT_W);

		msg.orientation_covariance.fill(0.01);
	}
	else
	{
		msg.orientation_covariance.fill(-1);
	}

	if (obj.has(IMU_X_ACC))
	{
		msg.linear_acceleration.x = m.at(IMU_X_ACC);
		msg.linear_acceleration.y = m.at(IMU_Y_ACC);
		msg.linear_acceleration.z = m.at(IMU_Z_ACC);

		msg.linear_acceleration_covariance.fill(0.01);
	}
	else
	{
		msg.linear_acceleration_covariance.fill(-1);
	}

	if (obj.has(IMU_WX))
	{
		msg.angular_velocity.x = m.at(IMU_WX);
		msg.angular_velocity.y = m.at(IMU_WY);
		msg.angular_velocity.z = m.at(IMU_WZ);

		msg.angular_velocity_covariance.fill(0.01);
	}
	else
	{
		msg.angular_velocity_covariance.fill(-1);
	}

	return true;
}

/*
std_msgs/Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance
 */
