/* +------------------------------------------------------------------------+
   |                     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 "slam-precomp.h"  // Precompiled headers
//
#include <mrpt/maps/CLandmark.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/utils.h>
#include <mrpt/obs/CObservationBeaconRanges.h>
#include <mrpt/slam/CRejectionSamplingRangeOnlyLocalization.h>

using namespace mrpt::math;
using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace std;

/*---------------------------------------------------------------
            Constructor
---------------------------------------------------------------*/
CRejectionSamplingRangeOnlyLocalization::CRejectionSamplingRangeOnlyLocalization() :
    m_oldPose(),

    m_dataPerBeacon()
{
}

/*---------------------------------------------------------------
          RS_drawFromProposal
---------------------------------------------------------------*/
void CRejectionSamplingRangeOnlyLocalization::RS_drawFromProposal(CPose2D& outSample)
{
  MRPT_START

  // Use the first beacon data to draw a pose:
  if (m_dataPerBeacon.size() == 0)
    THROW_EXCEPTION(
        "There is no information from which to draw samples!! Use "
        "'setParams' with valid data!");

  ASSERT_(m_drawIndex < m_dataPerBeacon.size());

  float ang = getRandomGenerator().drawUniform(
      m_dataPerBeacon[m_drawIndex].minAngle, m_dataPerBeacon[m_drawIndex].maxAngle);
  float R = getRandomGenerator().drawGaussian1D(
      m_dataPerBeacon[m_drawIndex].radiusAtRobotPlane, m_sigmaRanges);

  // This is the point where the SENSOR is:
  outSample.x(m_dataPerBeacon[m_drawIndex].beaconPosition.x + cos(ang) * R);
  outSample.y(m_dataPerBeacon[m_drawIndex].beaconPosition.y + sin(ang) * R);

  outSample.phi(getRandomGenerator().drawGaussian1D(m_oldPose.phi(), 2.0_deg));

  // Compute the robot pose P.
  //	  P = SAMPLE - ROT · SENSOR_ON_ROBOT
  mrpt::math::TPoint2D on(
      m_dataPerBeacon[m_drawIndex].sensorOnRobot.x, m_dataPerBeacon[m_drawIndex].sensorOnRobot.y);
  mrpt::math::TPoint2D S(outSample.x(), outSample.y());
  on = mrpt::math::TPoint2D(mrpt::math::TPose2D(0, 0, outSample.phi()) + on);
  S = S - on;

  outSample.x(S.x);
  outSample.y(S.y);

  MRPT_END
}

/*---------------------------------------------------------------
          RS_observationLikelihood
---------------------------------------------------------------*/
double CRejectionSamplingRangeOnlyLocalization::RS_observationLikelihood(const CPose2D& x)
{
  MRPT_START

  double lik = 1.0;
  double m_sigmaRanges2 = square(m_sigmaRanges);

  // Evaluate the likelihood for all the observations but the "m_drawIndex":
  for (size_t i = 0; i < m_dataPerBeacon.size(); i++)
  {
    // TODO: height now includes the sensor "z"!!!...
    CPoint3D P(
        x + CPoint3D(m_dataPerBeacon[i].sensorOnRobot.x, m_dataPerBeacon[i].sensorOnRobot.y, 0));

    if (i != m_drawIndex)
      // Evalute:
      lik *=
          exp(-0.5 *
              square(
                  m_dataPerBeacon[i].radiusAtRobotPlane -
                  P.distanceTo(mrpt::math::TPoint3D(m_dataPerBeacon[i].beaconPosition))) /
              m_sigmaRanges2);
  }

  return lik;

  MRPT_END
}

/*---------------------------------------------------------------
          setParams
---------------------------------------------------------------*/
bool CRejectionSamplingRangeOnlyLocalization::setParams(
    const CLandmarksMap& beaconsMap,
    const CObservationBeaconRanges& observation,
    float sigmaRanges,
    const CPose2D& oldPose,
    float robot_z,
    bool autoCheckAngleRanges)

