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

/*
 * pose.cpp
 *
 *  Created on: Mar 14, 2012
 *      Author: Pablo Iñigo Blasco
 *
 *      To understand better how this is implemented see the references:
 *      - http://www.mrpt.org/2D_3D_Geometry
 *
 * Aug 17, 2019: Refactored into mrpt::ros1bridge library for MRPT 2.0 (JLBC)
 *
 */

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/Quaternion.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/math/CQuaternion.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFGaussianInf.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFGaussianInf.h>
#include <mrpt/ros1bridge/pose.h>
#include <tf2/LinearMath/Matrix3x3.h>

// MRPT -> ROS functions:

tf2::Matrix3x3 mrpt::ros1bridge::toROS(const mrpt::math::CMatrixDouble33& src)
{
  tf2::Matrix3x3 des;
  for (int r = 0; r < 3; r++)
    for (int c = 0; c < 3; c++) des[r][c] = src(r, c);
  return des;
}

tf2::Transform mrpt::ros1bridge::toROS_tfTransform(const mrpt::poses::CPose2D& src)
{
  return toROS_tfTransform(mrpt::poses::CPose3D(src));
}

geometry_msgs::Pose mrpt::ros1bridge::toROS_Pose(const mrpt::poses::CPose2D& src)
{
  return toROS_Pose(mrpt::poses::CPose3D(src));
}

tf2::Transform mrpt::ros1bridge::toROS_tfTransform(const mrpt::math::TPose2D& src)
{
  return toROS_tfTransform(mrpt::poses::CPose3D(mrpt::math::TPose3D(src)));
}

geometry_msgs::Pose mrpt::ros1bridge::toROS_Pose(const mrpt::math::TPose2D& src)
{
  geometry_msgs::Pose des;

  des.position.x = src.x;
  des.position.y = src.y;
  des.position.z = 0;

  const double yaw = src.phi;
  if (std::abs(yaw) < 1e-10)
  {
    des.orientation.x = 0.;
    des.orientation.y = 0.;
    des.orientation.z = .5 * yaw;
    des.orientation.w = 1.;
  }
  else
  {
    const double s = ::sin(yaw * .5);
    const double c = ::cos(yaw * .5);
    des.orientation.x = 0.;
    des.orientation.y = 0.;
    des.orientation.z = s;
    des.orientation.w = c;
  }

  return des;
}

tf2::Transform mrpt::ros1bridge::toROS_tfTransform(const mrpt::poses::CPose3D& src)
{
  tf2::Transform des;
  des.setBasis(toROS(src.getRotationMatrix()));
  des.setOrigin(tf2::Vector3(src.x(), src.y(), src.z()));
  return des;
}

geometry_msgs::Pose mrpt::ros1bridge::toROS_Pose(const mrpt::poses::CPose3D& src)
{
  geometry_msgs::Pose des;
  des.position.x = src.x();
  des.position.y = src.y();
  des.position.z = src.z();

  mrpt::math::CQuaternionDouble q;
  src.getAsQuaternion(q);

  des.orientation.x = q.x();
  des.orientation.y = q.y();
  des.orientation.z = q.z();
  des.orientation.w = q.r();

  return des;
}

tf2::Transform mrpt::ros1bridge::toROS_tfTransform(const mrpt::math::TPose3D& src)
{
  return toROS_tfTransform(mrpt::poses::CPose3D(src));
}

geometry_msgs::Pose mrpt::ros1bridge::toROS_Pose(const mrpt::math::TPose3D& src)
{
  return toROS_Pose(mrpt::poses::CPose3D(src));
}

geometry_msgs::PoseWithCovariance mrpt::ros1bridge::toROS_Pose(
    const mrpt::poses::CPose3DPDFGaussian& src)
{
  geometry_msgs::PoseWithCovariance des;
  des.pose = toROS_Pose(src.mean);

  // Read REP103: http://ros.org/reps/rep-0103.html#covariance-representation
  // # Row-major representation of the 6x6 covariance matrix
  // # The orientation parameters use a fixed-axis representation.
  // # In order, the parameters are:
  // # (x, y, z, rotation about X axis, rotation about Y axis, rotation about
  // Z axis)
  // float64[36] covariance
  // Old comment: "MRPT uses non-fixed axis for 6x6 covariance: should use a
  // transform Jacobian here!"
  //           JL ==> Nope! non-fixed z-y-x equals fixed x-y-z rotations.

  // X,Y,Z,YAW,PITCH,ROLL
  const unsigned int indxs_map[6] = {0, 1, 2, 5, 4, 3};
  for (int i = 0; i < 6; i++)
    for (int j = 0; j < 6; j++) des.covariance[indxs_map[i] * 6 + indxs_map[j]] = src.cov(i, j);
  return des;
}

