/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#include "math-precomp.h"  // Precompiled headers
//
#include <mrpt/math/TPoint2D.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TPose3D.h>

using namespace mrpt::math;

static_assert(std::is_trivially_copyable_v<TPose2D>);

TPose2D::TPose2D(const TPoint2D& p) : x(p.x), y(p.y), phi(0.0) {}
TPose2D::TPose2D(const TPoint3D& p) : x(p.x), y(p.y), phi(0.0) {}
TPose2D::TPose2D(const TPose3D& p) : x(p.x), y(p.y), phi(p.yaw) {}
void TPose2D::asString(std::string& s) const { s = mrpt::format("[%f %f %f]", x, y, RAD2DEG(phi)); }
void TPose2D::fromString(const std::string& s)
{
  CMatrixDouble m;
  if (!m.fromMatlabStringFormat(s))
    THROW_EXCEPTION_FMT("Malformed expression in ::fromString, s=\"%s\"", s.c_str());
  ASSERTMSG_(m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
  x = m(0, 0);
  y = m(0, 1);
  phi = DEG2RAD(m(0, 2));
}
mrpt::math::TPose2D mrpt::math::TPose2D::operator+(const mrpt::math::TPose2D& b) const
{
  const double A_cosphi = cos(this->phi), A_sinphi = sin(this->phi);
  // Use temporary variables for the cases (A==this) or (B==this)
  const double new_x = this->x + b.x * A_cosphi - b.y * A_sinphi;
  const double new_y = this->y + b.x * A_sinphi + b.y * A_cosphi;
  const double new_phi = mrpt::math::wrapToPi(this->phi + b.phi);

  return mrpt::math::TPose2D(new_x, new_y, new_phi);
}

mrpt::math::TPose2D mrpt::math::TPose2D::operator-(const mrpt::math::TPose2D& b) const
{
  const double B_cosphi = cos(b.phi), B_sinphi = sin(b.phi);

  const double new_x = (this->x - b.x) * B_cosphi + (this->y - b.y) * B_sinphi;
  const double new_y = -(this->x - b.x) * B_sinphi + (this->y - b.y) * B_cosphi;
  const double new_phi = mrpt::math::wrapToPi(this->phi - b.phi);

  return mrpt::math::TPose2D(new_x, new_y, new_phi);
}
TPoint2D TPose2D::composePoint(const TPoint2D l) const
{
  const double ccos = ::cos(phi), csin = ::sin(phi);
  return {x + l.x * ccos - l.y * csin, y + l.x * csin + l.y * ccos};
}
mrpt::math::TPoint2D TPose2D::operator+(const mrpt::math::TPoint2D& b) const
{
  return this->composePoint(b);
}

mrpt::math::TPoint2D TPose2D::inverseComposePoint(const TPoint2D g) const
{
  const double Ax = g.x - x, Ay = g.y - y, ccos = ::cos(phi), csin = ::sin(phi);
  return {Ax * ccos + Ay * csin, -Ax * csin + Ay * ccos};
}
