/* +------------------------------------------------------------------------+
   |                     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 <CTraitsTest.h>
#include <gtest/gtest.h>
#include <mrpt/poses/CPosePDFGrid.h>

template class mrpt::CTraitsTest<mrpt::poses::CPosePDFGrid>;

TEST(CPosePDFGrid, defaultCtor)
{
  mrpt::poses::CPosePDFGrid pg;
  EXPECT_GE(pg.getXMax(), pg.getXMin());
  EXPECT_GE(pg.getYMax(), pg.getYMin());
  EXPECT_GE(pg.getPhiMax(), pg.getPhiMin());
}

TEST(CPosePDFGrid, resize)
{
  mrpt::poses::CPosePDFGrid pg;
  pg.setSize(0, 10.0, 0.0, 20.0, 1.0 /*res xy*/, 0.1 /*res phi*/);
}

TEST(CPosePDFGrid, basicOps)
{
  mrpt::poses::CPosePDFGrid pg;
  pg.setSize(0, 10.0, 0.0, 20.0, 1.0 /*res xy*/, 0.02 /*res phi*/);

  // Set:
  double* ptr_val = pg.getByPos(1.0, 2.0, 0.7);
  EXPECT_TRUE(ptr_val != nullptr);
  *ptr_val = 10.0;

  double* ptr_val2 = pg.getByPos(4.0, 7.0, -0.7);
  EXPECT_TRUE(ptr_val2 != nullptr);
  *ptr_val2 = 10.0;

  // Get:
  EXPECT_NEAR(*pg.getByPos(1.0, 2.0, 0.7), 10.0, 1e-5);
  EXPECT_NEAR(*pg.getByPos(4.0, 7.0, -0.7), 10.0, 1e-5);

  // Normalize:
  pg.normalize();
  EXPECT_NEAR(*pg.getByPos(1.0, 2.0, 0.7), 1.0 / 2.0, 1e-5);
  EXPECT_NEAR(*pg.getByPos(4.0, 7.0, -0.7), 1.0 / 2.0, 1e-5);

  // mean:
  const auto m = pg.getMeanVal();
  EXPECT_NEAR(m.x(), 2.5, 1e-4);
  EXPECT_NEAR(m.y(), 4.5, 1e-4);
  EXPECT_NEAR(m.phi(), 0.0, 5e-2);
}
