/* +------------------------------------------------------------------------+
   |                     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 <gtest/gtest.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/system/filesystem.h>
#include <test_mrpt_common.h>

TEST(NavTests, PTGs_tests)
{
  using namespace std;
  using namespace mrpt;
  using namespace mrpt::nav;

  const string sFil = mrpt::UNITTEST_BASEDIR() + string("/tests/PTGs_for_tests.ini");
  if (!mrpt::system::fileExists(sFil))
  {
    cerr << "**WARNING* Skipping tests since file cannot be found: '" << sFil << "'\n";
    return;
  }

  mrpt::config::CConfigFile cfg(sFil);

  const unsigned int PTG_COUNT = cfg.read_int("PTG_UNIT_TESTS", "PTG_COUNT", 0, true);
  EXPECT_TRUE(PTG_COUNT > 0);
  vector<CParameterizedTrajectoryGenerator::Ptr> PTGs(PTG_COUNT);

  for (unsigned int n = 0; n < PTG_COUNT; n++)
  {
    // Factory:
    const string sPTGName = cfg.read_string("PTG_UNIT_TESTS", format("PTG%u_Type", n), "", true);
    PTGs[n] = CParameterizedTrajectoryGenerator::CreatePTG(
        sPTGName, cfg, "PTG_UNIT_TESTS", format("PTG%u_", n));
    EXPECT_TRUE(PTGs[n] != nullptr) << "Failed creating PTG #" << n << endl;

    try
    {
      PTGs[n]->initialize(string(), false /*verbose */);
    }
    catch (const std::exception& e)
    {
      GTEST_FAIL() << "Failed initializing PTG #" << n << endl << e.what() << endl;
    }
  }

  // Run tests:
  // ---------------------------------------------------------
  for (unsigned int n = 0; n < PTG_COUNT; n++)
  {
    CParameterizedTrajectoryGenerator::Ptr ptg = PTGs[n];

    const std::string sPTGDesc = ptg->getDescription();
    const double refDist = ptg->getRefDistance();
    const size_t num_paths = ptg->getPathCount();
    size_t num_tests_run = 0;

    // TEST: step <-> dist match
    {
      for (double dist = 0.1; dist < refDist * 0.5; dist += 0.2)
      {
        bool any_good = false;
        for (size_t k = 0; k < num_paths; k++)
        {
          uint32_t step;

          if (ptg->getPathStepForDist(k, dist, step))
          {
            any_good = true;
            double d = ptg->getPathDist(k, step);
            EXPECT_NEAR(d, dist, 0.05) << "Test: step <-> dist match\n PTG: " << sPTGDesc << endl
                                       << "dist:" << dist << endl;
            num_tests_run++;
          }
        }
        EXPECT_TRUE(any_good) << "Test: step <-> dist match\n PTG: " << sPTGDesc << endl
                              << "dist:" << dist << endl;
      }
    }

    // TEST: inverseMap_WS2TP
    {
      bool any_ok = false;
      bool skip_this_ptg = false;
      for (double tx = -refDist * 0.5; !skip_this_ptg && tx < refDist * 0.5; tx += 0.1)
      {
        for (double ty = -refDist * 0.5; !skip_this_ptg && ty < refDist * 0.5; ty += 0.1)
        {
          if (std::abs(tx) < 1e-2 && std::abs(ty) < 1e-2)
            continue;  // TP-Space does not include the WS point
          // (0,0) in its domain

          const double tolerance_dist =
              std::max(0.10, 10.0 * std::sqrt(tx * tx + ty * ty) * M_PI * 2 / ptg->getPathCount());

          int k;
          double normalized_d;
          bool valid = ptg->inverseMap_WS2TP(tx, ty, k, normalized_d);
          if (valid && normalized_d < 1.0)
          {
            any_ok = true;
            // Now, do the inverse operation:
            uint32_t step;
            bool step_ok = ptg->getPathStepForDist(k, normalized_d * refDist, step);
            EXPECT_TRUE(step_ok) << "PTG: " << sPTGDesc << endl
                                 << "(tx,ty): " << tx << " " << ty << " k= " << k
                                 << " normalized_d=" << normalized_d << endl;
            if (step_ok)
            {
              const mrpt::math::TPose2D pose = ptg->getPathPose(k, step);
              EXPECT_NEAR(pose.x, tx, tolerance_dist)
                  << "Test: inverseMap_WS2TP\n PTG#" << n << ": " << sPTGDesc << endl
                  << "(tx,ty): " << tx << " " << ty << " k= " << k
                  << " normalized_d=" << normalized_d << endl;
              EXPECT_NEAR(pose.y, ty, tolerance_dist)
                  << "Test: inverseMap_WS2TP\n PTG#" << n << ": " << sPTGDesc << endl
                  << "(tx,ty): " << tx << " " << ty << " k= " << k
                  << " normalized_d=" << normalized_d << endl;

              if (std::abs(pose.x - tx) >= tolerance_dist ||
                  std::abs(pose.y - ty) >= tolerance_dist)
                skip_this_ptg = true;
              else
                num_tests_run++;
            }
          }
        }
      }
      EXPECT_TRUE(any_ok) << "PTG: " << sPTGDesc << endl;
    }

    // TEST: TP_obstacles
    {
      bool skip_this_ptg = false;
      bool any_change_all = false;
      for (double ox = -refDist * 0.5; !skip_this_ptg && ox < refDist * 0.5; ox += 0.1)
      {
        for (double oy = -refDist * 0.5; !skip_this_ptg && oy < refDist * 0.5; oy += 0.1)
        {
          if (std::abs(ox) < 1e-2 && std::abs(oy) < 1e-2)
            continue;  // TP-Space does not include the WS point
          // (0,0) in its domain

          std::vector<double> TP_obstacles;
          ptg->initTPObstacles(TP_obstacles);

          const std::vector<double> TP_obstacles_org = TP_obstacles;
          ptg->updateTPObstacle(ox, oy, TP_obstacles);

          const bool any_change = (TP_obstacles_org != TP_obstacles);
          if (any_change) any_change_all = true;
          num_tests_run++;
        }
      }
      EXPECT_TRUE(any_change_all);
    }

    printf("PTG `%50s` run %6u tests.\n", sPTGDesc.c_str(), (unsigned int)num_tests_run);

  }  // for each ptg
}
