/* +------------------------------------------------------------------------+
   |                     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/apps/RBPF_SLAM_App.h>
#include <mrpt/maps/CBeaconMap.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/system/filesystem.h>
#include <test_mrpt_common.h>

#include <iostream>

using config_changer_t = std::function<void(mrpt::config::CConfigFileBase&)>;
using post_tester_t = std::function<void(mrpt::apps::RBPF_SLAM_App_Base&)>;

void generic_rbpf_slam_test(
    const std::string& ini_filename,
    const std::string& rawlog_filename,
    config_changer_t cfg_changer,
    post_tester_t post_tester)
{
  using namespace std::string_literals;

  const auto ini_fil =
      mrpt::UNITTEST_BASEDIR() + "/share/mrpt/config_files/rbpf-slam/"s + ini_filename;
  EXPECT_TRUE(mrpt::system::fileExists(ini_fil));

  const auto rawlog_fil = mrpt::UNITTEST_BASEDIR() + "/share/mrpt/datasets/"s + rawlog_filename;
  EXPECT_TRUE(mrpt::system::fileExists(rawlog_fil));

  try
  {
    mrpt::apps::RBPF_SLAM_App_Rawlog app;
    app.setMinLoggingLevel(mrpt::system::LVL_WARN);

    const char* argv[] = {"rbpf-slam", ini_fil.c_str(), rawlog_fil.c_str()};
    const int argc = sizeof(argv) / sizeof(argv[0]);

    app.initialize(argc, argv);

    app.params.write(
        "MappingApplication", "logOutput_dir", mrpt::system::getTempFileName() + "_dir"s);
    app.params.write("MappingApplication", "SHOW_PROGRESS_IN_WINDOW", false);

#if !MRPT_HAS_OPENCV
    app.params.write("MappingApplication", "SAVE_3D_SCENE", false);
    app.params.write("MappingApplication", "LOG_FREQUENCY", 0);
#endif

    cfg_changer(app.params);
    app.run();

    // Check results:
    post_tester(app);
  }
  catch (const std::exception& e)
  {
    std::cerr << mrpt::exception_to_str(e);
    GTEST_FAIL();
  }
}

static auto tester_for_2006_01_21 = [](mrpt::apps::RBPF_SLAM_App_Base& o)
{
  EXPECT_EQ(o.out_estimated_path.size(), 224U);
  const auto p = mrpt::poses::CPose3D(o.out_estimated_path.rbegin()->second);
  const auto p_gt =
      mrpt::poses::CPose3D::FromString("[3.4548 -18.0399 0.000000 -86.48 0.000000 0.000000]");

  EXPECT_LT(mrpt::poses::Lie::SE<3>::log(p - p_gt).norm(), 1.0)
      << "actual pose  =" << p.asString() << "\nexpected pose=" << p_gt.asString();
};

static auto tester_for_ROSLAM_demo = [](mrpt::apps::RBPF_SLAM_App_Base& o)
{
  EXPECT_EQ(o.out_estimated_path.size(), 99U);
  const auto p = mrpt::poses::CPose3D(o.out_estimated_path.rbegin()->second);
  const auto p_gt =
      mrpt::poses::CPose3D::FromString("[2.009005 3.330509 0.000000 115.004460 0.000000 0.000000]");

  EXPECT_LT(mrpt::poses::Lie::SE<3>::log(p - p_gt).norm(), 1.0)
      << "actual pose  =" << p.asString() << "\nexpected pose=" << p_gt.asString();

  const auto& maps = o.mapBuilder->getCurrentlyBuiltMetricMap().maps;
  ASSERT_EQUAL_(maps.size(), 1U);
  auto beaconMap = std::dynamic_pointer_cast<mrpt::maps::CBeaconMap>(maps.at(0));
  ASSERT_(beaconMap);

  // TODO(jlbc): check why actual LM mean positions seem not to match GT?
  // beaconMap->saveMetricMapRepresentationToFile("beaconMap");

  // there are 14 beacons:
  EXPECT_EQ(beaconMap->size(), 14UL);
};

TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_optimal_sampling)
{
  using namespace std::string_literals;

  generic_rbpf_slam_test(
      "gridmapping_optimal_sampling.ini", "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
      [](mrpt::config::CConfigFileBase&) {}, tester_for_2006_01_21);
}

TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_gridICP)
{
  using namespace std::string_literals;

  generic_rbpf_slam_test(
      "gridmapping_RBPF_grid_ICPbased_malaga.ini",
      "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
      [](mrpt::config::CConfigFileBase&) {}, tester_for_2006_01_21);
}

TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_pointsICP)
{
  using namespace std::string_literals;

  generic_rbpf_slam_test(
      "gridmapping_RBPF_ICPbased_malaga.ini",
      "2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog",
      [](mrpt::config::CConfigFileBase&) {}, tester_for_2006_01_21);
}

TEST(RBPF_SLAM_App, MapFromRawlog_ROSLAM_MC)
{
  using namespace std::string_literals;

  generic_rbpf_slam_test(
      "RO-SLAM_simulatedData_MC.ini", "RO-SLAM_demo.rawlog", [](mrpt::config::CConfigFileBase&) {},
      tester_for_ROSLAM_demo);
}

TEST(RBPF_SLAM_App, MapFromRawlog_ROSLAM_SOG)
{
  using namespace std::string_literals;

  generic_rbpf_slam_test(
      "RO-SLAM_simulatedData_SOG.ini", "RO-SLAM_demo.rawlog", [](mrpt::config::CConfigFileBase&) {},
      tester_for_ROSLAM_demo);
}
