/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <gtsam/geometry/Point3.h>
#include <gtsam/navigation/NavState.h>  // Velocity3
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/math/TTwist3D.h>

namespace mrpt::gtsam_wrappers
{
/** @name mrpt_gtsam_wrappers GTSAM <-> MRPT conversion functions
 *  \note New in MRPT 2.3.3
 *  \sa <mrpt/poses/gtsam_wrappers.h>
 * @{ */

static gtsam::Point3 toPoint3(const mrpt::math::TPoint3D& p)
{
  return gtsam::Point3(p.x, p.y, p.z);
}

static mrpt::math::TPoint3D toTPoint3(const gtsam::Point3& p) { return {p.x(), p.y(), p.z()}; }

static mrpt::math::TTwist3D toTTwist3D(const gtsam::Velocity3& v)
{
  mrpt::math::TTwist3D t;
  t.vx = v.x();
  t.vy = v.y();
  t.vz = v.z();
  return t;
}
static std::array<double, 3> toVelArray(const gtsam::Velocity3& v) { return {v.x(), v.y(), v.z()}; }

static mrpt::math::CMatrixDouble66 to_mrpt_se3_cov6(const gtsam::Matrix6& se3_cov)
{
  mrpt::math::CMatrixDouble66 C;
  // MRPT : X ,Y,Z,YAW,PITCH,ROLL
  // GTSAM: RX RY RZ X Y Z
  constexpr unsigned int mapping[6] = {5, 4, 3, 0, 1, 2};
  for (int i = 0; i < 6; i++)
    for (int j = 0; j < 6; j++) C(mapping[i], mapping[j]) = se3_cov(i, j);

  return C;
}

static gtsam::Matrix6 to_gtsam_se3_cov6_reordering(const mrpt::math::CMatrixDouble66& se3_cov)
{
  gtsam::Matrix6 C;
  // MRPT : X ,Y,Z,YAW,PITCH,ROLL
  // GTSAM: RX RY RZ X Y Z
  constexpr unsigned int mapping[6] = {5, 4, 3, 0, 1, 2};
  for (int i = 0; i < 6; i++)
    for (int j = 0; j < 6; j++) C(i, j) = se3_cov(mapping[i], mapping[j]);

  return C;
}

static gtsam::Matrix6 to_gtsam_se3_cov6_isotropic(const mrpt::math::CMatrixDouble66& se3_cov)
{
  gtsam::Matrix6 C = to_gtsam_se3_cov6_reordering(se3_cov);
  for (int i = 0; i < 6; i++)
    for (int j = 0; j < 6; j++)
      if (i != j) ASSERT_EQUAL_(C(i, j), 0);

  return C;
}

/** @} */

}  // namespace mrpt::gtsam_wrappers
