/* +------------------------------------------------------------------------+
   |                     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 <mrpt/3rdparty/tclap/CmdLine.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

using namespace mrpt;
using namespace mrpt::system;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::random;
using namespace mrpt::math;
using namespace mrpt::io;
using namespace mrpt::poses;
using namespace std;

void do_simulation();
void simulOdometry(
    const CPose2D& real_pose,
    CPose2D& last_pose,
    CPose2D& Apose,
    const CActionRobotMovement2D::TMotionModelOptions& odo_opts);

int LASER_N_RANGES;
double LASER_STD_ERROR;
double LASER_BEARING_STD_ERROR;
string grid_file;
string gt_file;
string out_rawlog_file, in_rawlog_file;

int main(int argc, char** argv)
{
  try
  {
    // Declare the supported options.
    TCLAP::CmdLine cmd("simul-gridmap", ' ', MRPT_getVersion().c_str());

    TCLAP::ValueArg<std::string> arg_grid(
        "g", "grid", "grid map file (*.gridmap or *.gridmap.gz)", true, "", "icp_goodness.txt",
        cmd);
    TCLAP::ValueArg<std::string> arg_poses(
        "p", "poses", "poses text file, one 'time x y phi' line per pose", true, "", "poses.txt",
        cmd);
    TCLAP::ValueArg<std::string> arg_out_rawlog(
        "o", "out-rawlog", "the output rawlog to generate  from which to take noisy odometry", true,
        "", "out.rawlog", cmd);
    TCLAP::ValueArg<std::string> arg_in_rawlog(
        "i", "in-rawlog", "(optional) the rawlog from which to take noisy odometry", false, "",
        "input.rawlog", cmd);
    TCLAP::ValueArg<int> arg_ranges(
        "r", "ranges", "number of laser ranges per scan (default=361)", false, 361,
        "icp_goodness.txt", cmd);
    TCLAP::ValueArg<double> arg_span(
        "s", "span", "span of the laser scans (default=180 degrees)", false, 180, "span", cmd);
    TCLAP::ValueArg<double> arg_std_r(
        "R", "std_r", "range noise sigma (default=0.01 meters)", false, 0.01, "std_r", cmd);
    TCLAP::ValueArg<double> arg_std_b(
        "B", "std_b", "bearing noise sigma (default=0.05 degrees)", false, 0.05, "std_b", cmd);

    TCLAP::SwitchArg arg_nologo("n", "nologo", "skip the logo at startup", cmd, false);

    // Parse arguments:
    if (!cmd.parse(argc, argv)) return 0;  // should exit.

    grid_file = arg_grid.getValue();
    gt_file = arg_poses.getValue();
    out_rawlog_file = arg_out_rawlog.getValue();
    in_rawlog_file = arg_in_rawlog.getValue();
    LASER_N_RANGES = arg_ranges.getValue();
    LASER_STD_ERROR = arg_std_r.getValue();
    LASER_BEARING_STD_ERROR = DEG2RAD(arg_std_b.getValue());

    if (arg_nologo.getValue())
    {
      printf(" simul-gridmap - Part of the MRPT\n");
      printf(
          " MRPT C++ Library: %s - Sources timestamp: %s\n", MRPT_getVersion().c_str(),
          MRPT_getCompilationDate().c_str());
    }

    // Invoke method:
    do_simulation();

    return 0;
  }
  catch (const std::exception& e)
  {
    cerr << mrpt::exception_to_str(e) << endl;
    return -1;
  }
  catch (...)
  {
    cerr << "Untyped exception." << endl;
    return -1;
  }
}

void do_simulation()
{
  ASSERT_FILE_EXISTS_(gt_file);

  bool have_in_rawlog;
  if (!in_rawlog_file.empty())
  {
    ASSERT_FILE_EXISTS_(in_rawlog_file);
    have_in_rawlog = true;
  }
  else
    have_in_rawlog = false;

  ASSERT_FILE_EXISTS_(grid_file);

  // Load the grid:
  COccupancyGridMap2D the_grid;
  CFileGZInputStream f(grid_file);
  mrpt::serialization::archiveFrom(f) >> the_grid;

  // GT file rows are:
  //  time x y z
  CMatrixDouble GT;
  GT.loadFromTextFile(gt_file);

  // Rawlog:
  CRawlog rawlog;
  if (have_in_rawlog)
  {
    rawlog.loadFromRawLogFile(in_rawlog_file);
    ASSERT_(rawlog.size() > 0);
    ASSERT_(rawlog.getType(0) == CRawlog::etActionCollection);
    ASSERT_(int(rawlog.size() / 2) == GT.rows());
  }

  // # of simulation steps:
  size_t N = GT.rows();

  // Assert sizes:
  ASSERT_(GT.cols() >= 4);

  // ------------------------------------------
  // Resimulate scans:
  // ------------------------------------------
  if (have_in_rawlog)
  {
    // Modify old rawlog in-place:
    for (size_t i = 1; i < rawlog.size(); i += 2)
    {
      ASSERT_(rawlog.getType(i) == CRawlog::etSensoryFrame);

      CSensoryFrame::Ptr sf = rawlog.getAsObservations(i);
      CPose2D gt_pose(GT(i / 2, 1), GT(i / 2, 2), GT(i / 2, 3));

      CObservation2DRangeScan::Ptr the_scan = sf->getObservationByClass<CObservation2DRangeScan>();
      the_grid.laserScanSimulator(
          *the_scan, gt_pose, 0.5f, LASER_N_RANGES, LASER_STD_ERROR, 1, LASER_BEARING_STD_ERROR);
    }
  }
  else
  {
    rawlog.clear();

    CActionCollection acts;
    CActionRobotMovement2D act;
    CActionRobotMovement2D::TMotionModelOptions odo_opts;

    odo_opts.modelSelection = CActionRobotMovement2D::mmGaussian;
    // odo_opts.gaussianModel.

    CPose2D last_pose(GT(0, 1), GT(0, 2), GT(0, 3));
    CPose2D real_pose = last_pose;
    CPose2D Apose;

    simulOdometry(real_pose, last_pose, Apose, odo_opts);
    act.computeFromOdometry(CPose2D(0, 0, 0), odo_opts);
    act.timestamp = mrpt::Clock::now();
    acts.insert(act);
    rawlog.insert(acts);

    // Create a rawlog from scratch:
    for (size_t i = 1; i < N; i++)
    {
      // simulate scan:
      real_pose = CPose2D(GT(i, 1), GT(i, 2), GT(i, 3));

      CSensoryFrame::Ptr sf = std::make_shared<CSensoryFrame>();

      CObservation2DRangeScan::Ptr the_scan = std::make_shared<CObservation2DRangeScan>();
      the_scan->aperture = M_PIf;
      the_scan->timestamp = mrpt::Clock::now();
      the_grid.laserScanSimulator(
          *the_scan, real_pose, 0.5f, LASER_N_RANGES, LASER_STD_ERROR, 1, LASER_BEARING_STD_ERROR);
      sf->insert(the_scan);
      rawlog.insert(sf);

      // Robot moves:
      simulOdometry(real_pose, last_pose, Apose, odo_opts);
      act.computeFromOdometry(CPose2D(0, 0, 0), odo_opts);
      act.timestamp = mrpt::Clock::now();
      acts.clear();
      acts.insert(act);
      rawlog.insert(acts);
    }
  }

  // Save the new rawlog:
  rawlog.saveToRawLogFile(out_rawlog_file);
}

void simulOdometry(
    const CPose2D& real_pose,
    CPose2D& last_pose,
    CPose2D& Apose,
    const CActionRobotMovement2D::TMotionModelOptions& odo_opts)
{
  Apose = real_pose - last_pose;
  last_pose = real_pose;

  // Noise:
  Apose.x_incr(getRandomGenerator().drawGaussian1D_normalized() * odo_opts.gaussianModel.minStdXY);
  Apose.y_incr(getRandomGenerator().drawGaussian1D_normalized() * odo_opts.gaussianModel.minStdXY);
  Apose.phi_incr(
      getRandomGenerator().drawGaussian1D_normalized() * odo_opts.gaussianModel.minStdPHI);
  Apose.normalizePhi();
}
