/* +------------------------------------------------------------------------+
   |                     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 "hwdrivers-precomp.h"  // Precompiled headers
//
#include <mrpt/hwdrivers/C2DRangeFinderAbstract.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CPlanarLaserScan.h>  // in library mrpt-maps
#include <mrpt/system/os.h>

using namespace std;
using namespace mrpt::obs;
using namespace mrpt::io;
using namespace mrpt::hwdrivers;

/*-------------------------------------------------------------
            Constructor
-------------------------------------------------------------*/
C2DRangeFinderAbstract::C2DRangeFinderAbstract() :
    mrpt::system::COutputLogger("C2DRangeFinderAbstract")
{
}

/*-------------------------------------------------------------
            Destructor
-------------------------------------------------------------*/
C2DRangeFinderAbstract::~C2DRangeFinderAbstract() = default;

void C2DRangeFinderAbstract::bindIO(const std::shared_ptr<CStream>& streamIO)
{
  m_csChangeStream.lock();
  m_stream = streamIO;
  m_csChangeStream.unlock();
}

/*-------------------------------------------------------------
            getObservation
-------------------------------------------------------------*/
void C2DRangeFinderAbstract::getObservation(
    bool& outThereIsObservation,
    mrpt::obs::CObservation2DRangeScan& outObservation,
    bool& hardwareError)
{
  m_csLastObservation.lock();

  hardwareError = m_hardwareError;
  outThereIsObservation = m_lastObservationIsNew;

  if (outThereIsObservation) outObservation = m_lastObservation;

  m_csLastObservation.unlock();
}

/*-------------------------------------------------------------
            doProcess
-------------------------------------------------------------*/
void C2DRangeFinderAbstract::doProcess()
{
  bool thereIs, hwError;

  if (!m_nextObservation) m_nextObservation = std::make_shared<CObservation2DRangeScan>();

  doProcessSimple(thereIs, *m_nextObservation, hwError);

  if (hwError)
  {
    m_state = ssError;
    MRPT_LOG_THROTTLE_ERROR(5.0, "Error reading from the sensor hardware. Will retry.");
  }

  if (thereIs)
  {
    m_state = ssWorking;

    appendObservation(m_nextObservation);
    m_nextObservation.reset();  // Create a new object in the next call
  }
}

void C2DRangeFinderAbstract::internal_notifyGoodScanNow()
{
  const auto new_t = mrpt::Clock::now();

  if (m_last_good_scan != INVALID_TIMESTAMP)
  {
    m_estimated_scan_period =
        0.9 * m_estimated_scan_period + 0.1 * mrpt::system::timeDifference(m_last_good_scan, new_t);
  }
  m_last_good_scan = new_t;
  m_failure_waiting_scan_counter = 0;
}

// Returns true if ok, false if this seems to be an error
bool C2DRangeFinderAbstract::internal_notifyNoScanReceived()
{
  if (m_last_good_scan == INVALID_TIMESTAMP) return true;

  const double dt = mrpt::system::timeDifference(m_last_good_scan, mrpt::Clock::now());

  if (dt > 1.50 * m_estimated_scan_period)
    if (++m_failure_waiting_scan_counter >= m_max_missed_scan_failures) return false;

  return true;
}

/*-------------------------------------------------------------
            loadExclusionAreas
-------------------------------------------------------------*/
void C2DRangeFinderAbstract::loadCommonParams(
    const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
{
  // Params:
  m_showPreview = configSource.read_bool(iniSection, "preview", false);

  // Load exclusion areas:
  m_lstExclusionPolys.clear();
  m_lstExclusionAngles.clear();

  unsigned int N = 1;

  for (;;)
  {
    vector<double> x, y, z_range;
    configSource.read_vector(iniSection, format("exclusionZone%u_x", N), vector<double>(0), x);
    configSource.read_vector(iniSection, format("exclusionZone%u_y", N), vector<double>(0), y);
    configSource.read_vector(
        iniSection, format("exclusionZone%u_z", N++), vector<double>(0), z_range);

    if (!x.empty() && !y.empty())
    {
      ASSERT_(x.size() == y.size());
      CObservation2DRangeScan::TListExclusionAreasWithRanges::value_type dat;

      dat.first.setAllVertices(x, y);
      if (z_range.empty())
      {
        dat.second.first = -std::numeric_limits<double>::max();
        dat.second.second = std::numeric_limits<double>::max();
      }
      else
      {
        ASSERTMSG_(z_range.size() == 2, "exclusionZone%u_z must be a range [z_min z_max]");
        ASSERT_(z_range[0] <= z_range[1]);

        dat.second.first = z_range[0];
        dat.second.second = z_range[1];
      }

      m_lstExclusionPolys.push_back(dat);
    }
    else
      break;
  }

  // Load forbiden angles;
  N = 1;

  for (;;)
  {
    const double ini =
        DEG2RAD(configSource.read_double(iniSection, format("exclusionAngles%u_ini", N), -1000));
    const double end =
        DEG2RAD(configSource.read_double(iniSection, format("exclusionAngles%u_end", N++), -1000));

    if (ini > -M_PI && end > -M_PI)
      m_lstExclusionAngles.emplace_back(ini, end);
    else
      break;
  }

  // Max. missed scan failures:
  m_max_missed_scan_failures =
      configSource.read_int(iniSection, "maxMissedScansToDeclareError", m_max_missed_scan_failures);
}

/*-------------------------------------------------------------
            filterByExclusionAreas
-------------------------------------------------------------*/
void C2DRangeFinderAbstract::filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan& obs) const
{
  obs.filterByExclusionAreas(m_lstExclusionPolys);
}

/*-------------------------------------------------------------
            filterByExclusionAngles
-------------------------------------------------------------*/
void C2DRangeFinderAbstract::filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan& obs) const
{
  obs.filterByExclusionAngles(m_lstExclusionAngles);
}

void C2DRangeFinderAbstract::processPreview(const mrpt::obs::CObservation2DRangeScan& obs)
{
  using namespace mrpt::opengl;

  // show laser scan
  if (m_showPreview)
  {
    if (!m_win)
    {
      string caption = string("Preview of ") + m_sensorLabel;
      m_win = mrpt::gui::CDisplayWindow3D::Create(caption, 640, 480);
      m_win->setCameraAzimuthDeg(180);
      m_win->setCameraElevationDeg(90);
      Scene::Ptr& theScene = m_win->get3DSceneAndLock();
      theScene->insert(std::make_shared<CAxis>(-300, -300, -50, 300, 300, 50, 1.0, 3, true));
      m_win->unlockAccess3DScene();
    }

    if (m_win && m_win->isOpen())
    {
      Scene::Ptr& theScene = m_win->get3DSceneAndLock();
      opengl::CPlanarLaserScan::Ptr laser;
      CRenderizable::Ptr obj = theScene->getByName("laser");
      if (!obj)
      {
        laser = std::make_shared<opengl::CPlanarLaserScan>();
        laser->setName("laser");
        laser->setScan(obs);
        theScene->insert(laser);
      }
      else
      {
        laser = std::dynamic_pointer_cast<CPlanarLaserScan>(obj);
        laser->setScan(obs);
      }
      m_win->unlockAccess3DScene();
      m_win->forceRepaint();
    }  // end if
  }    // end if
}