{
  MRPT_START

  // Various vars:
  m_z_robot = robot_z;
  m_oldPose = oldPose;
  m_sigmaRanges = sigmaRanges;  // TODO: observation.stdError;

  double xMin = 1e30, xMax = -1e30, yMin = 1e30, yMax = -1e30, gridRes = 2 * m_sigmaRanges;
  float R;

  // Save each observed beacon data:
  m_dataPerBeacon.clear();

  // Minimum radius:
  std::deque<mrpt::obs::CObservationBeaconRanges::TMeasurement>::const_iterator it;
  size_t i;

  // For each observation:
  for (i = 0, it = observation.sensedData.begin(); it != observation.sensedData.end(); ++it, ++i)
  {
    // Is in the map?
    const CLandmark* lm = beaconsMap.landmarks.getByBeaconID(it->beaconID);
    if (lm != nullptr)
    {
      TDataPerBeacon data;

      data.sensorOnRobot = it->sensorLocationOnRobot.asTPoint();

      data.beaconPosition.x = lm->pose_mean.x;
      data.beaconPosition.y = lm->pose_mean.y;

      // First compute squared:
      data.radiusAtRobotPlane = square(it->sensedDistance) - square(lm->pose_mean.z - robot_z);

      if (data.radiusAtRobotPlane > 0)
      {
        data.radiusAtRobotPlane = sqrt(data.radiusAtRobotPlane);
        data.minAngle = -M_PIf;
        data.maxAngle = M_PIf;
        m_dataPerBeacon.push_back(data);

        // std::cout << "BEACON: " << m_dataPerBeacon.size() << "\n   "
        // <<data.beaconPosition << " R=" << data.radiusAtRobotPlane <<
        // "\n";

        // Keep max/min:
        xMin =
            min(xMin, data.beaconPosition.x - (data.radiusAtRobotPlane + 4 * m_sigmaRanges + 1.0));
        xMax =
            max(xMax, data.beaconPosition.x + (data.radiusAtRobotPlane + 4 * m_sigmaRanges + 1.0));
        yMin =
            min(yMin, data.beaconPosition.y - (data.radiusAtRobotPlane + 4 * m_sigmaRanges + 1.0));
        yMax =
            max(yMax, data.beaconPosition.y + (data.radiusAtRobotPlane + 4 * m_sigmaRanges + 1.0));
      }
      else
      {
        printf(
            "\nWARNING: Beacon range is shorter than distance between "
            "the robot and the beacon in the Z axis!!!: Ignoring this "
            "measurement\n");
      }
    }
  }  // end for

  // ------------------------------------------------------------------------
  // Precompute the min/max angles for potential samples, for each beacon:
  // ------------------------------------------------------------------------
  if (autoCheckAngleRanges && m_dataPerBeacon.size() > 1)
  {
    // Build the grid:
    // Each cell in the grid is a vector of bools, each one indicating
    // whether some samples from the i'th beacon falls there.
    mrpt::containers::CDynamicGrid<std::vector<bool>> grid(xMin, xMax, yMin, yMax, gridRes);
    grid.fill(std::vector<bool>(m_dataPerBeacon.size(), false));
    std::vector<bool>* cell;

    // The ngular step size:
    float Aa = 5.0_deg;

    // Fill the grid:
    for (i = 0; i < m_dataPerBeacon.size(); i++)
    {
      for (float a = -M_PIf; a <= M_PIf; a += Aa)
      {
        // Radius R - 3*SIGMA:
        R = m_dataPerBeacon[i].radiusAtRobotPlane - 3.0f * m_sigmaRanges;
        R = max(R, 0.0f);
        cell = grid.cellByPos(
            m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
            m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
        ASSERT_(cell != nullptr);
        (*cell)[i] = true;
        // Radius R :
        R = m_dataPerBeacon[i].radiusAtRobotPlane;
        cell = grid.cellByPos(
            m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
            m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
        ASSERT_(cell != nullptr);
        (*cell)[i] = true;
        // Radius R + 3*SIGMA:
        R = m_dataPerBeacon[i].radiusAtRobotPlane + 3.0f * m_sigmaRanges;
        cell = grid.cellByPos(
            m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
            m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
        ASSERT_(cell != nullptr);
        (*cell)[i] = true;
      }  // end for a
    }    // end for i

    // Check the angles:
    for (i = 0; i < m_dataPerBeacon.size(); i++)
    {
      float maxA = -M_PIf, minA = M_PIf;
      for (float a = -M_PIf; a < M_PIf; a += Aa)
      {
        // Radius R - 3*SIGMA:
        R = m_dataPerBeacon[i].radiusAtRobotPlane - 3.0f * m_sigmaRanges;
        R = max(R, 0.0f);
        cell = grid.cellByPos(
            m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
            m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
        ASSERT_(cell != nullptr);
        if (std::count(cell->begin(), cell->end(), true) > 1)
        {
          maxA = max(maxA, a);
          minA = min(minA, a);
        }
        // Radius R :
        R = m_dataPerBeacon[i].radiusAtRobotPlane;
        cell = grid.cellByPos(
            m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
            m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
        ASSERT_(cell != nullptr);
        if (std::count(cell->begin(), cell->end(), true) > 1)
        {
          maxA = max(maxA, a);
          minA = min(minA, a);
        }
        // Radius R + 3*SIGMA:
        R = m_dataPerBeacon[i].radiusAtRobotPlane + 3.0f * m_sigmaRanges;
        cell = grid.cellByPos(
            m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
            m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
        ASSERT_(cell != nullptr);
        if (std::count(cell->begin(), cell->end(), true) > 1)
        {
          maxA = max(maxA, a);
          minA = min(minA, a);
        }
      }  // end for a
      m_dataPerBeacon[i].minAngle = max(-M_PIf, minA - Aa);
      m_dataPerBeacon[i].maxAngle = min(M_PIf, maxA + Aa);
    }  // end for i
  }    // end if >1 beacons

  // Select best candidate for "m_drawIndex":
  float minCoberture = 1e30f;
  m_drawIndex = 0;
  for (i = 0; i < m_dataPerBeacon.size(); i++)
  {
    float c = m_dataPerBeacon[i].radiusAtRobotPlane *
              (m_dataPerBeacon[i].maxAngle - m_dataPerBeacon[i].minAngle);
    if (c < minCoberture)
    {
      minCoberture = c;
      m_drawIndex = i;
    }
  }  // end for i

  // End!
  return m_dataPerBeacon.size() != 0;
  MRPT_END
}
