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

#pragma once
#include <mrpt/maps/CSimplePointsMap.h>

namespace mrpt::graphslam::deciders
{
template <class GRAPH_T>
void CRangeScanOps<GRAPH_T>::getICPEdge(
    const mrpt::obs::CObservation2DRangeScan& from,
    const mrpt::obs::CObservation2DRangeScan& to,
    constraint_t* rel_edge,
    const mrpt::poses::CPose2D* initial_pose_in,
    mrpt::slam::CICP::TReturnInfo* icp_info)
{
  MRPT_START

  mrpt::maps::CSimplePointsMap m1, m2;
  mrpt::slam::CICP::TReturnInfo info;

  // have them initialized prior - and then just clear them
  m1.insertObservation(from);
  m2.insertObservation(to);

  // If given, use initial_pose_in as a first guess for the ICP
  mrpt::poses::CPose2D initial_pose;
  if (initial_pose_in)
  {
    initial_pose = *initial_pose_in;
  }

  mrpt::poses::CPosePDF::Ptr pdf = params.icp.Align(&m1, &m2, initial_pose, info);

  // return the edge regardless of the goodness of the alignment
  rel_edge->copyFrom(*pdf);

  // if given, fill the TReturnInfo Struct
  if (icp_info) *icp_info = info;

  MRPT_END
}
template <class GRAPH_T>
void CRangeScanOps<GRAPH_T>::getICPEdge(
    const mrpt::obs::CObservation3DRangeScan& from,
    const mrpt::obs::CObservation3DRangeScan& to,
    constraint_t* rel_edge,
    const mrpt::poses::CPose2D* initial_pose_in,
    mrpt::slam::CICP::TReturnInfo* icp_info)
{
  MRPT_START

  ASSERTDEBMSG_(from.hasRangeImage, "Laser scan doesn't contain valid range image");
  ASSERTDEBMSG_(to.hasRangeImage, "Laser scan doesn't contain valid range image");

  // TODO - have this as a class member
  mrpt::maps::CSimplePointsMap m1, m2;
  mrpt::slam::CICP::TReturnInfo info;

  m1.insertObservation(from);
  m2.insertObservation(to);

  // If given, use initial_pose_in as a first guess for the ICP
  mrpt::poses::CPose3D initial_pose;
  if (initial_pose_in)
  {
    initial_pose = mrpt::poses::CPose3D(*initial_pose_in);
  }

  mrpt::poses::CPose3DPDF::Ptr pdf = params.icp.Align3D(&m1, &m2, initial_pose, info);

  // return the edge regardless of the goodness of the alignment
  // copy from the 3D PDF
  // NULLIFY contraint_t if Z displacement is high (normally this should be 0)
  // since we are moving in 2D
  if (fabs(pdf->getMeanVal().z()) < 0.1)
  {
    rel_edge->copyFrom(*pdf);
  }
  else
  {
    rel_edge = nullptr;
  }

  // if given, fill the TReturnInfo Struct
  if (icp_info) *icp_info = info;

  MRPT_END
}

template <class GRAPH_T>
void CRangeScanOps<GRAPH_T>::decimatePointsMap(
    mrpt::maps::CPointsMap* m, size_t keep_point_every, size_t low_lim)
{
  MRPT_START

  size_t map_size = m->size();

  if (low_lim)
  {
    // check if current keep_point_every variable is too large
    size_t conservative_keep_point_every = map_size / low_lim;
    keep_point_every = std::min(keep_point_every, conservative_keep_point_every);
  }

  // insert a false every "keep_point_every" points
  std::vector<bool> deletion_mask(map_size, true);
  for (size_t i = 0; i != map_size; ++i)
  {
    if (i % keep_point_every == 0)
    {
      deletion_mask[i] = false;
    }
  }
  m->applyDeletionMask(deletion_mask);

  // std::cout << "Map size: " << map_size << " => " << m->size() <<
  // std::endl;

  MRPT_END
}

template <class GRAPH_T>
bool CRangeScanOps<GRAPH_T>::convert3DTo2DRangeScan(
    mrpt::obs::CObservation3DRangeScan::Ptr& scan3D_in,
    mrpt::obs::CObservation2DRangeScan::Ptr* scan2D_out)
{
  MRPT_START

  bool success = false;
  // if it doesn't exist, create it
  if (!*scan2D_out)
  {
    *scan2D_out = mrpt::obs::CObservation2DRangeScan::Create();
  }

  if (scan3D_in->hasRangeImage)
  {
    scan3D_in->convertTo2DScan(**scan2D_out, params.conversion_params);
    success = true;
  }
  else
  {
    std::cout << "No valid rangeImage found" << std::endl;
    success = false;
  }

  return success;
  MRPT_END
}

template <class GRAPH_T>
void CRangeScanOps<GRAPH_T>::TParams::dumpToTextStream(std::ostream& out) const
{
  MRPT_START

  out << mrpt::format(
      "3D=>2D LaserScan Conversion Sensor label       = %s\n",
      conversion_params.sensorLabel.c_str());
  out << mrpt::format(
      "3D=>2D LaserScan Conversion angle sup          = %.2f deg\n",
      mrpt::RAD2DEG(conversion_params.angle_sup));
  out << mrpt::format(
      "3D=>2D LaserScan Conversion angle inf          = %.2f deg\n",
      mrpt::RAD2DEG(conversion_params.angle_inf));
  out << mrpt::format(
      "3D=>2D LaserScan Conversion oversampling ratio = %.2f\n",
      conversion_params.oversampling_ratio);
  out << mrpt::format(
      "3D=>2D LaserScan Conversion Z minimum          = %.2f\n", conversion_params.z_min);

  icp.options.dumpToTextStream(out);

  MRPT_END
}
template <class GRAPH_T>
void CRangeScanOps<GRAPH_T>::TParams::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& source, const std::string& section)
{
  MRPT_START

  conversion_params.sensorLabel =
      source.read_string(section, "conversion_sensor_label", "KINECT_TO_2D_SCAN", false);
  conversion_params.angle_sup =
      mrpt::DEG2RAD(source.read_double(section, "conversion_angle_sup", 10, false));
  conversion_params.angle_inf =
      mrpt::DEG2RAD(source.read_double(section, "conversion_angle_inf", 10, false));
  conversion_params.oversampling_ratio =
      source.read_double(section, "conversion_oversampling_ratio", 1.1, false);
  conversion_params.z_min =
      source.read_double(section, "conversion_z_min", 0, false);  // TODO - is this accurate?

  // load the icp parameters - from "ICP" section explicitly
  icp.options.loadFromConfigFile(source, "ICP");

  has_read_config = true;

  MRPT_END
}
}  // namespace mrpt::graphslam::deciders
