/* +------------------------------------------------------------------------+
   |                     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 "obs-precomp.h"  // Precompiled headers
//
#include <mrpt/core/lock_helper.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CSinCosLookUpTableFor2DScans.h>

using namespace std;
using namespace mrpt::obs;

/** Return two vectors with the cos and the sin of the angles for each of the
 * rays in a scan, computing them only the first time and returning a cached
 * copy the rest.
 */
const CSinCosLookUpTableFor2DScans::TSinCosValues& CSinCosLookUpTableFor2DScans::getSinCosForScan(
    const CObservation2DRangeScan& scan) const
{
  T2DScanProperties scan_prop;
  scan.getScanProperties(scan_prop);
  return getSinCosForScan(scan_prop);
}

/** Return two vectors with the cos and the sin of the angles for each of the
 * rays in a scan, computing them only the first time and returning a cached
 * copy the rest.
 */
const CSinCosLookUpTableFor2DScans::TSinCosValues& CSinCosLookUpTableFor2DScans::getSinCosForScan(
    const T2DScanProperties& scan_prop) const
{
  auto lck = mrpt::lockHelper(m_cache_mtx);

  auto it = m_cache.find(scan_prop);
  if (it != m_cache.end())
  {  // Found in the cache:
    return it->second;
  }
  else
  {
    // If there're too many LUTs, something is wrong, so just free the
    // memory:
    // (If you someday find someone with TWENTY *different* laser scanner
    // models,
    //   please, accept my apologies... but send me a photo of them all!!
    //   ;-)
    if (m_cache.size() > 20) m_cache.clear();

    // Compute and insert in the cache:
    TSinCosValues& new_entry = m_cache[scan_prop];

    // Make sure the allocated memory at least have room for 4 extra
    // double's at the end,
    //  for the case we use these buffers for SSE2 optimized code. If the
    //  final values are uninitialized it doesn't matter.
    new_entry.ccos.resize(scan_prop.nRays + 4);
    new_entry.csin.resize(scan_prop.nRays + 4);

    ASSERT_(scan_prop.nRays >= 2);
    if (scan_prop.nRays > 0)
    {
      double Ang = (scan_prop.rightToLeft ? -0.5 : +0.5) * scan_prop.aperture;
      const double dA =
          (scan_prop.rightToLeft ? 1.0 : -1.0) * (scan_prop.aperture / (scan_prop.nRays - 1));

      for (size_t i = 0; i < scan_prop.nRays; i++)
      {
        new_entry.ccos[i] = d2f(cos(Ang));
        new_entry.csin[i] = d2f(sin(Ang));
        Ang += dA;
      }
    }
    // return them:
    return new_entry;
  }
}
