/* +------------------------------------------------------------------------+
   |                     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 "apps-precomp.h"  // Precompiled headers
//
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/obs/CObservationImage.h>
#include <mrpt/obs/CObservationStereoImages.h>

#include "rawlog-edit-declarations.h"

using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::system;
using namespace mrpt::apps;
using namespace std;
using namespace mrpt::io;

// ======================================================================
//		op_undistort
// ======================================================================
DECLARE_OP_FUNCTION(op_undistort)
{
  // A class to do this operation:
  class CRawlogProcessor_Undistort : public CRawlogProcessorFilterObservations
  {
   protected:
    string m_out_file;

   public:
    CRawlogProcessor_Undistort(
        CFileGZInputStream& in_rawlog,
        TCLAP::CmdLine& cmdline,
        bool Verbose,
        mrpt::io::CFileGZOutputStream& out_rawlog) :
        CRawlogProcessorFilterObservations(in_rawlog, cmdline, Verbose, out_rawlog)
    {
    }

    bool tellIfThisObsPasses(mrpt::obs::CObservation::Ptr& obs) override { return true; }

    bool processOneObservation(CObservation::Ptr& obs) override
    {
      obs->load();

      mrpt::img::CImage tmp;

      if (auto obsSt = std::dynamic_pointer_cast<CObservationStereoImages>(obs); obsSt)
      {
        // save image to file & convert into external storage:
        obsSt->imageLeft.undistort(tmp, obsSt->leftCamera);
        obsSt->imageLeft = std::move(tmp);

        obsSt->imageRight.undistort(tmp, obsSt->rightCamera);
        obsSt->imageRight = std::move(tmp);
      }
      else if (auto obsIm = std::dynamic_pointer_cast<CObservationImage>(obs); obsIm)
      {
        obsIm->image.undistort(tmp, obsIm->cameraParams);
        obsIm->image = std::move(tmp);
      }
      else if (auto obs3D = std::dynamic_pointer_cast<CObservation3DRangeScan>(obs); obs3D)
      {
        obs3D->undistort();
      }

      obs->unload();
      return true;
    }
  };

  // Process
  // ---------------------------------
  TOutputRawlogCreator outrawlog;
  CRawlogProcessor_Undistort proc(in_rawlog, cmdline, verbose, outrawlog.out_rawlog_io);
  proc.doProcessRawlog();

  // Dump statistics:
  // ---------------------------------
  VERBOSE_COUT << "Time to process file (sec)        : " << proc.m_timToParse << "\n";
}