geometry_msgs::PoseWithCovariance mrpt::ros1bridge::toROS(
    const mrpt::poses::CPose3DPDFGaussianInf& src)
{
  mrpt::poses::CPose3DPDFGaussian src2;
  src2.copyFrom(src);
  return toROS_Pose(src2);
}

geometry_msgs::PoseWithCovariance mrpt::ros1bridge::toROS(const mrpt::poses::CPosePDFGaussian& src)
{
  geometry_msgs::PoseWithCovariance des;

  des.pose = toROS_Pose(src.mean);

  // Read REP103: http://ros.org/reps/rep-0103.html#covariance-representation
  // Old comment: "MRPT uses non-fixed axis for 6x6 covariance: should use a
  // transform Jacobian here!"
  //           JL ==> Nope! non-fixed z-y-x equals fixed x-y-z rotations.

  // geometry_msgs/PoseWithCovariance msg stores the covariance matrix in
  // row-major representation
  // Indexes are :
  // [ 0   1   2   3   4   5  ]
  // [ 6   7   8   9   10  11 ]
  // [ 12  13  14  15  16  17 ]
  // [ 18  19  20  21  22  23 ]
  // [ 24  25  26  27  28  29 ]
  // [ 30  31  32  33  34  35 ]

  des.covariance[0] = src.cov(0, 0);
  des.covariance[1] = src.cov(0, 1);
  des.covariance[5] = src.cov(0, 2);
  des.covariance[6] = src.cov(1, 0);
  des.covariance[7] = src.cov(1, 1);
  des.covariance[11] = src.cov(1, 2);
  des.covariance[30] = src.cov(2, 0);
  des.covariance[31] = src.cov(2, 1);
  des.covariance[35] = src.cov(2, 2);

  return des;
}

geometry_msgs::PoseWithCovariance mrpt::ros1bridge::toROS(
    const mrpt::poses::CPosePDFGaussianInf& src)
{
  mrpt::poses::CPosePDFGaussian src2;
  src2.copyFrom(src);

  return toROS(src2);
}

geometry_msgs::Quaternion mrpt::ros1bridge::toROS(const mrpt::math::CQuaternionDouble& src)
{
  geometry_msgs::Quaternion des;
  des.x = src.x();
  des.y = src.y();
  des.z = src.z();
  des.w = src.r();
  return des;
}

// ROS -> MRPT functions:
mrpt::poses::CPose3D mrpt::ros1bridge::fromROS(const tf2::Transform& src)
{
  mrpt::poses::CPose3D des;
  const tf2::Vector3& t = src.getOrigin();
  des.x(t[0]);
  des.y(t[1]);
  des.z(t[2]);
  des.setRotationMatrix(fromROS(src.getBasis()));
  return des;
}
mrpt::math::CMatrixDouble33 mrpt::ros1bridge::fromROS(const tf2::Matrix3x3& src)
{
  mrpt::math::CMatrixDouble33 des;
  for (int r = 0; r < 3; r++)
    for (int c = 0; c < 3; c++) des(r, c) = src[r][c];
  return des;
}

mrpt::poses::CPose3D mrpt::ros1bridge::fromROS(const geometry_msgs::Pose& src)
{
  const mrpt::math::CQuaternionDouble q(
      src.orientation.w, src.orientation.x, src.orientation.y, src.orientation.z);
  return mrpt::poses::CPose3D(q, src.position.x, src.position.y, src.position.z);
}

mrpt::poses::CPose3DPDFGaussian mrpt::ros1bridge::fromROS(
    const geometry_msgs::PoseWithCovariance& src)
{
  mrpt::poses::CPose3DPDFGaussian des;

  des.mean = fromROS(src.pose);

  const unsigned int indxs_map[6] = {0, 1, 2, 5, 4, 3};

  for (int i = 0; i < 6; i++)
    for (int j = 0; j < 6; j++) des.cov(i, j) = src.covariance[indxs_map[i] * 6 + indxs_map[j]];

  return des;
}

mrpt::math::CQuaternionDouble mrpt::ros1bridge::fromROS(const geometry_msgs::Quaternion& src)
{
  mrpt::math::CQuaternionDouble des;
  des.x(src.x);
  des.y(src.y);
  des.z(src.z);
  des.r(src.w);
  return des;
}
